Fixed wrist manual command

src/main/java/frc/robot/RobotContainer.java

@@ -38,7 +38,6 @@
 import frc.robot.commands.RotateVelocityDriveCommand;
 import frc.robot.commands.ScoreCommand;
 import frc.robot.commands.ScoreCommand.ScoreStep;
-import frc.robot.commands.SetZeroModeCommand;
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.commands.WristManualCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
@@ -188,8 +187,8 @@ public void containerMatchStarting() {
   private void configureButtonBindings() {
     // FIXME the left and right triggers are already in use, either change or delete these button
     // bindings
-    jason.leftTrigger().onTrue(new WristManualCommand(elevatorSubsystem, 0));
-    jason.rightTrigger().onTrue(new WristManualCommand(elevatorSubsystem, 20));
+    jason.leftBumper().onTrue(new WristManualCommand(elevatorSubsystem, 0));
+    jason.rightBumper().onTrue(new WristManualCommand(elevatorSubsystem, 20));
     jason
         .y()
         .onTrue(new ElevatorPositionCommand(elevatorSubsystem, Constants.Elevator.MAX_HEIGHT, 0));
@@ -300,11 +299,7 @@ private void configureButtonBindings() {
     jasonLayer
         .off(jason.rightTrigger())
         .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.OUTTAKE));
-    jasonLayer
-        .off(jason.x())
-        .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.OFF))
-        // FIXME delete this line, you can't be in both HOLD and OFF at the same time
-        .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.HOLD));
+    jasonLayer.off(jason.x()).onTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.OFF));
 
     // intake presets
     // jasonLayer
@@ -325,7 +320,7 @@ private void configureButtonBindings() {
     //     .whileTrue(
     //         new ForceOuttakeSubsystemModeCommand(
     //             intakeSubsystem, IntakeSubsystem.IntakeMode.INTAKE));
-            // FIXME delete or replace outtake stuff with new intake stuff
+    // FIXME delete or replace outtake stuff with new intake stuff
 
     // ground pickup
     jasonLayer

src/main/java/frc/robot/commands/WristManualCommand.java

@@ -4,50 +4,33 @@
 
 package frc.robot.commands;
 
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ElevatorSubsystem;
 
 public class WristManualCommand extends CommandBase {
-  private double desiredAngle;
-  private double kp;
-  private double ki;
-  private double kd;
-  private PIDController pidController;
-  private TalonFX wristMotor;
-  private CANCoder canCoder;
-
-  public WristManualCommand(ElevatorSubsystem subsystem, double desiredAngle) {
-    // FIXME woahh there we cant be putting pid controllers in commands now
-    // all this logic belongs in the subsystem!!
-    this.desiredAngle = desiredAngle;
-    pidController = new PIDController(0, 0, 0);
-    canCoder = new CANCoder(0);
+  private double targetAngle;
+  private ElevatorSubsystem subsystem;
+
+  public WristManualCommand(ElevatorSubsystem subsystem, double targetAngle) {
+    this.targetAngle = targetAngle;
+    this.subsystem = subsystem;
     addRequirements(subsystem);
   }
 
-  public void setDesiredAngle(double desiredAngle) {
-    this.desiredAngle = desiredAngle;
+  public void settargetAngle(double targetAngle) {
+    this.targetAngle = targetAngle;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    // FIXME you should set the desired angle in here
+    subsystem.setTargetAngle(targetAngle);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    // FIXME this logic is already in the subsystem you don't need it again here!
-    wristMotor.set(
-        TalonFXControlMode.PercentOutput,
-        MathUtil.clamp(
-            pidController.calculate(canCoder.getAbsolutePosition(), desiredAngle), -0.25, 0.25));
+    subsystem.setTargetAngle(targetAngle);
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

@@ -26,9 +26,8 @@ public class ElevatorSubsystem extends SubsystemBase {
   private TalonFX wristMotor;
   private double currentHeight;
   private double targetHeight;
-  private double currentAngle;
   private double targetAngle;
-
+  private double currentAngle;
   private PIDController heightController;
   private PIDController wristController;
   private CANCoder canCoder;
@@ -57,8 +56,9 @@ public ElevatorSubsystem() {
 
     currentHeight = 0.0;
     targetHeight = 0.0;
-    currentAngle = 0.0;
     targetAngle = 0.0;
+    // FIXME Add way to get current angle value
+    currentAngle = 0;
 
     right_motor.configFactoryDefault();
     left_motor.configFactoryDefault();
@@ -91,7 +91,7 @@ public static double ticksToHeight(double ticks) {
 
   // FIXME getCurrentTicks is for the elevator, not the wrist
   private double getCurrentTicks() {
-    return rightMotor.getSelectedSensorPosition();
+    return right_motor.getSelectedSensorPosition();
   }
 
   // FIXME you can just use the cancoder for this, that's what it's for
@@ -103,7 +103,6 @@ public void setTargetHeight(double targetHeight) {
     this.targetHeight = targetHeight;
   }
 
-  // FIXME rename with consistent naming
   public void setTargetAngle(double targetAngle) {
     this.targetAngle = targetAngle;
   }
@@ -128,7 +127,7 @@ public void periodic() {
     wristMotor.set(
         TalonFXControlMode.PercentOutput,
         MathUtil.clamp(
-            wristController.calculate(currentAngle, targetAngle), -0.25, 0.25));
+            wristController.calculate(canCoder.getAbsolutePosition(), targetAngle), -0.25, 0.25));
     // FIXME replace all uses of "canCOder.getAbsolutePosition()" with currentAngle (variable)
   }
 }