Skip to content

Commit

Permalink
Merge pull request #21 from 4201VitruvianBots/drive-auto-testing
Browse files Browse the repository at this point in the history 
DriveForward auto
  • Loading branch information
@jonathandao0
jonathandao0 authored Jan 20, 2025
2 parents 38b8d72 + 46fcdc5 commit f2e52ae
Show file tree
Hide file tree
Showing 13 changed files with 284 additions and 120 deletions.
16 changes: 8 additions & 8 deletions src/main/deploy/pathplanner/paths/DriveForward.path
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,20 +8,20 @@
},
"prevControl": null,
"nextControl": {
"x": 2.425937499999999,
"x": 1.6759375,
"y": 6.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.0,
"y": 6.0
"x": 4.648,
"y": 5.9878289473684205
},
"prevControl": {
"x": 3.0,
"y": 6.0
"x": 4.398,
"y": 5.9878289473684205
},
"nextControl": null,
"isLocked": false,
Expand All @@ -33,8 +33,8 @@
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxVelocity": 5.5,
"maxAcceleration": 6.7,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
Expand All @@ -50,5 +50,5 @@
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}
86 changes: 86 additions & 0 deletions src/main/deploy/pathplanner/paths/New Path.path
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,86 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 1.884630681818182,
"y": 6.921747159090909
},
"prevControl": null,
"nextControl": {
"x": 2.134630681818182,
"y": 6.921747159090909
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.029,
"y": 6.922
},
"prevControl": {
"x": 3.779,
"y": 6.922
},
"nextControl": {
"x": 4.279,
"y": 6.922
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.029,
"y": 5.645383522727273
},
"prevControl": {
"x": 3.779,
"y": 5.645383522727273
},
"nextControl": {
"x": 4.279,
"y": 5.645383522727273
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.884630681818182,
"y": 5.645383522727273
},
"prevControl": {
"x": 1.6346306818181817,
"y": 5.645383522727273
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": true
}
32 changes: 32 additions & 0 deletions src/main/deploy/pathplanner/settings.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
{
"robotWidth": 0.9398,
"robotLength": 0.9398,
"holonomicMode": true,
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"defaultNominalVoltage": 12.0,
"robotMass": 68.0,
"robotMOI": 6.883,
"robotTrackwidth": 0.546,
"driveWheelRadius": 0.0508,
"driveGearing": 5.36,
"maxDriveSpeed": 5.96,
"driveMotorType": "krakenX60",
"driveCurrentLimit": 60.0,
"wheelCOF": 1.2,
"flModuleX": 0.34925,
"flModuleY": 0.34925,
"frModuleX": 0.34925,
"frModuleY": -0.34925,
"blModuleX": -0.34925,
"blModuleY": 0.34925,
"brModuleX": -0.34925,
"brModuleY": -0.34925,
"bumperOffsetX": 0.0,
"bumperOffsetY": 0.0,
"robotFeatures": []
}
125 changes: 68 additions & 57 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,23 +6,31 @@

import static edu.wpi.first.units.Units.*;

import com.ctre.phoenix6.SignalLogger;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.wpilibj.Joystick;
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.InstantCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.commands.DriveForward;
import frc.robot.commands.ResetGyro;
import frc.robot.commands.RunAlgaeIntake;
import frc.robot.commands.RunCoralOuttake;
import frc.robot.commands.SwerveCharacterization;
import frc.robot.constants.ROBOT;
import frc.robot.constants.SWERVE;
import frc.robot.constants.SWERVE.ROUTINE_TYPE;
import frc.robot.constants.USB;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.AlgaeIntake;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.CoralOuttake;
import frc.robot.utils.SysIdUtils;
import frc.robot.utils.Telemetry;

/**
Expand All @@ -41,6 +49,7 @@ public class RobotContainer {
// Replace with CommandPS4Controller or CommandJoystick if needed
private final Joystick leftJoystick = new Joystick(USB.leftJoystick);
private final SendableChooser<Command> m_sysidChooser = new SendableChooser<>();
private final SendableChooser<Command> m_chooser = new SendableChooser<>();
private final Joystick rightJoystick = new Joystick(USB.rightJoystick);
private final CommandXboxController m_driverController =
new CommandXboxController(USB.xBoxController);
Expand All @@ -66,7 +75,6 @@ public RobotContainer() {

// Configure the trigger bindings
configureBindings();
initAutoChooser();
}

private void initializeSubSystems() {
Expand All @@ -87,60 +95,67 @@ private void initializeSubSystems() {
}

private void initAutoChooser() {
// Add the autonomous chooser to the dashboard
// SmartDashboard.putData("Auto Mode", m_chooser);
SmartDashboard.putData("Auto Mode", m_chooser);
m_chooser.setDefaultOption("Do Nothing", new WaitCommand(0));

m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive));
}

private void initSmartDashboard() {
// if (ROBOT.useSysID) initSysidChooser();
// else initAutoChooser();
if (ROBOT.useSysID) initSysidChooser();
else initAutoChooser();
SmartDashboard.putData("ResetGyro", new ResetGyro(m_swerveDrive));
}

// private void initSysidChooser(){
// SignalLogger.setPath("/media/sda1/");

// SysIdUtils.createSwerveDriveRoutines(m_swerveDrive);
// SysIdUtils.createSwerveTurnRoutines(m_swerveDrive);

// SmartDashboard.putData(
// "Start Logging", new InstantCommand(SignalLogger::start).ignoringDisable(true));
// SmartDashboard.putData(
// "Stop Logging", new InstantCommand(SignalLogger::stop).ignoringDisable(true));
// SmartDashboard.putData(
// "initDriveSettings",
// new InstantCommand(m_swerveDrive::initDriveSysid).ignoringDisable(true));
// SmartDashboard.putData(
// "initTurnSettings",new
// InstantCommand(m_swerveDrive::initTurnSysid).ignoringDisable(true));

// m_sysidChooser.addOption(
// "driveQuasistaticForward",
// new SwerveDriveQuasistatic(m_swerveDrive, SysIdRoutine.Direction.kForward));
// m_sysidChooser.addOption(
// "driveQuasistaticBackwards",
// new SwerveDriveQuasistatic(m_swerveDrive, SysIdRoutine.Direction.kReverse));
// m_sysidChooser.addOption(
// "driveDynamicForward",
// new SwerveDriveDynamic(m_swerveDrive, SysIdRoutine.Direction.kForward));
// m_sysidChooser.addOption(
// "driveDynamicBackward",
// new SwerveDriveDynamic(m_swerveDrive, SysIdRoutine.Direction.kReverse));

// m_sysidChooser.addOption(
// "turnQuasistaticForward",
// new SwerveTurnQuasistatic(m_swerveDrive, SysIdRoutine.Direction.kForward));
// m_sysidChooser.addOption(
// "turnQuasistaticBackwards",
// new SwerveTurnQuasistatic(m_swerveDrive, SysIdRoutine.Direction.kReverse));
// m_sysidChooser.addOption(
// "turnDynamicForward",
// new SwerveTurnDynamic(m_swerveDrive, SysIdRoutine.Direction.kForward));
// m_sysidChooser.addOption(
// "turnDynamicBackward",
// new SwerveTurnDynamic(m_swerveDrive, SysIdRoutine.Direction.kReverse));

// }
private void initSysidChooser() {
SignalLogger.setPath("/media/sda1/");

SysIdUtils.createSwerveDriveRoutines(m_swerveDrive);
SysIdUtils.createSwerveTurnRoutines(m_swerveDrive);

SmartDashboard.putData(
"Start Logging", new InstantCommand(SignalLogger::start).ignoringDisable(true));
SmartDashboard.putData(
"Stop Logging", new InstantCommand(SignalLogger::stop).ignoringDisable(true));
SmartDashboard.putData(
"initDriveSettings",
new InstantCommand(m_swerveDrive::initDriveSysid).ignoringDisable(true));

m_sysidChooser.addOption(
"driveQuasistaticForward",
new SwerveCharacterization(
m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_QUASISTATIC));
m_sysidChooser.addOption(
"driveQuasistaticBackwards",
new SwerveCharacterization(
m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_QUASISTATIC));
m_sysidChooser.addOption(
"driveDynamicForward",
new SwerveCharacterization(
m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_DYNAMIC));
m_sysidChooser.addOption(
"driveDynamicBackward",
new SwerveCharacterization(
m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_DYNAMIC));

m_sysidChooser.addOption(
"turnQuasistaticForward",
new SwerveCharacterization(
m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_QUASISTATIC));
m_sysidChooser.addOption(
"turnQuasistaticBackwards",
new SwerveCharacterization(
m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_QUASISTATIC));
m_sysidChooser.addOption(
"turnDynamicForward",
new SwerveCharacterization(
m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_DYNAMIC));
m_sysidChooser.addOption(
"turnDynamicBackward",
new SwerveCharacterization(
m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_DYNAMIC));
}

/**
* Use this method to define your trigger->command mappings. Triggers can be created via the
* {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
Expand All @@ -151,13 +166,9 @@ private void initSmartDashboard() {
* joysticks}.
*/
private void configureBindings() {
// // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
// new Trigger(m_exampleSubsystem::exampleCondition)
// .onTrue(new ExampleCommand(m_exampleSubsystem));
//
// // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
// // cancelling on release.
// m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
// Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
// cancelling on release.
// m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
m_driverController.leftBumper().whileTrue(new RunCoralOuttake(m_coralOuttake, 0.15)); // outtake
m_driverController
.rightBumper()
Expand All @@ -172,7 +183,7 @@ private void configureBindings() {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return new WaitCommand(0);
return m_chooser.getSelected();
}

public void testInit() {
Expand Down
Loading

0 comments on commit f2e52ae

Please sign in to comment.