diff --git a/src/main/deploy/pathplanner/LeftSideRamTotes.path b/src/main/deploy/pathplanner/LeftSideRamTotes.path new file mode 100644 index 0000000..376d9de --- /dev/null +++ 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": [] +} \ No newline at end of file diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java index ccc5eae..f690f05 100644 --- 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 index c787001..64ac8ee 100644 --- 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 new file mode 100644 index 0000000..90dd6e0 --- /dev/null +++ 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 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 index 7c9dde6..832d2ea 100644 --- 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);