added getArmClampedGoal

DeepBlueRobotics · Jan 17, 2024 · 07dffeb · 07dffeb
1 parent b9495e1
commit 07dffeb
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
@@ -26,8 +26,9 @@ public static final class Arm {
 		public static final double ampAngle = .3;
 		public static final double speakerAngle = .4;
 		//if needed
-//		public static final trapAngle = 80;
-
+		public static final double UPPER_ANGLE = 70;
+		public static final double LOWER_ANGLE = 0;
+		public static final double ARM_DICONT_RAD = (LOWER_ANGLE + UPPER_ANGLE) /2 - Math.PI;
 		//Motor Controllers: pid, FF
 		public static final double[] pidVals = new double[] { 0.1, 0.0, 0.1 };
 		// Feedforward

diff --git a/src/main/java/org/carlmontrobotics/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/subsystems/Arm.java
@@ -23,6 +23,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
+
 public class Arm extends SubsystemBase {
     private final CANSparkMax armMotor = MotorControllerFactory.createSparkMax(Constants.Arm.MOTOR_PORT,MotorConfig.NEO);
     private final SimpleMotorFeedforward armFeed = new SimpleMotorFeedforward(Constants.Arm.kS, Constants.Arm.kV);
@@ -41,20 +42,24 @@ public Arm(XboxController controller) {
       this.controller = controller;
     }
 
-    public void setArmPos(double targetPosition, double targetVelocity) {
+    public void setArmGoal(double targetPosition, double targetVelocity) {
       //Sets arm to the optimal angle for amp, speaker and Ground intake | used to score in amp
       //these values are in constants
       //pass in where scorign and use switch statement
-
+      targetPosition = getArmClampedGoal(targetPosition);
 
     }
-
+    public static boolean positionForbidden(double armPos) {
+
+      return false;
+    }
     public void controllerSetSpeed(double rightJoystick) {
       //move the arm around based off the right joystick movement on the manipulator joystick
       //use the trapezoid thingy from robot code 2023
+
     }
     public double getArmClampedGoal(double goal) {
-      return MathUtil.clamp(MathUtil.inputModulus(goal, goal, goal), goal, goal);
+      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);
     }
     @Override
     public void periodic() {