Merge branch 'sofie-arm' of https://github.com/DeepBlueRobotics/Robot…

…Code2024 into sofie-arm
DeepBlueRobotics · Mar 3, 2024 · a0e46b3 · a0e46b3
2 parents 32bf0ca + 5a215e8
commit a0e46b3
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 6 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -36,6 +36,8 @@ public static final class Arm {
 		public static final boolean MOTOR_INVERTED_FOLLOWER = false;
 		public static final double ROTATION_TO_RAD = 2 * Math.PI;
 		public static final boolean ENCODER_INVERTED = false;
+
+
 		public static final int MAX_VOLTAGE = 12;
 		public static final double ENCODER_OFFSET_RAD = 0;
 
@@ -66,6 +68,8 @@ public static final class Arm {
 		//if needed
 		public static final double COM_ARM_LENGTH_METERS = 0.381 ;
 		public static final double ARM_MASS_KG = 9.59302503;
+
+
 		public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);
 		//other0;
 

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -64,6 +64,7 @@ public class Arm extends SubsystemBase {
     private final ArmFeedforward armFeed = new ArmFeedforward(kS, kG, kV, kA);
     private final SparkPIDController armPIDMaster = armMotorMaster.getPIDController();
     private final SparkPIDController armPIDFollower = armMotorFollower.getPIDController();
+    private static TrapezoidProfile.State setPoint;
 
     private TrapezoidProfile armProfile = new TrapezoidProfile(TRAP_CONSTRAINTS);
     TrapezoidProfile.State goalState = new TrapezoidProfile.State(0,0);//TODO: update pos later
@@ -77,9 +78,10 @@ public Arm() {
         armMotorMaster.setIdleMode(IdleMode.kBrake);
         armMotorFollower.setInverted(MOTOR_INVERTED_FOLLOWER);
         armMotorFollower.setIdleMode(IdleMode.kBrake);
-
+        // Comment out when running sysid
         armMasterEncoder.setPositionConversionFactor(ROTATION_TO_RAD);
         armMasterEncoder.setVelocityConversionFactor(ROTATION_TO_RAD);
+        // ------------------------------------------------------------
         armMasterEncoder.setInverted(ENCODER_INVERTED);
 
         armMotorFollower.follow(armMotorMaster);
@@ -96,19 +98,19 @@ public Arm() {
 
         //armPID.setTolerance(posToleranceRad, velToleranceRadPSec);
 
-        armPIDMaster.setFeedbackDevice(armMotorMaster.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle));
+        armPIDMaster.setFeedbackDevice(armMasterEncoder);
         armPIDMaster.setPositionPIDWrappingEnabled(true);
         armPIDMaster.setPositionPIDWrappingMinInput(LOWER_ANGLE_LIMIT);
         armPIDMaster.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT);
         //two PIDs?
         armPIDFollower.setFeedbackDevice(armMotorFollower.getEncoder());
         armPIDFollower.setPositionPIDWrappingEnabled(true);
-        armPIDFollower.setPositionPIDWrappingMinInput(LOWER_ANGLE_LIMIT);
-        armPIDFollower.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT);
+        armPIDFollower.setPositionPIDWrappingMinInput(LOWER_ANGLE_LIMIT - Math.PI); //Wierd math, im not sure it this works but basically
+        armPIDFollower.setPositionPIDWrappingMaxInput(UPPER_ANGLE_LIMIT - Math.PI);// since absolute is between -180 and 180 and relative is between 0 and 360(correct me if im wrong) so if we subract 180, then its the same
 
         SmartDashboard.putData("Arm", this);
 
-
+        setPoint = getCurrentArmState();
 
         goalState = getCurrentArmState();
 
@@ -162,7 +164,7 @@ public void autoCancelArmCommand() {
     //#region Drive Methods
     private void driveArm(){
 
-      TrapezoidProfile.State setPoint = armProfile.calculate(kDt, getCurrentArmState(), goalState);
+      setPoint = armProfile.calculate(kDt, setPoint, goalState);
       double armFeedVolts = armFeed.calculate(goalState.position, goalState.velocity);
       if ((getArmPos() < LOWER_ANGLE_LIMIT && getCurrentArmGoal().velocity > 0) || (getArmPos() > UPPER_ANGLE_LIMIT && getCurrentArmGoal().velocity > 0)){
         armFeedVolts = armFeed.calculate(getCurrentArmGoal().position, 0);