feat: a few working autos and tuned to driver preferences

Co-authored-by: DaviTheDinosaur <[email protected]>
flamingchickens1540 · Dec 16, 2023 · 93861ad · 93861ad
1 parent 71b96c2
commit 93861ad
Show file tree

Hide file tree

Showing 5 changed files with 109 additions and 5 deletions.
diff --git a/src/main/deploy/pathplanner/LeftSideRamTotes.path b/src/main/deploy/pathplanner/LeftSideRamTotes.path
@@ -0,0 +1,74 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.0449181196961987,
+        "y": 6.538041767106257
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.0395434268215062,
+        "y": 6.538041767106257
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 10.256617190445908,
+        "y": 6.559956990906613
+      },
+      "prevControl": {
+        "x": 8.890568240223708,
+        "y": 6.494211319505545
+      },
+      "nextControl": {
+        "x": 8.890568240223708,
+        "y": 6.494211319505545
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 1.8192560273087832,
+        "y": 7.604582658723591
+      },
+      "prevControl": {
+        "x": 2.856576620525642,
+        "y": 7.538836987322523
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}
diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java
@@ -23,7 +23,7 @@ public enum SimulationMode {
 
     public static class DrivetrainConstants {
         public static final double GEAR_RATIO = 6.11;
-        public static final double WHEEL_DIAMETER = Units.inchesToMeters(3.973); // The drivetrain wheels have slightly different diameters, so we take the average
+        public static final double WHEEL_DIAMETER = Units.inchesToMeters(4); // The drivetrain wheels have slightly different diameters, so we take the average
         public static final double TRACK_WIDTH = Units.inchesToMeters(22.75);
 
         public static final double MASS = Units.lbsToKilograms(118); // TODO: 11/18/2023 omg its tem 118 teh robnots

diff --git a/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java b/src/main/java/org/team1540/bunnybotTank2023/RobotContainer.java
@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import org.team1540.bunnybotTank2023.commands.auto.AutoLeftSideRamTotes;
 import org.team1540.bunnybotTank2023.commands.auto.AutoShoot5RamTotes;
 import org.team1540.bunnybotTank2023.commands.auto.TestAuto;
 import org.team1540.bunnybotTank2023.commands.drivetrain.ArcadeDriveCommand;
@@ -96,14 +97,14 @@ private void configureButtonBindings() {
 
         copilot.y().whileTrue(new TurretZeroSequenceCommand(turret));
         copilot.leftBumper().whileTrue(new TurretSetpointCommand(turret, Rotation2d.fromDegrees(0)));
-        copilot.rightTrigger().onTrue(new ShootSequenceCommand(shooter, indexer, 2500));
+        copilot.rightTrigger().onTrue(new ShootSequenceCommand(shooter, indexer, 3218));
         copilot.leftTrigger().whileTrue(new TurretTrackTargetCommand(turret, limelight));
 
 //        copilot.x().onTrue(new InstantCommand(() -> intake.setFold(false))).onFalse(new InstantCommand(() -> intake.setFold(true)));
     }
 
     public void setTeleopDefaultCommands() {
-        drivetrain.setDefaultCommand(new TankdriveCommand(drivetrain, driver));
+        drivetrain.setDefaultCommand(new ArcadeDriveCommand(drivetrain, driver));
         shooter.setDefaultCommand(new StartEndCommand(
                 shooter::stop,
                 () -> {},
@@ -134,6 +135,7 @@ public void setAutoDefaultCommands() {
     private void initAutoChooser() {
         autoChooser.addDefaultOption("ZeroTurret", new TurretZeroSequenceCommand(turret));
         autoChooser.addOption("Forward1Meter", new TestAuto(drivetrain));
+        autoChooser.addOption("LeftSideRamTotes", new AutoLeftSideRamTotes(drivetrain, turret));
     }
 
 

diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/auto/AutoLeftSideRamTotes.java b/src/main/java/org/team1540/bunnybotTank2023/commands/auto/AutoLeftSideRamTotes.java
@@ -0,0 +1,28 @@
+package org.team1540.bunnybotTank2023.commands.auto;
+
+import com.pathplanner.lib.PathConstraints;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import org.team1540.bunnybotTank2023.commands.drivetrain.Drivetrain;
+import org.team1540.bunnybotTank2023.commands.turret.Turret;
+import org.team1540.bunnybotTank2023.commands.turret.TurretZeroSequenceCommand;
+import org.team1540.bunnybotTank2023.utils.AutoCommand;
+
+import java.util.List;
+
+public class AutoLeftSideRamTotes extends AutoCommand {
+    public AutoLeftSideRamTotes(Drivetrain drivetrain, Turret turret) {
+        List<Command> pathCommands = getPathPlannerDriveCommandGroup(
+                drivetrain,
+                "LeftSideRamTotes",
+                new PathConstraints[] {new PathConstraints(4, 2)},
+                true
+        );
+        addCommands(
+                Commands.parallel(
+                        pathCommands.get(0),
+                        new TurretZeroSequenceCommand(turret).asProxy()
+                )
+        );
+    }
+}
diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/drivetrain/ArcadeDriveCommand.java b/src/main/java/org/team1540/bunnybotTank2023/commands/drivetrain/ArcadeDriveCommand.java
@@ -24,10 +24,10 @@ public void execute() {
         double throttle = MathUtil.applyDeadband(-xBoxController.getLeftY(), Constants.DEADZONE_RADIUS);
         double turn = MathUtil.applyDeadband(xBoxController.getRightX(), Constants.DEADZONE_RADIUS);
         double left = leftRateLimiter.calculate(
-                MathUtil.clamp(throttle + turn,-0.75, 0.75)
+                MathUtil.clamp(throttle + turn,-1, 1)
         );
         double right  = rightRateLimiter.calculate(
-                MathUtil.clamp(throttle - turn, -0.75, 0.75)
+                MathUtil.clamp(throttle - turn, -1, 1)
         );
 
         drivetrain.drive(left, right);