buttons, constants, and arm bounds

DeepBlueRobotics · Mar 3, 2024 · 32bf0ca · 32bf0ca
1 parent 173787b
commit 32bf0ca
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 9 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -36,9 +36,6 @@ 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 double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0; //placeholder
-		public static final double POS_TOLERANCE_RAD = 0; //placeholder
-		public static final double VEL_TOLERANCE_RAD_P_SEC = 0; //placeholder
 		public static final int MAX_VOLTAGE = 12;
 		public static final double ENCODER_OFFSET_RAD = 0;
 
@@ -69,15 +66,18 @@ 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 final double UPPER_ANGLE_LIMIT = Units.degreesToRadians(70); 
-		public static final double LOWER_ANGLE_LIMIT = Units.degreesToRadians(0);
-		public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
 		public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_FF_VEL, MAX_FF_ACCEL);
 		//other0;
 
 		public static final double MARGIN_OF_ERROR = Math.PI/18;
-		//Boundaries
 
+		//Boundaries
+		public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 0; //placeholder
+		public static final double POS_TOLERANCE_RAD = 0; //placeholder
+		public static final double VEL_TOLERANCE_RAD_P_SEC = 0; //placeholder
+		public static final double UPPER_ANGLE_LIMIT = Units.degreesToRadians(70); 
+		public static final double LOWER_ANGLE_LIMIT = Units.degreesToRadians(0);
+		public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT + UPPER_ANGLE_LIMIT) /2 - Math.PI;
 
 
 		//Arm buttons

diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -16,7 +16,7 @@
 
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController.Axis;
-
+import edu.wpi.first.wpilibj.XboxController.Button;
 //commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -27,6 +27,7 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
 public class RobotContainer {
   //defaultCommands: elevator, dt
@@ -60,6 +61,10 @@ private void setDefaultCommands() {
     arm.setDefaultCommand(new ArmTeleop(arm, () -> inputProcessing(getStickValue(manipulatorController, Axis.kLeftY))));
   }
   private void setBindingsDriver() {
+    new JoystickButton(driverController, Button.kX.value).whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    new JoystickButton(driverController, Button.kY.value).whileTrue(arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    new JoystickButton(driverController, Button.kB.value).whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    new JoystickButton(driverController, Button.kA.value).whileTrue(arm.sysIdDynamic(SysIdRoutine.Direction.kReverse));
     // 4 cardinal directions on arrowpad
 		// slowmode toggle on trigger
 		// 3 cardinal directions on letterpad

diff --git a/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/commands/ArmTeleop.java
@@ -51,7 +51,7 @@ public void execute() {
 
     double goalArmRad = goalState.position + speeds * deltaT; 
     goalArmRad = MathUtil.clamp(goalArmRad, LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT);
-    goalState = new TrapezoidProfile.State(goalArmRad, 0);
+    goalState = new TrapezoidProfile.State(goalArmRad, armSubsystem.getArmPos()-ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD);
     armSubsystem.setArmTarget(goalState.position);
     lastTime = currTime;
   }