Trying values, testing in teleop

CyberCoyotes · Mar 19, 2024 · dc9199d · dc9199d
1 parent e028f27
commit dc9199d
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -35,14 +35,25 @@ public static class ArmConstants {
     public static final double ARM_HOME_POSE = 0;
     public static final double ARM_LOW_POSE = 10;
     public static final double ARM_MID_POSE = 20; // 30 // 20
-    /* @202 inches
-     * 30 was too steep at 
-     * 40 was too flat
-     */
     public static final double ARM_HIGH_POSE = 55;
     public static final double ARM_AMP_POSE = 90;
+    public static final double ARM_SAFETY_POSE = 20;
     public static final int ARM_FWD_LIMIT = 100; // Previous event 91
 
+    /* Distances (inches) from front Bumper Speaker Base to front Bumper and arm in canon units
+    Pose 0 is at the base, Pose 1 is at the leg of the Stage, i.e. "Safety Pose"
+
+    | Pose  | Dist  | Arm |
+    |------ |-----  | --- |
+    | 0     | 0     | 0   |
+    | 1     | 126   | 20  | Safety Pose
+    | 2     | 150   | 00  |
+    | 3     | 180   | 00  |
+    | 4     | 200   | 00  |
+    
+    */
+
+
     public static final int ARM_MAX_ACCEL = 100;
     public static final int ARM_MAX_VEL = 200;
     public static final int ARM_JERK = 0;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -28,6 +28,7 @@
 import frc.robot.commands.SetIndex;
 import frc.robot.commands.SetWinch;
 import frc.robot.commands.ShootClose;
+import frc.robot.commands.ShootSafetyPose;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.IndexSubsystem;
@@ -88,6 +89,8 @@ public class RobotContainer {
   private final IntakeRevCommandGroup intakeRevGroup = new IntakeRevCommandGroup(index, intake);
   // ChargeIntakeCommand chargeIntake = new ChargeIntakeCommand(drivetrain, intake, driveRequest);
 
+  private final ShootSafetyPose shootSafetyPose = new ShootSafetyPose(arm, index, intake, shooter);
+
   /* Autonomous Chooser */
   SendableChooser<Command> autoChooser;
 
@@ -163,7 +166,9 @@ private void configureBindings() {
     m_driverController.a().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_HOME_POSE)));
     m_driverController.b().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_MID_POSE)));
     m_driverController.x().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_AMP_POSE)));
-    m_driverController.y().whileTrue(new InstantCommand(() -> arm.setArmPose(Constants.ArmConstants.ARM_MID_POSE)));
+
+    // FIXME Using this for Testing
+    m_driverController.y().whileTrue(shootSafetyPose);
 
     m_driverController.rightBumper().whileTrue(new IntakeCommandGroup(index, intake));
     m_driverController.leftBumper().whileTrue(new IntakeRevCommandGroup(index, intake));

diff --git a/src/main/java/frc/robot/commands/ShootSafetyPose.java b/src/main/java/frc/robot/commands/ShootSafetyPose.java
@@ -0,0 +1,22 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
+import frc.robot.subsystems.ArmSubsystem;
+import frc.robot.subsystems.IndexSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ShooterSubsystemVelocity;
+
+public class ShootSafetyPose extends SequentialCommandGroup {
+    public ShootSafetyPose(ArmSubsystem arm, IndexSubsystem index, IntakeSubsystem intake, ShooterSubsystemVelocity shooter) {
+
+        addCommands(
+            /* Moving the arm is shouldn't be necessary, but just in case */
+            new SetArmPosition(arm, Constants.ArmConstants.ARM_SAFETY_POSE),
+            // new ShooterIndex(shooter, index).withTimeout(1.0)
+            new RevAndShootCommand(index, shooter).withTimeout(1.25).andThen(new InstantCommand(() -> shooter.Disable(), shooter)),
+            new SetIndex(index, 0).withTimeout(.2)
+        );
+    }
+}