constants + issues

DeepBlueRobotics · Mar 2, 2024 · 07eec62 · 07eec62
1 parent a6b4f82
commit 07eec62
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 3 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -76,13 +76,19 @@ public static final class Arm {
 		public static final int raiseToClimberButton = Button.kLeftBumper.value;
 		public static final int lowerToClimberButton = Button.kRightBumper.value;
 		//other
-		public static final boolean motorInverted = true; //Todo: find all these
+		public static final boolean motorInverted = true; //Todo: find all these (they are definetely wrong)
 		public static final double rotationToRad = 2 * Math.PI;
 		public static final boolean encoderInverted = false;
 		public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0;
 		public static final double posToleranceRad = 0;
 		public static final double velToleranceRadPSec = 0;
 
+		public static final double MARGIN_OF_ERROR = Math.PI/18;
+		public static final double ARM_LOWER_LIMIT_RAD = -3.569 + MARGIN_OF_ERROR;
+		public static final double ARM_UPPER_LIMIT_RAD = .36 - MARGIN_OF_ERROR;
+
+		public static final double ARM_DISCONTINUITY_RAD = (ARM_LOWER_LIMIT_RAD + ARM_UPPER_LIMIT_RAD) / 2 - Math.PI;
+
 	}
 	public static final class IntakeShooter {
 		//in set() speed

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -51,7 +51,7 @@ public class Arm extends SubsystemBase {
     private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(kS, kV, kA);
     private final SparkPIDController armPID1 = armMotor1.getPIDController();
     private final SparkPIDController armPID2 = armMotor2.getPIDController();
-    private TrapezoidProfile armProfile = new TrapezoidProfile(armConstraints);
+    private TrapezoidProfile armProfile = new TrapezoidProfile(trapConstraints);
     private Timer armProfileTimer = new Timer();
     TrapezoidProfile.State goalState = new TrapezoidProfile.State(0,0);//TODO: update pos later
 
@@ -149,7 +149,7 @@ public void driveArm(double goalAngle) {
     public void setArmTarget(double targetPos) {
         targetPos = getArmClampedGoal(targetPos);
 
-        armProfile = new TrapezoidProfile(armConstraints)
+        armProfile = new TrapezoidProfile(trapConstraints);
         armProfileTimer.reset();
 
         goalState.position = targetPos;