climber subsystem 3.0

src/main/java/frc4388/robot/Robot.java

@@ -257,7 +257,7 @@ public class Robot extends TimedRobot {
     if (selectedOdo == null) {
       selectedOdo = m_robotContainer.getOdometry();
     }
-    m_robotContainer.resetOdometry(selectedOdo);
+    // m_robotContainer.resetOdometry(selectedOdo);
 
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to

src/main/java/frc4388/robot/RobotContainer.java

@@ -113,7 +113,7 @@ public class RobotContainer {
   public final RobotMap m_robotMap = new RobotMap();
 
   /* Subsystems */
-  public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
+  public final Climber m_robotClimber = new Climber(m_robotMap.elbow);
   public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); 
   public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
   public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
@@ -235,7 +235,7 @@ public class RobotContainer {
        }, m_robotHood));
 
     m_robotClimber.setDefaultCommand(
-      new RunCommand(() -> m_robotClimber.setMotors(0.0, getOperatorController().getRightY() * 0.5)
+      new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getRightY() * 0.5)
       , m_robotClimber));
 
       //  m_robotTurret.setDefaultCommand(
@@ -293,8 +293,11 @@ public class RobotContainer {
     new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
       .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
       
+    // new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+    //   .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+
     new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-      .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+      .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry));
 
     new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
       .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
@@ -359,7 +362,6 @@ public class RobotContainer {
       .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
       
       .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
-      .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber))
       
       .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
       .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
@@ -368,7 +370,6 @@ public class RobotContainer {
       .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
 
       .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
-      .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber))
 
       .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
       .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))

src/main/java/frc4388/robot/RobotMap.java

@@ -49,6 +49,7 @@ public class RobotMap {
     configureExtenderMotors();
     configureSerializerMotors();
     configureStorageMotors();
+    configureClimberMotors();
   }
 
   /* LED Subsystem */
@@ -56,29 +57,30 @@ public class RobotMap {
 
 //   void configureLEDMotorControllers() {}
 
-  /* Swerve Subsystem */
-  public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
-  public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
-  public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
-  public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
-  public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
-  public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
-  public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
-  public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
-  public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
-  public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
-  public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
-  public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
 
-  public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
+/* Swerve Subsystem */
+public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
+public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
+public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
+public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
+public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
+public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
+public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
+public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
+public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
+public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
+public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
+public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
 
-  public SwerveModule leftFront;
+public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
-  public SwerveModule leftBack;
-  public SwerveModule rightFront;
-  public SwerveModule rightBack;
 
-  void configureSwerveMotorControllers() {
+public SwerveModule leftFront;
+public SwerveModule leftBack;
+public SwerveModule rightFront;
+public SwerveModule rightBack;
 
+void configureSwerveMotorControllers() {
+    
     leftFrontSteerMotor.configFactoryDefault();
     leftFrontWheelMotor.configFactoryDefault();
     rightFrontSteerMotor.configFactoryDefault();
@@ -87,58 +89,58 @@ public class RobotMap {
     leftBackWheelMotor.configFactoryDefault();
     rightBackSteerMotor.configFactoryDefault();
     rightBackWheelMotor.configFactoryDefault();
-
+    
     leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
+        
-    leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-    rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+        rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
         SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
+    
     leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
+    
     NeutralMode mode = NeutralMode.Coast;
     leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
     leftFrontWheelMotor.setNeutralMode(mode);// Coast
@@ -148,7 +150,7 @@ public class RobotMap {
     leftBackWheelMotor.setNeutralMode(mode);// Coast
     rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
     rightBackWheelMotor.setNeutralMode(mode);// Coast
-
+    
     // current limits
     
     leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER);
@@ -160,7 +162,7 @@ public class RobotMap {
     rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
     leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
     rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL);
-
+    
     leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
     rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
     leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER);
@@ -170,55 +172,56 @@ public class RobotMap {
     rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
     leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
     rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL);
-
+    
     leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder,
-        SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
+    SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
     leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder,
-        SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
+    SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
     rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder,
-        SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
+    SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
     rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder,
-        SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
+    SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
-
+    
     // config cancoder as remote encoder for swerve steer motors
     leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
-        RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
-        RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
-        RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
-        RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+    RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
-        SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+    SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 }
 
-  /* Climb Subsystem */
+/* Climb Subsystem */
-  public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO
+public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
-  public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
 
-  /* Hooks Subsystem */
+private void configureClimberMotors() {
-//   public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless);
+    elbow.configFactoryDefault();
-//   public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless);
+    elbow.setNeutralMode(NeutralMode.Brake);
-  public final Servo leftClaw = new Servo(1);  // TODO: find actual channel
+}
-  public final Servo rightClaw = new Servo(2); // TODO: find actual channel
+/* Hooks Subsystem */
+public final Servo leftClaw = new Servo(1);
+public final Servo rightClaw = new Servo(2);
 
-  // Shooter Config
+// Shooter Config
-  /* Boom Boom Subsystem */
+/* Boom Boom Subsystem */
-  public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
+public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
-  public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
+public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
 
-  // turret subsystem
+// turret subsystem
-  public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
+public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
 
-  // hood subsystem
+// hood subsystem
-  public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
-
-  // Create motor CANSparkMax
-  void configureShooterMotorControllers() {
 
+// Create motor CANSparkMax
+void configureShooterMotorControllers() {
+    
     // LEFT FALCON
     shooterFalconLeft.configFactoryDefault();
     shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
@@ -227,14 +230,14 @@ public class RobotMap {
     shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
-        ShooterConstants.SHOOTER_TIMEOUT_MS);
+    ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
-        ShooterConstants.SHOOTER_TIMEOUT_MS);
+    ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER,
-        ShooterConstants.SHOOTER_TIMEOUT_MS);
+    ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER,
-        ShooterConstants.SHOOTER_TIMEOUT_MS);
+    ShooterConstants.SHOOTER_TIMEOUT_MS);
-
+    
     // RIGHT FALCON
     shooterFalconRight.configFactoryDefault();
     shooterFalconRight.setNeutralMode(NeutralMode.Coast);
@@ -244,59 +247,59 @@ public class RobotMap {
     // m_shooterFalconRight.configPeakOutputForward(0,
     // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
     shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
-        ShooterConstants.SHOOTER_TIMEOUT_MS);
+    ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
-        ShooterConstants.SHOOTER_TIMEOUT_MS);
+    ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER,
-        ShooterConstants.SHOOTER_TIMEOUT_MS);
+    ShooterConstants.SHOOTER_TIMEOUT_MS);
     shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER,
-        ShooterConstants.SHOOTER_TIMEOUT_MS);
+    ShooterConstants.SHOOTER_TIMEOUT_MS);
-
+    
     shooterFalconRight.follow(shooterFalconLeft);
-
+    
     // turret
     shooterTurret.restoreFactoryDefaults();
     shooterTurret.setIdleMode(IdleMode.kBrake);
     shooterTurret.setInverted(true);
-
+    
     // hood subsystem
     angleAdjusterMotor.restoreFactoryDefaults();
     angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
     angleAdjusterMotor.setInverted(true);
-  }
+}
 
-  /* Serializer Subsystem */
+/* Serializer Subsystem */
-  public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
+public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless);
-  public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
+public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM);
-  
-  /* Intake Subsystem */
-  public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
-  public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
 
-  void configureIntakeMotors() {
+/* Intake Subsystem */
+public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR);
+public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless);
+
+void configureIntakeMotors() {
     intakeMotor.configFactoryDefault();
     intakeMotor.setInverted(false);
     intakeMotor.setNeutralMode(NeutralMode.Coast);
-
+    
     intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE);
     intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE);
-  }
+}
 
-  void configureExtenderMotors() {
+void configureExtenderMotors() {
-      extenderMotor.restoreFactoryDefaults();
+    extenderMotor.restoreFactoryDefaults();
-      extenderMotor.setInverted(true);
+    extenderMotor.setInverted(true);
-      extenderMotor.setIdleMode(IdleMode.kBrake);
+    extenderMotor.setIdleMode(IdleMode.kBrake);
   }
-
+  
   void configureSerializerMotors() {
       serializerBelt.restoreFactoryDefaults();
-  }
+    }
 
     /* Storage Subsystem */
     public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless);
-
+    
     void configureStorageMotors() {
         storageMotor.restoreFactoryDefaults();
     }
-
+    
 }

src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java

@@ -106,8 +106,8 @@ public class Shoot extends CommandBase {
      * Updates error for custom PID.
      */
     public void updateError() {
-      targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
+      // targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees());
-      
+      targetAngle = 0;
       error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360;
         // if (error > 180) {
         //   error = 360 - error; // TODO: error - 360

src/main/java/frc4388/robot/subsystems/Climber.java

@@ -4,429 +4,20 @@
 
 package frc4388.robot.subsystems;
 
-import java.util.Arrays;
-import java.util.HashMap;
-
-import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
-import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
 
-import org.opencv.core.Point;
-
-import edu.wpi.first.wpilibj.motorcontrol.Talon;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants;
 import frc4388.robot.Constants.ClimberConstants;
-
 public class Climber extends SubsystemBase {
-  private static double[][] shoulderFeedMap;
+  private WPI_TalonFX elbow;
-  private static double[][] elbowFeedMap;
 
-  public static void configureFeedMaps() {
+  /** Creates a new Climber */
-    
+  public Climber(WPI_TalonFX elbow) { this.elbow = elbow; }
-  }
+  
+  public void setEncoders(double value) { this.elbow.setSelectedSensorPosition(value); }
+  public double getCurrent() { return this.elbow.getSupplyCurrent(); }
+  public void setMotors(double elbowOutput) { this.elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); }
 
-  private WPI_TalonFX m_shoulder;
-  private WPI_TalonFX m_elbow;
-  private WPI_Pigeon2 m_gyro;
-
-  private Point tPoint;
-
-  private double[] tJointAngles;
-  private double[] tJointSpeeds;
-
-  private boolean groundRelative;
-  private double shoulderSpeedLimiter;
-  private double elbowSpeedLimiter;
-
-  /** Creates a new ClimberRewrite. */
-  public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) {
-    m_shoulder = shoulder;
-    m_shoulder.configFactoryDefault();
-    m_shoulder.setNeutralMode(NeutralMode.Brake);
-
-    m_elbow = elbow;
-    m_elbow.configFactoryDefault();
-    m_elbow.setNeutralMode(NeutralMode.Brake);
-
-    // setClimberPositionGains();
-    setClimberVelocityGains();
-    useVelocityGains();
-
-    tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()};
-
-    // shoulderStartPosition = m_shoulder.getSelectedSensorPosition();
-    // elbowStartPosition = m_elbow.getSelectedSensorPosition();
-    // m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
-    // m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO);
-
-    m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT);
-    m_elbow.configForwardSoftLimitEnable(true);
-    // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
-    // m_elbow.configReverseSoftLimitEnable(true);
-
-    m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT);
-    m_shoulder.configForwardSoftLimitEnable(true);
-    m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT);
-    m_shoulder.configReverseSoftLimitEnable(false);
-
-    m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-    m_elbow.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-    m_shoulder.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-    
-    m_shoulder.overrideLimitSwitchesEnable(true);
-    m_elbow.overrideLimitSwitchesEnable(true);
-
-    tPoint = new Point();
-
-    if(groundRelative)
-      m_gyro = gyro;
-    
-    groundRelative = _groundRelative;
-    this.elbowSpeedLimiter = 1.0;
-  }
-
-  // public void setClimberPositionGains() {
-  //   m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
-  //   m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  //   m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  //   m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  //   m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
-
-  //   m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
-  //   m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  //   m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  //   m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  //   m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  // }
-
-  // public void usePositionGains() {
-  //   m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
-  //   m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
-  // }
-
-  public void setClimberVelocityGains() {
-    m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
-    m_shoulder.config_kF(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
-    m_shoulder.config_kP(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
-    m_shoulder.config_kI(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
-    m_shoulder.config_kD(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
-
-    m_elbow.selectProfileSlot(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
-    m_elbow.config_kF(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
-    m_elbow.config_kP(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
-    m_elbow.config_kI(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
-    m_elbow.config_kD(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  }
-
-  public void useVelocityGains() {
-    m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
-    m_elbow.selectProfileSlot(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
-  }
-
-  // public void setClimberFeedForward(double shoulderF, double elbowF) {
-  //   m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
-  //   m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, shoulderF, ClimberConstants.CLIMBER_TIMEOUT_MS);
-
-  //   m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
-  //   m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, elbowF, ClimberConstants.CLIMBER_TIMEOUT_MS);
-  // }
-
-  // public void compensateFeedForArmAngles() {
-  //   double shoulderPosition = m_shoulder.getSelectedSensorPosition();
-  //   double elbowPosition = m_elbow.getSelectedSensorPosition();
-
-  //   double shoulderFeed = 0;
-  //   double elbowFeed = 0;
-
-  //   for(int i = 1; i < shoulderFeedMap.length; i++) {
-  //     if(shoulderFeedMap[i][0] > shoulderPosition) {
-  //       double percentDifference = (shoulderPosition - shoulderFeedMap[i-1][0]) / (shoulderFeedMap[i][0] - shoulderFeedMap[i-1][0]);
-  //       double feedDifference = shoulderFeedMap[i][1] - shoulderFeedMap[i-1][1];
-  //       shoulderFeed = (percentDifference * feedDifference) + shoulderFeedMap[i-1][1];
-  //     }
-  //   }
-
-  //   for(int i = 1; i < elbowFeedMap.length; i++) {
-  //     if(elbowFeedMap[i][0] > elbowPosition) {
-  //       double percentDifference = (elbowPosition - elbowFeedMap[i-1][0]) / (elbowFeedMap[i][0] - elbowFeedMap[i-1][0]);
-  //       double feedDifference = elbowFeedMap[i][1] - elbowFeedMap[i-1][1];
-  //       elbowFeed = (percentDifference * feedDifference) + elbowFeedMap[i-1][1];
-  //     }
-  //   }
-
-  //   setClimberFeedForward(shoulderFeed, elbowFeed);
-  // }
-
-  public void setMotors(double shoulderOutput, double elbowOutput) {
-    m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER );//* this.shoulderSpeedLimiter);
-    m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER);// * this.elbowSpeedLimiter);
-  }
-
-  public double[] getJointAngles() {
-    return new double[] {
-      (m_shoulder.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.SHOULDER_GB_RATIO,
-      (m_elbow.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.ELBOW_GB_RATIO
-    };
-  }
-
-  public void setJointAngles(double[] angles) {
-    // System.out.println(Arrays.toString(angles));
-    setJointAngles(angles[0], angles[1]);
-  }
-
-  public void setJointAngles(double shoulderAngle, double elbowAngle) {
-    // shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
-    // elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
-
-    // shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
-    // elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
-
-    // // Convert radians to ticks
-    // System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
-
-    // double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
-    // double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
-
-    // shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO;
-    // elbowPosition *= ClimberConstants.ELBOW_GB_RATIO;
-
-    // // shoulderPosition += m_shoulderOffset;
-    // // elbowPosition += m_elbowOffset;
-
-    // m_shoulder.set(TalonFXControlMode.Position, shoulderAngle);
-    // m_elbow.set(TalonFXControlMode.Position, elbowAngle);
-  }
-
-  public void setJointSpeeds(double[] speeds) {
-    setJointSpeeds(speeds[0], speeds[1]);
-  }
-  /**
-   * Velocity PID set for joints
-   * @param shoulderSpeed
-   * @param elbowSpeed
-   */
-  public void setJointSpeeds(double shoulderSpeed, double elbowSpeed) {
-    m_shoulder.set(TalonFXControlMode.Velocity, shoulderSpeed);
-    m_elbow.set(TalonFXControlMode.Velocity, elbowSpeed);
-  }
-
-  boolean movingPrev = false;
-  boolean moving;
-  /**
-   * 
-   * @param xInput Rate of change of X position of target point
-   * @param yInput Rate of change of Y position of target point
-   * @deprecated use controlJointsWithInput() instead
-   */
-  @Deprecated
-  public void controlWithInput(double xInput, double yInput) {
-    moving = xInput != 0 || yInput != 0;
-
-    if(movingPrev != moving) {
-      if(moving) {
-        useVelocityGains();
-        SmartDashboard.putString("Climber Gains", "Velocity");
-      } else {
-        // usePositionGains();
-        tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()};
-        SmartDashboard.putString("Climber Gains", "Position");
-      }
-    }
-
-    if(moving) {
-      double[] jointSpeeds = new double[] {xInput * ClimberConstants.MOVE_SPEED, yInput * ClimberConstants.MOVE_SPEED};
-      setJointSpeeds(jointSpeeds);
-    } else {
-      setJointAngles(tJointAngles);
-    }
-
-    movingPrev = moving;
-  }
-
-  public void controlJointsWithInput(double shoulderInput, double elbowInput) {
-    tJointAngles[0] += shoulderInput * ClimberConstants.MOVE_SPEED * .02;
-    tJointAngles[1] += elbowInput * ClimberConstants.MOVE_SPEED * .02;
-  }
-
-  int pCount = 0;
   @Override
-  public void periodic() {
+  public void periodic() {}
-    SmartDashboard.putBoolean("Moving", moving);
+}
-    SmartDashboard.putBoolean("MovingPrev", movingPrev);
-    SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
-    SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
-    // double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
-    // if(pCount % 1 == 0)
-    //   setJointAngles(tJointAngles);
-
-    // pCount ++;
-
-    // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
-    double currentElbowPos = this.m_elbow.getSelectedSensorPosition();
-    double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT);
-    double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT);
-
-    if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) {
-      this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE));
-    }
-
-    if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) {
-      this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE));
-    }
-
-    if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) {
-      this.elbowSpeedLimiter = 1.0;
-    }
-
-    // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
-    double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition();
-    double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT);
-    double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT);
-
-    if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) {
-      this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE));
-    }
-
-    if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) {
-      this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE));
-    }
-
-    if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) {
-      this.shoulderSpeedLimiter = 1.0;
-    }
-  }
-
-  /**
-   * Gets joint angles for climber arm
-   * {@code targetPoint.x} and {@code targetPoint.y} are set in the xy plane of the climber, accounting for the
-   * rotation of the robot. Both parameters should be in cm.
-   * Does not set the motors automatically
-   * <p><p>
-   * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399
-   * 
-   * @param targetPoint Target xy point for the climber arm to go to
-   * @param tiltAngle The tilt of the robot
-   * @return [shoulderAngle, elbowAngle] in radians 
-   * @deprecated
-   * */
-    @Deprecated
-   public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) {
-    double [] angles = new double[2];
-  
-    double mag = Math.hypot(targetPoint.x, targetPoint.y);
-    if(mag >= ClimberConstants.MAX_ARM_LENGTH) {
-      targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH;
-      targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH;
-      mag = ClimberConstants.MAX_ARM_LENGTH;
-    } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) {
-      targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH;
-      targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH;
-      mag = ClimberConstants.MIN_ARM_LENGTH;
-    } else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
-      targetPoint.x = ClimberConstants.MIN_ARM_LENGTH;
-      targetPoint.y = 0.d;
-      mag = ClimberConstants.MIN_ARM_LENGTH;
-    }
-  
-    // The angle to the target point
-    double theta;
-    if(targetPoint.x == 0.d) {
-      theta = Math.PI/2.d; // TODO rename variable
-    } else {
-      theta = Math.atan(targetPoint.y / targetPoint.x);
-    }
-    theta += tiltAngle;
-    
-    if(targetPoint.x < 0.d)
-        theta += Math.PI;
-    
-    
-    // Correct target position for tilt
-    targetPoint.x = Math.cos(theta) * mag;
-    targetPoint.y = Math.sin(theta) * mag;
-    
-    // Law and Order: Cosines edition
-    double shoulderAngle;
-    if(mag == 0) {
-      shoulderAngle = 0;
-    } else {
-      shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) /
-      (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
-    }
-    shoulderAngle = theta - shoulderAngle;
-    
-    double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) /
-      (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
-    //elbowAngle = Math.PI - elbowAngle;
-    // System.out.println("sa1: " + shoulderAngle);
-    // System.out.println("ea1: " + elbowAngle);
-  
-    // If the climber is resting on the robot base, rotate the upper arm in the direction of the target
-    if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
-      shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
-      double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH;
-      // System.out.println("xDiff: " + xDiff);
-  
-      elbowAngle = Math.atan(-targetPoint.y / xDiff);
-      // System.out.println("ea2: " + elbowAngle);
-      if(elbowAngle < 0) {
-        elbowAngle = Math.PI - Math.abs(elbowAngle);
-      }
-
-      if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
-        elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
-      // System.out.println("ea3: " + elbowAngle);
-  
-      // Deal with negative wraparound
-      // if(xDiff >= 0.d)
-      //   elbowAngle += Math.PI;
-      // System.out.println("ea4: " + elbowAngle);
-    }
-  
-    angles[0] = shoulderAngle;
-    angles[1] = elbowAngle;
-    return angles;
-  }
-  /**
-   * Forward kinematics for climber
-   * @param shoulderAngle in radians
-   * @param elbowAngle in radians
-   * @return Point in 2d of end effector
-   */
-  public static Point getClimberPosition(double shoulderAngle, double elbowAngle) {
-    Point climberPoint = new Point(0, 0);
-
-    climberPoint.x += Math.sin(shoulderAngle);
-    climberPoint.y += Math.cos(shoulderAngle);
-
-    climberPoint.x -= Math.sin(elbowAngle - shoulderAngle);
-    climberPoint.y += Math.cos(elbowAngle - shoulderAngle);
-
-    return climberPoint;
-  }
-
-  public static Point getClimberPosition(double[] jointAngles) {
-    return getClimberPosition(jointAngles[0], jointAngles[1]);
-  }
-
-  public void setClimberSoftLimits(boolean set){
-    m_elbow.configForwardSoftLimitEnable(set);
-    m_shoulder.configForwardSoftLimitEnable(set);
-  }
-
-  public void setEncoders(double value){
-    m_elbow.setSelectedSensorPosition(value);
-    m_shoulder.setSelectedSensorPosition(value);
-  }
-
-  public double getCurrent(){
-    return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent());
-  }
-
-}