Feat: Changed units from degrees from radians / added offsets for enc…

…oder
SaintsRobotics · Feb 24, 2024 · 2ba8338 · 2ba8338
1 parent ae7dc15
commit 2ba8338
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -100,10 +100,12 @@ public static final class IntakeConstants {
     public static final int kArmMotorID = 39;
     public static final int kArmEncoderChannel = 0;
 
+    // In degrees
     public static final double kIntakeLoweredAngle = 0;
-    public static final double kIntakeRaisedAngle = 120;
+    public static final double kIntakeRaisedAngle = 194;
 
-    public static final double kArmEncoderOffset = 0;
+    /** Encoder offset in rotations */
+    public static final double kArmEncoderOffset = 0.6692;
 
     public static final double kIntakeSpeed = 3.0;
 

diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -37,11 +37,12 @@ public IntakeSubsystem() {
     // m_intakeMotor.setInverted(IntakeConstants.kIntakeMotorInverted);
 
     m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset);
-    m_armEncoder.setDistancePerRotation(2 * Math.PI);
+    m_armEncoder.setDistancePerRotation(360);
 
     m_intakeMotor.setIdleMode(IdleMode.kCoast);
     m_armMotor.setIdleMode(IdleMode.kBrake);
 
+    // m_armPID.disableContinuousInput();
     m_armPID.setTolerance(0.05);
 
     // TODO: See if this is needed
@@ -98,12 +99,13 @@ public void periodic() {
       armRetract();
     }
 
-    m_armMotor.set(m_armPID.calculate(m_armEncoder.getAbsolutePosition()));
-    m_intakeMotor.set(m_intakeSpeed);
+    // m_armMotor.set(m_armPID.calculate(m_armEncoder.getDistance()));
+    // m_intakeMotor.set(m_intakeSpeed);
 
-    SmartDashboard.putNumber("Arm Angle", m_armEncoder.getAbsolutePosition());
+    SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance());
     SmartDashboard.putBoolean("Arm Deployed?", deployed);
     SmartDashboard.putBoolean("Have Note?", haveNote);
+    SmartDashboard.putNumber("pid output", m_armPID.calculate(m_armEncoder.getDistance()));
   }
 
   public boolean readyToShoot() {