fix climb angle methods

StuyPulse · Feb 3, 2025 · ae0840d · ae0840d
1 parent 87903d8
commit ae0840d
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 21 deletions.
diff --git a/src/main/java/com/stuypulse/robot/commands/climb/ClimbDriveToAngle.java b/src/main/java/com/stuypulse/robot/commands/climb/ClimbDriveToAngle.java
@@ -26,10 +26,9 @@ public void end(boolean interrupted) {
 
     @Override
     public boolean isFinished() {
-        if (Math.abs(climb.getDegrees() - climb.getTargetAngle().getDegrees()) <= Settings.Climb.CLIMB_ANGLE_TOLERANCE){
+        if (Math.abs(climb.getAngle().getDegrees() - climb.getTargetAngle().getDegrees()) <= Settings.Climb.CLIMB_ANGLE_TOLERANCE){
             return true;
-        }
-        else {
+        } else {
             return false;
         }
     }

diff --git a/src/main/java/com/stuypulse/robot/subsystems/climb/Climb.java b/src/main/java/com/stuypulse/robot/subsystems/climb/Climb.java
@@ -14,9 +14,11 @@ public static Climb getInstance() {
         return instance;
     }
 
+    public abstract void setTargetAngle(Rotation2d targetAngle);
+
     public abstract void setTargetDegrees(double targetDegrees);
 
-    public abstract double getDegrees();
+    public abstract Rotation2d getAngle();
 
     public abstract Rotation2d getTargetAngle();
 

diff --git a/src/main/java/com/stuypulse/robot/subsystems/climb/ClimbImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climb/ClimbImpl.java
@@ -1,9 +1,7 @@
  package com.stuypulse.robot.subsystems.climb;
 
-import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -55,28 +53,28 @@ public ClimbImpl() {
 
         // integrate CANcoder readings into controller
         climbMotorConfig.Feedback.FeedbackRemoteSensorID = climbEncoder.getDeviceID();
-        climbMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
+        climbMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
 
         climbMotor.getConfigurator().apply(climbMotorConfig);
     }
 
-
     public void setTargetAngle(Rotation2d targetAngle) {
         this.targetAngle = targetAngle;
     }
 
+    public void setTargetDegrees(double targetDegrees) {
+        this.targetAngle = new Rotation2d(targetDegrees / 360);
+    }
+
     @Override
     public Rotation2d getTargetAngle() {
         return targetAngle;
     }
 
-    @Override
-    public double getDegrees() {
-        return (climbEncoder.getAbsolutePosition().getValueAsDouble() * 360);
+    public Rotation2d getAngle() {
+        return new Rotation2d((climbEncoder.getAbsolutePosition().getValueAsDouble()));
     }
 
-    // field/method check target an
-
     @Override
     public void stop() {
         climbMotor.stopMotor();
@@ -87,7 +85,7 @@ public void periodic() {
         PositionVoltage controlRequest = new PositionVoltage(0).withSlot(0);
         climbMotor.setControl(controlRequest.withPosition(getTargetAngle().getDegrees()));
 
-        SmartDashboard.putNumber("Climb/Current Angle (deg)", getDegrees());
+        SmartDashboard.putNumber("Climb/Current Angle (deg)", getAngle().getDegrees());
         SmartDashboard.putNumber("Climb/Target Angle (deg)", getTargetAngle().getDegrees());
 
         SmartDashboard.putNumber("Climb/Motor Voltage", climbMotor.getMotorVoltage().getValueAsDouble());
@@ -96,11 +94,4 @@ public void periodic() {
         SmartDashboard.putNumber("Climb/Supply Current", climbMotor.getSupplyCurrent().getValueAsDouble());
 
     }
-
-
-    @Override
-    public void setTargetDegrees(double targetDegrees) {
-        // TODO Auto-generated method stub
-        throw new UnsupportedOperationException("Unimplemented method 'setTargetDegrees'");
-    }
 }