Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Bunch of cleanup #26

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Bunch of cleanup #26

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
129 changes: 63 additions & 66 deletions src/main/java/frc/robot/DriveController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,21 +1,24 @@
package frc.robot;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.subsystems.climber.LoosenClimber;
import frc.robot.subsystems.climber.TightenClimber;
import frc.robot.subsystems.swerve.SwerveTurnTo;

import java.util.Arrays;

/**
* <p> This is the specific controller that controls Swerve due to the fact that swerve requires 2 separate joysticks and buttons to rezero the robot's gyro/position.
* <p> This also contains controls for the climber since movement and the climber go hand in hand.
* <p> This also contains Vision buttons (that currently do nothing) and a WestCoast control (that does not work, but was useful for testing with a different robot)
*/
public class DriveController extends XboxController {
public class DriveController extends CommandXboxController {

private RobotMap map;
private RobocketsShuffleboard shuffleboard;
private final RobotMap map;
private final RobocketsShuffleboard shuffleboard;

/**
* Initializes the Shooter Controller and makes an internal copy of the RobotMap to save performance.
Expand All @@ -27,6 +30,28 @@ public DriveController(int port, RobotMap map, RobocketsShuffleboard shuffleboar
super(port);
this.map = map;
this.shuffleboard = shuffleboard;

// TODO: this does not really do anything
a().toggleOnTrue(Commands.runOnce(() -> System.out.println(map.vision)));

x().toggleOnTrue(Commands.runOnce(map.swerve::zeroGyro));
y().toggleOnTrue(Commands.runOnce(map.swerve::resetPose));

rightBumper().toggleOnTrue(
Commands.startEnd(
this::loosenClimber,
this::tightenClimber,
map.climber
)
);
}

private void loosenClimber() {
CommandScheduler.getInstance().schedule(new LoosenClimber());
}

private void tightenClimber() {
CommandScheduler.getInstance().schedule(new TightenClimber());
}

/**
Expand All @@ -50,9 +75,9 @@ public static double deadzone (double value, double deadzone) {
private final int SMOOTH_FRAME_LENGTH = 5;

private int smoothNextFrameToWrite = 0;
private double[] smoothLeftX = new double[SMOOTH_FRAME_LENGTH];
private double[] smoothLeftY = new double[SMOOTH_FRAME_LENGTH];
private double[] smoothRightX = new double[SMOOTH_FRAME_LENGTH];
private final double[] smoothLeftX = new double[SMOOTH_FRAME_LENGTH];
private final double[] smoothLeftY = new double[SMOOTH_FRAME_LENGTH];
private final double[] smoothRightX = new double[SMOOTH_FRAME_LENGTH];

/**
* <p> Applies input smoothing.
Expand All @@ -61,17 +86,12 @@ public static double deadzone (double value, double deadzone) {
* @return The average (mean) value of the input list of doubles. This will be the average value of a joystick axis.
*/
private double smooth(double[] history) {
double average = 0;
for(int i = 0; i < history.length; i++) {
average += history[i];
}
average /= (double)history.length;
return average;
return Arrays.stream(history).average().orElse(0.0);
}

/**
* <p> This should run during the Robot.java's teleopPeriodic method.
* <p> This applies input smoothing to the joystick axises to make them smoother.
* <p> This applies input smoothing to the joystick axex to make them smoother.
* <p> This also checks for all button pushes and runs their respected Swerve commands/functions.
*/
public void teleopPeriodic() {
Expand All @@ -87,65 +107,42 @@ public void teleopPeriodic() {
double RightX = smooth(smoothRightX);

// Swerve
if (map.swerve != null) {
double xyCof = 1;//0.75/Math.max(0.001, Math.sqrt(Math.pow(deadzone(controller.getLeftX(), 0.1), 2)+Math.pow(deadzone(controller.getLeftY(), 0.1), 2)));

// ROBOT RELATIVE
// map.swerve.swerveDriveR(new ChassisSpeeds(
// SmartDashboard.getNumber("Swerve Speed", 0.7) * -xyCof * deadzone(LeftY, 0.1), // Foward/backwards
// SmartDashboard.getNumber("Swerve Speed", 0.7) * -xyCof * deadzone(LeftX, 0.1), // Left/Right
// SmartDashboard.getNumber("Swerve Speed", 0.7) * deadzone(RightX, 0.08) // Rotation
// ));

// FIELD RELATIVE
if (RightX==0) {
map.swerve.setDriveFXY(
// On the controller, upwards is negative and left is also negative. To correct this, the negative version of both are sent.
shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftY, 0.1), // Foward/backwards
shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftX, 0.1), // Left/Right
true); //square inputs to ease small adjustments
map.swerve.setDriveRot(0, false); // Should not be rotating if not rotating lol
} else {
map.swerve.swerveDriveF(
// On the controller, upwards is negative and left is also negative. To correct this, the negative version of both are sent.
shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftY, 0.1), // Foward/backwards
shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftX, 0.1), // Left/Right
shuffleboard.getSettingNum("Rotation Speed") * deadzone(RightX, 0.08), // Rotation
true); //square inputs to ease small adjustments
}

if (getXButtonPressed()) {
map.swerve.zeroGyro();
}
if (getYButtonPressed()) {
map.swerve.resetPose();
}

// turn to align with gyro
if(getPOV()!=-1) {
CommandScheduler.getInstance().schedule(new SwerveTurnTo(map.swerve, new Rotation2d(-getPOV()*0.01745329)));
}
}

if (map.climber != null) {
if (getRightBumperPressed()) {
CommandScheduler.getInstance().schedule(new LoosenClimber());
}
if (getRightBumperReleased()) {
CommandScheduler.getInstance().schedule(new TightenClimber());
}
double xyCof = 1;//0.75/Math.max(0.001, Math.sqrt(Math.pow(deadzone(controller.getLeftX(), 0.1), 2)+Math.pow(deadzone(controller.getLeftY(), 0.1), 2)));

// ROBOT RELATIVE
// map.swerve.swerveDriveR(new ChassisSpeeds(
// SmartDashboard.getNumber("Swerve Speed", 0.7) * -xyCof * deadzone(LeftY, 0.1), // Forward/backwards
// SmartDashboard.getNumber("Swerve Speed", 0.7) * -xyCof * deadzone(LeftX, 0.1), // Left/Right
// SmartDashboard.getNumber("Swerve Speed", 0.7) * deadzone(RightX, 0.08) // Rotation
// ));

// FIELD RELATIVE
if (RightX==0) {
map.swerve.setDriveFXY(
// On the controller, upwards is negative and left is also negative. To correct this, the negative version of both are sent.
shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftY, 0.1), // Forward/backwards
shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftX, 0.1), // Left/Right
true); //square inputs to ease small adjustments
map.swerve.setDriveRot(0, false); // Should not be rotating if not rotating lol
} else {
map.swerve.swerveDriveF(
// On the controller, upwards is negative and left is also negative. To correct this, the negative version of both are sent.
shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftY, 0.1), // Forward/backwards
shuffleboard.getSettingNum("Movement Speed") * -xyCof * deadzone(LeftX, 0.1), // Left/Right
shuffleboard.getSettingNum("Rotation Speed") * deadzone(RightX, 0.08), // Rotation
true); //square inputs to ease small adjustments
}

// Vision
if (map.vision != null) {
if(getAButtonPressed()){
map.vision.toString();
}
// turn to align with gyro
if(getHID().getPOV() !=-1) {
CommandScheduler.getInstance().schedule(new SwerveTurnTo(map.swerve, new Rotation2d(-getHID().getPOV() *0.01745329)));
}

// West Coast
if (map.westcoast != null) {
map.westcoast.arcadeDrive(getLeftY(), getRightX());
}
}


}
41 changes: 18 additions & 23 deletions src/main/java/frc/robot/RobotMap.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,17 @@

import edu.wpi.first.math.geometry.Translation2d;
import frc.robot.subsystems.climber.ClimberSubsystem;
import frc.robot.subsystems.climber.ClimberSubsystemInterface;
import frc.robot.subsystems.intake.IntakeSubsystem;
import frc.robot.subsystems.intake.IntakeSubsystemInterface;
import frc.robot.subsystems.leds.LedSubsystem;
import frc.robot.subsystems.leds.LedSubsystemInterface;
import frc.robot.subsystems.shooter.ShooterSubsystem;
import frc.robot.subsystems.shooter.ShooterSubsystemInterface;
import frc.robot.subsystems.swerve.SwerveDriveSubsystem;
import frc.robot.subsystems.swerve.SwerveModuleNeo;
import frc.robot.subsystems.swerve.SwerveDriveSubsystemInterface;
import frc.robot.subsystems.vision.VisionSubsystem;
import frc.robot.subsystems.vision.VisionSubsystemInterface;
import frc.robot.subsystems.westcoast.WestCoastSubsystem;

/**
Expand All @@ -18,24 +23,14 @@
*/
public class RobotMap
{

// Swerve
// Gian: why are these neos here if we end up just making new ones in the swervedrivesubsystem?
public SwerveModuleNeo swerve_frontLeftModule;
public SwerveModuleNeo swerve_frontRightModule;
public SwerveModuleNeo swerve_backLeftModule;
public SwerveModuleNeo swerve_backRightModule;

public SwerveDriveSubsystem swerve = null;
public VisionSubsystem vision = null;
public IntakeSubsystem intake = null;
public ShooterSubsystem shooter = null;
public final SwerveDriveSubsystemInterface swerve;
public final VisionSubsystemInterface vision;
public final IntakeSubsystemInterface intake;
public final ShooterSubsystemInterface shooter;
public WestCoastSubsystem westcoast = null;
public LedSubsystem leds = null;
public ClimberSubsystem climber = null;
public final LedSubsystemInterface leds;
public final ClimberSubsystemInterface climber;

// Gian: Ok neat system, this is not something I did on the team
// But why is the swerve drive commented out?
/**
* <p> Contains all the subsystems for the robot to avoid static initialization order problems.
* <p> Various subsystems will be constantly commented out for testing as not all subsystems exist on the robot at one time.
Expand All @@ -44,12 +39,12 @@ public class RobotMap
*/
public RobotMap()
{
// intake = new IntakeSubsystem();
// swerve = new SwerveDriveSubsystem(new Translation2d(0.31115, 0.31115), new Translation2d(0.31115, -0.31115), new Translation2d(-0.31115, 0.31115), new Translation2d(-0.31115, -0.31115)); // All translations are the swerve module positions relative to the center of the bot
// vision = new VisionSubsystem();
shooter = new ShooterSubsystem();
// leds = new LedSubsystem();
// climber = new ClimberSubsystem();
intake = IntakeSubsystem.create();
swerve = SwerveDriveSubsystem.create(new Translation2d(0.31115, 0.31115), new Translation2d(0.31115, -0.31115), new Translation2d(-0.31115, 0.31115), new Translation2d(-0.31115, -0.31115)); // All translations are the swerve module positions relative to the center of the bot
vision = VisionSubsystem.create();
shooter = ShooterSubsystem.create();
leds = LedSubsystem.create();
climber = ClimberSubsystem.create();

// ONLY FOR TESTING
// westcoast = new WestCoastSubsystem();
Expand Down
Loading