From b0ce8aaad77aa1b5483b8a4cbafb89c0c45e7c7e Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Thu, 30 Jan 2025 19:18:56 -0500 Subject: [PATCH] Modes Autos complets --- .../deploy/pathplanner/autos/BlueBas1.auto | 6 +- .../deploy/pathplanner/autos/BlueBas2.auto | 22 ++++-- .../deploy/pathplanner/autos/BlueBas3.auto | 42 ++++++---- .../deploy/pathplanner/autos/BlueHaut1.auto | 6 +- .../deploy/pathplanner/autos/BlueHaut2.auto | 24 +++--- .../deploy/pathplanner/autos/BlueHaut3.auto | 42 ++++++---- .../deploy/pathplanner/autos/MilieuAlgue.auto | 28 +++++-- .../pathplanner/autos/MilieuPasAlgue.auto | 6 +- src/main/java/frc/robot/RobotContainer.java | 78 +++++++++++++++++-- 9 files changed, 186 insertions(+), 68 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/BlueBas1.auto b/src/main/deploy/pathplanner/autos/BlueBas1.auto index 7c320ae..6b665ea 100644 --- a/src/main/deploy/pathplanner/autos/BlueBas1.auto +++ b/src/main/deploy/pathplanner/autos/BlueBas1.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueBas2.auto b/src/main/deploy/pathplanner/autos/BlueBas2.auto index 62b0de7..57d7bf7 100644 --- a/src/main/deploy/pathplanner/autos/BlueBas2.auto +++ b/src/main/deploy/pathplanner/autos/BlueBas2.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -67,7 +67,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -83,7 +83,7 @@ { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -92,6 +92,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "CoralExpire" + } + }, { "type": "parallel", "data": { @@ -99,7 +105,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -121,7 +127,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -133,7 +139,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueBas3.auto b/src/main/deploy/pathplanner/autos/BlueBas3.auto index ee917a3..fe42865 100644 --- a/src/main/deploy/pathplanner/autos/BlueBas3.auto +++ b/src/main/deploy/pathplanner/autos/BlueBas3.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -67,7 +67,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -83,7 +83,7 @@ { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -92,6 +92,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -99,7 +105,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -121,7 +127,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -133,7 +139,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -159,13 +165,13 @@ { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "CoralInspire" } } ] @@ -174,12 +180,18 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } } ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -197,13 +209,13 @@ { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] @@ -212,7 +224,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } } ] @@ -221,7 +233,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueHaut1.auto b/src/main/deploy/pathplanner/autos/BlueHaut1.auto index 90a7062..05c0205 100644 --- a/src/main/deploy/pathplanner/autos/BlueHaut1.auto +++ b/src/main/deploy/pathplanner/autos/BlueHaut1.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueHaut2.auto b/src/main/deploy/pathplanner/autos/BlueHaut2.auto index f54dfe2..f3cc72f 100644 --- a/src/main/deploy/pathplanner/autos/BlueHaut2.auto +++ b/src/main/deploy/pathplanner/autos/BlueHaut2.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,13 +45,13 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 1.0 } }, { @@ -67,7 +67,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -83,7 +83,7 @@ { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -92,6 +92,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -99,7 +105,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -121,7 +127,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -133,7 +139,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/BlueHaut3.auto b/src/main/deploy/pathplanner/autos/BlueHaut3.auto index f9be6f9..e492533 100644 --- a/src/main/deploy/pathplanner/autos/BlueHaut3.auto +++ b/src/main/deploy/pathplanner/autos/BlueHaut3.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,7 +33,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -45,7 +45,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -67,7 +67,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -83,7 +83,7 @@ { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -92,6 +92,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -99,7 +105,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -121,7 +127,7 @@ { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -133,7 +139,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } }, { @@ -159,13 +165,13 @@ { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "Station" } } ] @@ -174,12 +180,18 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } } ] } }, + { + "type": "named", + "data": { + "name": "CoralInspire" + } + }, { "type": "parallel", "data": { @@ -197,13 +209,13 @@ { "type": "wait", "data": { - "waitTime": 0 + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "L4" } } ] @@ -212,7 +224,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } } ] @@ -221,7 +233,7 @@ { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/deploy/pathplanner/autos/MilieuAlgue.auto b/src/main/deploy/pathplanner/autos/MilieuAlgue.auto index 17073b3..f618a26 100644 --- a/src/main/deploy/pathplanner/autos/MilieuAlgue.auto +++ b/src/main/deploy/pathplanner/autos/MilieuAlgue.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,25 +33,43 @@ { "type": "named", "data": { - "name": null + "name": "L4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "CoralExpire" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "L3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 } }, { "type": "named", "data": { - "name": null + "name": "CoraletAlgue" } } ] diff --git a/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto b/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto index b724388..999860b 100644 --- a/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto +++ b/src/main/deploy/pathplanner/autos/MilieuPasAlgue.auto @@ -17,7 +17,7 @@ { "type": "named", "data": { - "name": null + "name": "AprilTag" } }, { @@ -33,13 +33,13 @@ { "type": "named", "data": { - "name": null + "name": "L4" } }, { "type": "named", "data": { - "name": null + "name": "CoralExpire" } } ] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de2c9d0..8c59bf6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,17 +4,81 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.PointWheelsAt; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; + +import frc.robot.TunerConstants.TunerConstants; +import frc.robot.commands.Grimpe; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Grimpeur; + public class RobotContainer { - public RobotContainer() { - configureBindings(); - } + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - private void configureBindings() {} + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + private final Telemetry logger = new Telemetry(MaxSpeed); + + private final CommandXboxController joystick = new CommandXboxController(0); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + + private final SendableChooser autoChooser; + + + + public RobotContainer() { + autoChooser = AutoBuilder.buildAutoChooser("New Auto"); + SmartDashboard.putData("Auto Mode", autoChooser); + configureBindings(); + } + + private void configureBindings() { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> + drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + ) + ); + + // Run SysId routines when holding back/start and X/Y. + // Note that each routine should be run exactly once in a single log. + joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward)); + joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse)); + joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward)); + joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse)); + + // reset the field-centric heading on left bumper press + joystick.leftBumper().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + + drivetrain.registerTelemetry(logger::telemeterize); + } + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } }