Merge pull request #86 from DeepBlueRobotics/testing-changes-7-7

PID and Limelight changes 7/7/24 worksession
DeepBlueRobotics · Jul 9, 2024 · 8f9938b · 8f9938b
2 parents b47465a + 3f03f17
commit 8f9938b
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 9 deletions.
diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java
@@ -52,12 +52,19 @@ public static final class Effectorc {
 		public static final int INTAKE = 0;
 		public static final int OUTTAKE = 1;
 		// 0.0001184
-		public static final double[] kP = { 0, 0 /* 0.030717,0.0001 */ };
+
+		/*
+		 * public static final double kP = 0.0001067; public static final double kI = 0; public
+		 * static final double kD = 0; public static final double kS = 0; public static final double
+		 * kV = 0.11124; public static final double kA = 0.039757;
+		 */
+
+		public static final double[] kP = {0, 0.0001067};
 		public static final double[] kI = { /* /Intake/ */0, /* /Outake/ */0 };
 		public static final double[] kD = { /* /Intake/ */0, /* /Outake/ */0 };
 		public static final double[] kS = { /* /Intake/ */0.22, /* /Outake/ */0.29753 * 2 };
-		public static final double[] kV = { 0.122, 0/* 0.065239, 0.077913 */ };
-		public static final double[] kA = { 0, 0/* 0.0062809,0.05289 */ };
+		public static final double[] kV = {0.122, 0.11124};
+		public static final double[] kA = {0, 0.039757};
 		public static final int INTAKE_PORT = 9; // port
 		public static final int OUTTAKE_PORT = 10; // port
 		public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port
@@ -173,7 +180,8 @@ public static final class Armc {
 
 		// Boundaries
 		public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 1.8345; // placeholder
-		public static final double POS_TOLERANCE_RAD = Units.degreesToRadians(5); // placeholder //Whether or not this is the actual
+		public static final double POS_TOLERANCE_RAD =
+				Units.degreesToRadians(5); // placeholder //Whether or not this is the actual
 																		// account
 		// idk TODO: test on actual encoder without a conversion
 		// factor
@@ -386,6 +394,7 @@ public static final class Limelightc {
 																// to represent unreliable heading
 		public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity
 
+
 		public static final double ERROR_TOLERANCE_RAD = 0.1; // unused
 		public static final double HORIZONTAL_FOV_DEG = 0; // unused
 		public static final double RESOLUTION_WIDTH_PIX = 640; // unused

diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java
@@ -209,7 +209,9 @@ private void setBindingsManipulator() {
 
     //new JoystickButton(manipulatorController, A_BUTTON)
         //.onTrue(new RampMaxRPMDriving(intakeShooter));
-
+    axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue(
+            new SequentialCommandGroup(new AimArmSpeaker(arm, limelight),
+                    new PassToOuttake(intakeShooter)));
 
     new JoystickButton(manipulatorController, RAMP_OUTTAKE)
         .whileTrue(new RampMaxRPM(intakeShooter));
@@ -221,8 +223,8 @@ private void setBindingsManipulator() {
                     ? new TestRPM(intakeShooter)
                     : new Outtake(intakeShooter, arm)*/
 
-    axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
-            .onTrue(new PassToOuttake(intakeShooter));
+    // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON)
+    // .onTrue(new PassToOuttake(intakeShooter));
 
     axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON)
         .whileTrue(new Intake(intakeShooter));
@@ -241,6 +243,7 @@ private void setBindingsManipulator() {
     new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm));
     new POVButton(manipulatorController, LEFT_D_PAD)
         .onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD));
+
   }
 
 

diff --git a/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java b/src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java
@@ -19,10 +19,11 @@ public AimArmSpeaker(Arm arm, Limelight ll) {
     this.ll = ll;
   }
 
+
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    double goal = ll.getArmAngleToShootSpeakerRad();
+    double goal = ll.getOptimizedArmAngleRadsMT2();
     arm.setArmTarget(goal);
   }
 

diff --git a/src/main/java/org/carlmontrobotics/commands/Intake.java b/src/main/java/org/carlmontrobotics/commands/Intake.java
@@ -12,7 +12,7 @@ public class Intake extends Command {
   // intake until sees game peice or 4sec has passed
   private final IntakeShooter intake;
   private int index = 0;
-  private double increaseSpeed = .01;
+  private double increaseSpeed = .007;
   private double initSpeed = .5;
   private double slowSpeed = .1;