Completed merge of new stuff

DeepBlueRobotics · Feb 3, 2024 · 3c19b6a · 3c19b6a
2 parents edc775e + c5f4b75
commit 3c19b6a
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 19 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -6,6 +6,9 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Units.*;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -20,29 +23,30 @@ public static final class Drivetrain {
 	}
 
 	public static final class Arm {
-		// Array Indexes (Just to make things easier to read)
-        public static final int ARM = 0;
 		//Motor port
-		public static final int MOTOR_PORT1 = 7;
-		public final static int MOTOR_PORT2 = 8;
+		public static final int LEFT_MOTOR_PORT = 7;
+		public final static int RIGHT_MOTOR_PORT = 8;
 		//all angles in rot here
-		public static final double intakeAngle = -.1;
-		public static final double ampAngle = .3;
-		public static final double speakerAngle = .4;
+		//TODO: finish understand why this is broken public static final Measure<Angle> INTAKE_ANGLE = Degrees.to(-1);
+		public static final double intakeAngle = Math.toDegrees(-0.1);
+		public static final double ampAngle = Math.toDegrees(0.3);
+		public static final double speakerAngle = Math.toDegrees(0.4);
 		//if needed
-		public static final double UPPER_ANGLE = Math.toRadians(70); 
-		public static final double LOWER_ANGLE = Math.toRadians(0);
-		public static final double ARM_DICONT_RAD = (LOWER_ANGLE + UPPER_ANGLE) /2 - Math.PI;
+		public static final double UPPER_ANGLE_LIMIT = Math.toRadians(70); 
+		public static final double LOWER_ANGLE_LIMIT = Math.toRadians(0);
+		public static final double ARM_DICONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
+		//Motor Controllers: pid, FF
+		public static final double[] pidVals = new double[] { 0.1, 0.0, 0.1 };
 		// Feedforward
 		public static final double kS = 0.1;
 		public static final double kG = 0.1;
 		public static final double kV = 0.1;
 		public static final double kA = 0.1;
 		// PID Constants
 		// placeholder numbers for now
-        public static double[] kP = {0.1, 0.1};
-        public static double[] kI = {0.1, 0.1};
-        public static double[] kD = {0.1, 0.1};
+        public static final double kP = 0.1;
+		public static final double kI = 0.1;
+		public static final double kD = 0.1;
 
 		//Arm measurements - ALL OF THEM ARE PLACEHOLDERS THE NUMBERS MEAN NOTHING
 		public static final double COM_ARM_LENGTH_METERS = 1;

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -34,8 +34,8 @@
 
 
 public class Arm extends SubsystemBase {
-    private final CANSparkMax armMotor1 = MotorControllerFactory.createSparkMax(Constants.Arm.MOTOR_PORT1,MotorConfig.NEO);
-    //private final CANSparkMax armMotor2 = MotorControllerFactory.createSparkMax(Constants.Arm.MOTOR_PORT2,MotorConfig.NEO);
+    private final CANSparkMax armMotor1 = MotorControllerFactory.createSparkMax(Constants.Arm.LEFT_MOTOR_PORT,MotorConfig.NEO);
+    //private final CANSparkMax armMotor2 = MotorControllerFactory.createSparkMax(Constants.Arm.RIGHT_MOTOR_PORT,MotorConfig.NEO);
     //there is only one arm motor. 
     private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(Constants.Arm.kS, Constants.Arm.kV);
     private final SparkAbsoluteEncoder armEncoder = armMotor1.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle);
@@ -78,23 +78,27 @@ public Translation2d getCoM() {
     }
 
     public void driveArm(TrapezoidProfile.State state) {
+
+      /*
+      ignore this math its wrong as it includes wrist 
         double kgv = getKg();
         double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0);
         double armPIDVolts = armPID.calculate(getArmPos(), state.position);
-        if ((getArmPos() > Constants.Arm.UPPER_ANGLE && state.velocity > 0) || 
-            (getArmPos() < Constants.Arm.LOWER_ANGLE && state.velocity < 0)) {
+        if ((getArmPos() > Constants.Arm.UPPER_ANGLE_LIMIT && state.velocity > 0) || 
+            (getArmPos() < Constants.Arm.LOWER_ANGLE_LIMIT && state.velocity < 0)) {
             armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0);
     }
     double volts = armFeedVolts + armPIDVolts;
     armMotor1.setVoltage(volts);
   }
+  */
     public double getArmClampedGoal(double goal) {
       //Find the limits of the arm. Used to move it and ensure that the arm does not move past the amount
-      return MathUtil.clamp(MathUtil.inputModulus(goal, Constants.Arm.ARM_DICONT_RAD, Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI), Constants.Arm.LOWER_ANGLE, Constants.Arm.UPPER_ANGLE);
+      return MathUtil.clamp(MathUtil.inputModulus(goal, Constants.Arm.ARM_DICONT_RAD, Constants.Arm.ARM_DICONT_RAD + 2 * Math.PI), Constants.Arm.LOWER_ANGLE_LIMIT, Constants.Arm.UPPER_ANGLE_LIMIT);
     }
 
     @Override
     public void periodic() {
-
+      //run driveArm constantly and pass in 
 		}
 }