set correct motor and encoder configs

DeepBlueRobotics · Mar 2, 2024 · ed4363b · ed4363b
1 parent ae60a0c
commit ed4363b
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 37 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -26,14 +26,18 @@ public static final class Drivetrain {
 
 	public static final class Arm {
 		//#region subsytem constants
-
+		
 		// ports
 		public static final int MASTER_ARM_MOTOR = 7;
 		public static final int FOLLOW_ARM_MOTOR = 8;
 
 		// config
+		public static final int MASTER = 0;
+		public static final int FOLLOWER = 1;
 		public static final boolean[] motorInverted = {true, true}; //Todo: find all these (they are definetely wrong)
 		public static final boolean encoderInverted = false;
+		// TODO: Figure out offset
+		public static final double ENCODER_OFFSET = 0;
 
 		// goal positions
 		public static final double INTAKE_ANGLE = Units.degreesToRadians(0);

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -8,6 +8,7 @@
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkAbsoluteEncoder;
 import com.revrobotics.SparkPIDController;
 
 import edu.wpi.first.math.MathUtil;
@@ -25,24 +26,13 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-// Arm angle is measured from horizontal on the intake side of the robot and bounded between -3π/2 and π/2
-// Wrist angle is measured relative to the arm with 0 being parallel to the arm and bounded between -π and π (Center of Mass of Roller)
+// TODO: FIGURE OUT ANGLES
+// Arm angle is measured from horizontal on the intake side of the robot and bounded between __ and __
 public class Arm extends SubsystemBase {
-  ///GOALS
-  /*
-   * public static final double intakeAngle = Math.toRadians(0);
-		public static final double ampAngle = Math.toRadians(103);
-		public static final double placeholderSpeakerAngle1 = Math.toRadians(24);
-		public static final double placeholderSpeakerAngle2 = Math.toRadians(24);
-		public static final double placeholderSpeakerAngle3 = Math.toRadians(24);
-		public static final double climberUpAngle = Math.toRadians(24);
-		public static final double climberDownAngle = Math.toRadians(24);
-   */
-    // a boolean meant to tell if the arm is in a forbidden posistion AKA FORBIDDEN FLAG
 
     private final CANSparkMax masterArmMotor = MotorControllerFactory.createSparkMax(MASTER_ARM_MOTOR, MotorConfig.NEO);
     private final CANSparkMax followArmMotor = MotorControllerFactory.createSparkMax(FOLLOW_ARM_MOTOR, MotorConfig.NEO);
-    private final RelativeEncoder armEncoder = masterArmMotor.getEncoder();
+    private final SparkAbsoluteEncoder armEncoder = masterArmMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
 
     private static double kDt = 0.02;
 
@@ -52,49 +42,41 @@ public class Arm extends SubsystemBase {
 
     private TrapezoidProfile armProfile = new TrapezoidProfile(armConstraints);
 
-    private static TrapezoidProfile.State goalState = new TrapezoidProfile.State(0,0);//TODO: update pos later
-
+    //TODO: put in correct initial position
     // rad, rad/s
-    //public static TrapezoidProfile.State[] goalState = { new TrapezoidProfile.State(-Math.PI / 2, 0), new TrapezoidProfile.State(0, 0) };
+    private static TrapezoidProfile.State goalState = new TrapezoidProfile.State(0,0);
+
 
     public Arm() {
-      // weird math stuff
-        masterArmMotor.setInverted(motorInverted);
+        masterArmMotor.setInverted(motorInverted[MASTER]);
         masterArmMotor.setIdleMode(IdleMode.kBrake);
-        followArmMotor.setInverted(motorInverted);
+        followArmMotor.setInverted(motorInverted[FOLLOWER]);
         followArmMotor.setIdleMode(IdleMode.kBrake);
         armEncoder.setPositionConversionFactor(rotationToRad);
         armEncoder.setVelocityConversionFactor(rotationToRad);
         armEncoder.setInverted(encoderInverted);
         followArmMotor.follow(masterArmMotor);
 
-        //armEncoder1.setZeroOffset(offsetRad);
-
-        //armPID.setTolerance(posToleranceRad, velToleranceRadPSec);
+        armEncoder.setZeroOffset(ENCODER_OFFSET);
+        armPID.setFeedbackDevice(armEncoder);
+        armPID.setPositionPIDWrappingEnabled(true);
+        armPID.setPositionPIDWrappingMinInput(ARM_LOWER_LIMIT_RAD);
+        armPID.setPositionPIDWrappingMaxInput(ARM_UPPER_LIMIT_RAD);
 
+        armPID.setP(kP);
+        armPID.setI(kI);
+        armPID.setD(kD);
+
         SmartDashboard.putData("Arm", this);
 
-        //armProfileTimer.start(); <-- don't neeed timer anymore
-
         setArmTarget(goalState.position);
-
-        // SmartDashboard.putNumber("Arm Max Vel", MAX_FF_VEL );
-        SmartDashboard.putNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
-        SmartDashboard.putNumber("Arm Tolerance Pos", posToleranceRad);
-        SmartDashboard.putNumber("Arm Tolerance Vel", velToleranceRadPSec);
     }
 
     @Override
     public void periodic() {
 
         if(DriverStation.isDisabled()) resetGoal();
 
-        //ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = SmartDashboard.getNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
-        // armConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL , MAX_FF_ACCEL );
-        armPID.setP(kP);
-        armPID.setI(kI);
-        armPID.setD(kD);
-
         //smart dahsboard stuff
         //SmartDashboard.putBoolean("ArmPIDAtSetpoint", armPID1.atSetpoint());
         SmartDashboard.putBoolean("ArmProfileFinished", armProfile.isFinished(armProfileTimer.get()));