Merge pull request #49 from LakeEffectRobotics/cone-cube-auto

Cone cube auto
LakeEffectRobotics · Apr 6, 2023 · 2dfa2dd · 2dfa2dd
2 parents ea21e8a + e8e1efb
commit 2dfa2dd
Show file tree

Hide file tree

Showing 15 changed files with 224 additions and 49 deletions.
diff --git a/src/main/deploy/pathplanner/flat 1 cone 1 cube.path b/src/main/deploy/pathplanner/flat 1 cone 1 cube.path
@@ -0,0 +1,114 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.8771305963664562,
+        "y": 4.97
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.640460052849931,
+        "y": 4.914954391312292
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "score cone"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.33871636164325,
+        "y": 5.67668854635955
+      },
+      "prevControl": {
+        "x": 6.4838085816522515,
+        "y": 5.440913688844922
+      },
+      "nextControl": {
+        "x": 6.4838085816522515,
+        "y": 5.440913688844922
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 7.1056072869487314,
+        "y": 4.559477928847667
+      },
+      "prevControl": {
+        "x": 6.994098315862784,
+        "y": 4.732936328314697
+      },
+      "nextControl": {
+        "x": 6.994098315862784,
+        "y": 4.732936328314697
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 1.9885845026713236,
+        "y": 4.48
+      },
+      "prevControl": {
+        "x": 5.879008605003299,
+        "y": 4.373629643704419
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "outtake cube"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "maxVelocity": 1.4,
+  "maxAcceleration": 1.0,
+  "isReversed": true,
+  "markers": [
+    {
+      "position": 0.15,
+      "names": [
+        "event",
+        "intake cube"
+      ]
+    }
+  ]
+}
diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
@@ -140,7 +140,7 @@ private static class OPERATOR_MAP {
 
     private static double processDriveInput(double raw) {
         // TODO: Configure input processing to suit your liking
-        if (Math.abs(raw) < 0.05) {
+        if (Math.abs(raw) < 0.1) {
             raw = 0;
         }
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -104,6 +104,9 @@ public RobotContainer() {
 
     autoChooser.addOption("score cone", new AutoScoreConeCommand(arm, wrist, claw));
 
+    // CONES!!
+    autoChooser.addOption("flat 1 cone 1 cube", createPathUtils.createPathCommand("flat 1 cone 1 cube", 1.4, 1, true));
+
     Shuffleboard.getTab("my favourite tab")
       .add(autoChooser)
       .withPosition(3, 3)

diff --git a/src/main/java/frc/robot/commands/ScoringPositionCommand.java b/src/main/java/frc/robot/commands/ScoringPositionCommand.java
@@ -29,6 +29,7 @@ public class ScoringPositionCommand  extends CommandBase {
      * @param targetSelection
      */
     public ScoringPositionCommand(Arm arm, Wrist wrist, TargetSelection targetSelection){
+        addRequirements(arm);
         this.arm = arm;
         this.wrist = wrist;
         this.targetSelection = targetSelection;
@@ -42,6 +43,7 @@ public ScoringPositionCommand(Arm arm, Wrist wrist, TargetSelection targetSelect
      * @param targetSelection
      */
     public ScoringPositionCommand(Arm arm, Wrist wrist, Height height, Type type){
+        addRequirements(arm);
         this.arm = arm;
         this.wrist = wrist;
         this.type = type;
@@ -59,7 +61,7 @@ public void initialize() {
         }
 
         if (type == Type.CONE) {
-            wristAngle = Wrist.SCORE_CONE;
+            wristAngle = Wrist.SCORE_HIGH_CONE;
 
             // extend telescope based on target selection height
             // raise one or both pistons according to height as mid doesnt need 2
@@ -80,17 +82,20 @@ public void initialize() {
             // if wrist is ALIVE, dont need to use telescope (? unless it turns out to be useful ? ??)
             // so, just move wrist
             if (!wrist.isWristDeadAgain) {
-                telescopePosition = 0;
-
                 switch(height){
                     case HIGH:
-                        wristAngle = Wrist.SCORE_CUBE_FORWARD;
+                        telescopePosition = Arm.ALL_CUBE;
+                        wristAngle = Wrist.SCORE_HIGH_CUBE_FORWARD;
+                        arm.raiseBothPistons();
                         break;
                     case MID:
-                        wristAngle = Wrist.SCORE_CUBE_FORWARD;
+                        telescopePosition = Arm.ALL_CUBE;
+                        wristAngle = Wrist.SCORE_MID_CUBE_FORWARD;
+                        arm.raiseBothPistons();
                         break;
                     case LOW:
                         // transport position is conveniently also good for low cube backwards
+                        telescopePosition = 0;
                         wristAngle = Wrist.TRANSPORT;
                         break;
                 }
@@ -99,9 +104,11 @@ public void initialize() {
                 switch(height){
                     case HIGH:
                         telescopePosition = Arm.HIGH_CUBE_DEAD;
+                        arm.raiseBothPistons();
                         break;
                     case MID:
                         telescopePosition = Arm.MID_CUBE_DEAD;
+                        arm.raiseBothPistons();
                         break;
                     case LOW:
                         telescopePosition = 0;
@@ -110,6 +117,12 @@ public void initialize() {
             }
         }
         wrist.setTargetAngle(wristAngle);
+
+        // help wrist get to position when it starts way back with arm up
+        // probably temporary until motor alives more?
+        if (auto) {
+            wrist.setMotors(-1);
+        }
     }
 
     @Override
@@ -118,7 +131,7 @@ public void execute(){
         // otherwise keep waiting until wrist is close or timeout passes
         // OR, if timeout passes, continue even without wrist in position because something gone wrong
         if (!wrist.isWristDeadAgain) {
-            if (Math.abs(wrist.getCurrentAngle() - wristAngle) < 10  || System.currentTimeMillis() - startTime < timeout) {
+            if (Math.abs(wrist.getCurrentAngle() - wristAngle) < 10  || System.currentTimeMillis() - startTime > timeout) {
                 arm.setTelescopePosition(telescopePosition);
             }
         } else {
@@ -131,7 +144,7 @@ public boolean isFinished(){
         // if telescope and wrist are pretty much in position, end command
         // or if timeout passes, end command even if nothing is in position because something gone wrong!
         if (!wrist.isWristDeadAgain) {
-            if ((Math.abs(arm.getTelescopePosition() - telescopePosition) < 3 && Math.abs(wrist.getCurrentAngle() - wristAngle) < 10)  || System.currentTimeMillis() - startTime < timeout) {
+            if ((Math.abs(arm.getTelescopePosition() - telescopePosition) < 3 && Math.abs(wrist.getCurrentAngle() - wristAngle) < 10)  || System.currentTimeMillis() - startTime > timeout) {
                 return true;
             }
         } else {

diff --git a/src/main/java/frc/robot/commands/SpinClawCommand.java b/src/main/java/frc/robot/commands/SpinClawCommand.java
@@ -98,14 +98,6 @@ public void execute() {
                 }
             }
         }
-
-        // show on shuffleboard if limit switch pressed
-        if (claw.GetLimitPressed()) {
-            claw.limitswitchShuffle.setBoolean(true);
-        } else {
-            claw.limitswitchShuffle.setBoolean(false);
-        }
-
     }
 
     @Override

diff --git a/src/main/java/frc/robot/commands/autonomous/AutoIntakeCommand.java b/src/main/java/frc/robot/commands/autonomous/AutoIntakeCommand.java
@@ -27,7 +27,7 @@ public class AutoIntakeCommand extends SequentialCommandGroup {
 
   private static final double CLAW_SPEED = 0.8;
 
-  private static final double TIMEOUT = 8;
+  private static final double TIMEOUT = 5;
 
   /** Creates a new AutoIntakeCommand. */
   public AutoIntakeCommand(Drivetrain drivetrain, Arm arm, Wrist wrist, Claw claw, TargetSelection targetSelection) {

diff --git a/src/main/java/frc/robot/commands/autonomous/AutoScoreConeCommand.java b/src/main/java/frc/robot/commands/autonomous/AutoScoreConeCommand.java
@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.ScoringPositionCommand;
 import frc.robot.commands.instant.SetClawCommand;
+import frc.robot.commands.instant.TransportPositionCommand;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.Wrist;
@@ -16,9 +17,12 @@ public AutoScoreConeCommand(Arm arm, Wrist wrist, Claw claw) {
         addCommands(
             new ScoringPositionCommand(arm, wrist, Height.HIGH, Type.CONE),
 
-            new WaitCommand(1),
+            new WaitCommand(1.7),
 
-            new SetClawCommand(claw, Position.OPEN)
+            new SetClawCommand(claw, Position.OPEN),
+            new WaitCommand(0.3),
+
+            new TransportPositionCommand(arm, wrist)
         );
     }
 }
diff --git a/src/main/java/frc/robot/commands/autonomous/AutoShootBackwardsCommand.java b/src/main/java/frc/robot/commands/autonomous/AutoShootBackwardsCommand.java
@@ -13,6 +13,7 @@
 import frc.robot.commands.instant.LowerArmCommand;
 import frc.robot.commands.instant.RaiseArmCommand;
 import frc.robot.commands.instant.SetWristAngleCommand;
+import frc.robot.commands.instant.TransportPositionCommand;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.TargetSelection;
@@ -25,16 +26,16 @@
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class AutoShootBackwardsCommand extends SequentialCommandGroup {
 
-  private static final double CLAW_SPEED = 0.8;
+  // AIMING FOR MID CUBE FOR NOW
+  private static final double CLAW_SPEED = 0.45;
 
   /** Creates a new AutoShootBackwardsCommand. */
   public AutoShootBackwardsCommand(Arm arm, Wrist wrist, Claw claw, TargetSelection targetSelection) {
-    // Add your commands in the addCommands() call, e.g.
-    // addCommands(new FooCommand(), new BarCommand());
     addCommands(
       // Raise arm and set wrist to correct angle
       new RaiseArmCommand(arm, true),
-    //  new LowerArmCommand(arm),
+      // help wrist move to position?
+      Commands.runOnce(() -> wrist.setMotors(-0.5)),
 
       new SetWristAngleCommand(wrist, Wrist.SCORE_CUBE_BACKWARDS),
 
@@ -45,8 +46,7 @@ public AutoShootBackwardsCommand(Arm arm, Wrist wrist, Claw claw, TargetSelectio
       new SpinClawCommand(claw, Direction.OUT, () -> CLAW_SPEED, Type.CUBE).withTimeout(0.2),
 
       // Set arm and wrist to transport position
-      new SetWristAngleCommand(wrist, Wrist.TRANSPORT),
-      new LowerArmCommand(arm)
+      new TransportPositionCommand(arm, wrist)
     );
   }
 }
diff --git a/src/main/java/frc/robot/commands/autonomous/AutoShootForwardsCommand.java b/src/main/java/frc/robot/commands/autonomous/AutoShootForwardsCommand.java
@@ -4,22 +4,19 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.ShootScoreCommand;
 import frc.robot.commands.instant.SetWristAngleCommand;
+import frc.robot.commands.instant.TransportPositionCommand;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.Wrist;
 import frc.robot.subsystems.TargetSelection.Height;
 
 public class AutoShootForwardsCommand extends SequentialCommandGroup {
 
-    private static final double CLAW_SPEED = 0.8;
-
-    /** Creates a new AutoShootBackwardsCommand. */
+    /** Creates a new shoot forwards command. */
     public AutoShootForwardsCommand(Arm arm, Wrist wrist, Claw claw) {
-      // Add your commands in the addCommands() call, e.g.
-      // addCommands(new FooCommand(), new BarCommand());
       addCommands(
         // Raise arm and set wrist to correct angle
-        new SetWristAngleCommand(wrist, Wrist.SCORE_CUBE_FORWARD),
+        new SetWristAngleCommand(wrist, Wrist.SCORE_HIGH_CUBE_FORWARD),
 
         // Wait 1 second for arm + wrist to be in position
         new WaitCommand(1),
@@ -28,7 +25,7 @@ public AutoShootForwardsCommand(Arm arm, Wrist wrist, Claw claw) {
         new ShootScoreCommand(Height.HIGH , claw),
 
         // Set arm and wrist to transport position
-        new SetWristAngleCommand(wrist, Wrist.TRANSPORT)
+        new TransportPositionCommand(arm, wrist)
       );
     }
 }
diff --git a/src/main/java/frc/robot/commands/instant/SetTelescopeCommand.java b/src/main/java/frc/robot/commands/instant/SetTelescopeCommand.java
@@ -13,6 +13,7 @@ public class SetTelescopeCommand extends CommandBase {
      * @param position extension distance (0, 22)
      */
     public SetTelescopeCommand(Arm arm, double position) {
+        addRequirements(arm);
         this.arm = arm;
         this.position = position;
     }