diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d70b3db..df48452 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -101,6 +101,7 @@ public static final class IOConstants { } public static final class TrapConstants { + // FIXME: should say ID instead of PORT because it's a CAN ID public static final int VICTOR_MOTOR_PORT = 0; } } diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 7357aa6..ffbfd98 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -6,6 +6,7 @@ public class ArmCommand extends Command { + // FIXME: both member variables should be private and final (if possible) private ArmSubsystem armSubsystem; private double armSetpoint; diff --git a/src/main/java/frc/robot/commands/ClimbCommand.java b/src/main/java/frc/robot/commands/ClimbCommand.java index 5e962d1..1a68007 100644 --- a/src/main/java/frc/robot/commands/ClimbCommand.java +++ b/src/main/java/frc/robot/commands/ClimbCommand.java @@ -5,6 +5,7 @@ public class ClimbCommand extends Command { + // FIXME: both member variables should be private and final (if possible) private ClimbSubsystem climbSubsystem; private double climbSpeed; @@ -26,6 +27,7 @@ public void execute() { // FIXME: This is an infinate loop in an infinite loop, which is not good. while (climbSpeed != 0) { + // FIXME: these are assignments, not comparisons. Use == instead of = if (rightBottom = true) { climbSubsystem.setMotorSpeed(0); } else if (leftBottom = true) { diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index ce9376c..0cc5956 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -4,6 +4,8 @@ import frc.robot.subsystems.IntakeSubsystem; public class IntakeCommand extends Command { + + // FIXME: both member variables should be private and final (if possible) private IntakeSubsystem intakeSubsystem; private double intakeSpeed; @@ -18,6 +20,7 @@ public void execute() { intakeSubsystem.setSpeed(intakeSpeed); boolean beam = intakeSubsystem.getBeamBreak(); + // FIXME: this is an assignment, not a comparison. Use == instead of = if (beam = true) { intakeSubsystem.setSpeed(0); } @@ -25,6 +28,7 @@ public void execute() { @Override public void end(boolean interrupted) { + // FIXME: use stop() instead of setSpeed(0) because it's more clear intakeSubsystem.setSpeed(0); } diff --git a/src/main/java/frc/robot/commands/ManualCommand.java b/src/main/java/frc/robot/commands/ManualCommand.java index 1c7f955..6f9a3f2 100644 --- a/src/main/java/frc/robot/commands/ManualCommand.java +++ b/src/main/java/frc/robot/commands/ManualCommand.java @@ -5,6 +5,7 @@ public class ManualCommand extends Command { + // FIXME: both member variables should be private and final (if possible) private ArmSubsystem armSubsystem; private double armSpeed; @@ -22,6 +23,7 @@ public void execute() { @Override public void end(boolean interrupted) { + // FIXME: use stop() instead of setSpeed(0) because it's more clear armSubsystem.setSpeed(0); } } diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index 0899626..41b4f96 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -4,6 +4,7 @@ import frc.robot.subsystems.ShooterSubsystem; public class ShooterCommand extends Command { + // FIXME: both member variables should be private and final (if possible) private ShooterSubsystem shooterSubsystem; private double shooterSpeed; @@ -21,6 +22,7 @@ public void execute() { @Override public void end(boolean interupted) { + // FIXME: should use stop() instead of setBothSpeed(0) because it's more clear shooterSubsystem.setBothSpeed(0); } } diff --git a/src/main/java/frc/robot/commands/TrapCommand.java b/src/main/java/frc/robot/commands/TrapCommand.java index ffe269a..ad71368 100644 --- a/src/main/java/frc/robot/commands/TrapCommand.java +++ b/src/main/java/frc/robot/commands/TrapCommand.java @@ -5,8 +5,10 @@ public class TrapCommand extends Command { + // FIXME: both member variables should be private and final (if possible) TrapSubsystem trapsubsystem; + // FIXME: this name is not descriptive, would recommend using something like "trapSpeed" double trap; public TrapCommand(TrapSubsystem trapSubsystem, double trap) { diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 26439cb..c4c691c 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; +/** I still would recommend using a {@link edu.wpi.first.wpilibj2.command.PIDSubsystem} */ public class ArmSubsystem extends SubsystemBase { private CANSparkMax armMotor = new CANSparkMax(MOTOR1_ID, MotorType.kBrushless); private CANSparkMax motor2 = new CANSparkMax(MOTOR2_ID, MotorType.kBrushless); diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index ed41477..5f92be5 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -8,6 +8,7 @@ public class ClimbSubsystem extends SubsystemBase { + // FIXME: all should be private CANSparkMax leftMotor = new CANSparkMax(Constants.ClimbConstants.LEFT_MOTOR_ID, MotorType.kBrushless); CANSparkMax rightMotor = @@ -19,6 +20,7 @@ public class ClimbSubsystem extends SubsystemBase { DigitalInput leftTop = new DigitalInput(Constants.ClimbConstants.LEFT_TOP_LS); DigitalInput rightTop = new DigitalInput(Constants.ClimbConstants.RIGHT_TOP_LS); + // FIXME: rename to setSpeed() to keep consistent with other subsystems public void setMotorSpeed(double motorSpeed) { leftMotor.set(motorSpeed); rightMotor.set(motorSpeed); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 9a4a2a2..b92973e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -8,6 +8,7 @@ public class IntakeSubsystem extends SubsystemBase { + // FIXME: should be private CANSparkMax motor = new CANSparkMax(Constants.IntakeConstants.MOTOR_ID, MotorType.kBrushless); DigitalInput beam = new DigitalInput(Constants.IntakeConstants.BEAM_BREAK_PORT); @@ -23,5 +24,6 @@ public boolean getBeamBreak() { return beam.get(); } + // FIXME: doesn't need to be defined if its empty public IntakeSubsystem() {} } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index e7e87a7..0492958 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,16 +1,16 @@ package frc.robot.subsystems; +import static frc.robot.Constants.*; + import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; public class ShooterSubsystem extends SubsystemBase { CANSparkMax bottomMotor = - new CANSparkMax(Constants.ShooterConstants.BOTTOM_MOTOR_ID, MotorType.kBrushless); - CANSparkMax topMotor = - new CANSparkMax(Constants.ShooterConstants.TOP_MOTOR_ID, MotorType.kBrushless); + new CANSparkMax(ShooterConstants.BOTTOM_MOTOR_ID, MotorType.kBrushless); + CANSparkMax topMotor = new CANSparkMax(ShooterConstants.TOP_MOTOR_ID, MotorType.kBrushless); public ShooterSubsystem() { topMotor.setInverted(true); @@ -25,6 +25,7 @@ public void setTopSpeed(double topSpeed) { } // FIXME: doesn't set both speeds? + // FIXME: rename to setSpeed() to keep consistent with other subsystems public void setBothSpeed(double motorSpeed) { topMotor.set(motorSpeed); } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index c6bf759..f9ea77b 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -1,7 +1,5 @@ package frc.robot.subsystems; -// import frc.robot.subsystems.swervedrive.data; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathConstraints; @@ -64,13 +62,11 @@ public SwerveSubsystem(File directory) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection( - false); // Heading correction should only be used while controlling the robot via - // angle. - swerveDrive.setCosineCompensator( - !SwerveDriveTelemetry - .isSimulation); // Disables cosine compensation for simulations since it - // causes discrepancies not seen in real life. + // Heading correction should only be used while controlling the robot via angle + swerveDrive.setHeadingCorrection(false); + // Disables cosine compensation for simulations since it causes discrepancies not seen in + // real life + swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); swerveDrive.pushOffsetsToControllers(); setupPathPlanner(); diff --git a/src/main/java/frc/robot/subsystems/TrapSubsystem.java b/src/main/java/frc/robot/subsystems/TrapSubsystem.java index 691e727..61aa84b 100644 --- a/src/main/java/frc/robot/subsystems/TrapSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TrapSubsystem.java @@ -6,12 +6,18 @@ public class TrapSubsystem extends SubsystemBase { + /** + * FIXME: should be private, should be a {@link com.ctre.phoenix.motorcontrol.can.VictorSPX} not + * a {@link edu.wpi.first.wpilibj.motorcontrol.VictorSP} + */ VictorSP victor = new VictorSP(Constants.TrapConstants.VICTOR_MOTOR_PORT); + // FIXME: rename to setSpeed() to keep consistent with other subsystems public void motorOn(double victorSpeed) { victor.set(victorSpeed); } + // FIXME: remane to stop() to keep consistent with other subsystems public void motorOff() { victor.set(0); }