diff --git a/images/stuff.txt b/images/stuff.txt new file mode 100644 index 0000000..8748d5a --- /dev/null +++ b/images/stuff.txt @@ -0,0 +1,31 @@ + // TrajectoryConfig trajectoryConfig = new TrajectoryConfig( + // Constants.SwerveDrivetrain.kDriveMaxSpeedMPS, + // Constants.SwerveDrivetrain.kDriveMaxAcceleration); + // trajectoryConfig.setKinematics(swerveDrivetrain.getKinematics()); + + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + // new Pose2d(0, 0, new Rotation2d(0)), + // List.of( + // new Translation2d(1, 2), + // new Translation2d(3, 1) + // ), + // new Pose2d(4, 2, Rotation2d.fromDegrees(180.0)), + // trajectoryConfig); + + // swerveDrivetrain.getField().getObject("traj").setTrajectory(trajectory); + + // SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + // trajectory, + // swerveDrivetrain::getSimPose, + // swerveDrivetrain.getKinematics(), + // swerveDrivetrain.xController, + // swerveDrivetrain.yController, + // swerveDrivetrain.thetaController, + // swerveDrivetrain::setModuleStates, + // swerveDrivetrain); + + // return new SequentialCommandGroup( + // new InstantCommand(() -> swerveDrivetrain.resetSimOdometry(trajectory.getInitialPose())), + // swerveControllerCommand, + // new InstantCommand(() -> swerveDrivetrain.stopModules()) + // ); \ No newline at end of file diff --git a/simgui.json b/simgui.json index 231bc57..9413ccf 100644 --- a/simgui.json +++ b/simgui.json @@ -31,6 +31,7 @@ "/LiveWindow/Ungrouped/PIDController[7]": "PIDController", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro", + "/SmartDashboard/Auton Selector": "String Chooser", "/SmartDashboard/Field": "Field2d" }, "windows": { @@ -39,6 +40,11 @@ "visible": true } }, + "/SmartDashboard/Auton Selector": { + "window": { + "visible": true + } + }, "/SmartDashboard/Field": { "Robot": { "style": "Box/Image" @@ -58,6 +64,9 @@ "open": true }, "SmartDashboard": { + "Auton Selector": { + "open": true + }, "Field": { "open": true }, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8589ef7..ca5ce31 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,9 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.commands.AutonPathExample; import frc.robot.commands.SwerveJoystickCommand; import frc.robot.subsystems.SwerveDrivetrain; import edu.wpi.first.wpilibj2.command.Command; @@ -36,6 +39,7 @@ public class RobotContainer { private final Joystick driveJoystick = new Joystick(Constants.SwerveDrivetrain.kDriveJoystickPort); + private final SendableChooser autonSelector = new SendableChooser<>(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { swerveDrivetrain.setDefaultCommand(new SwerveJoystickCommand( @@ -43,7 +47,7 @@ public RobotContainer() { () -> -driveJoystick.getRawAxis(Constants.SwerveDrivetrain.kDriveXAxis), () -> -driveJoystick.getRawAxis(Constants.SwerveDrivetrain.kDriveYAxis), () -> -driveJoystick.getRawAxis(Constants.SwerveDrivetrain.kDriveWAxis), - () -> !driveJoystick.getRawButton(Constants.SwerveDrivetrain.kDriveFieldOrientButtonIdx))); + () -> driveJoystick.getRawButton(Constants.SwerveDrivetrain.kDriveFieldOrientButtonIdx))); // Configure the button bindings configureButtonBindings(); @@ -70,6 +74,8 @@ private void configureButtonBindings() { .whenPressed(() -> swerveDrivetrain.setRotationPointIdx(4)) .whenReleased(() -> swerveDrivetrain.setRotationPointIdx(0)); + autonSelector.setDefaultOption("Example", new AutonPathExample(swerveDrivetrain)); + SmartDashboard.putData("Auton Selector", autonSelector); } /** @@ -78,36 +84,6 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - TrajectoryConfig trajectoryConfig = new TrajectoryConfig( - Constants.SwerveDrivetrain.kDriveMaxSpeedMPS, - Constants.SwerveDrivetrain.kDriveMaxAcceleration); - trajectoryConfig.setKinematics(swerveDrivetrain.getKinematics()); - - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(1, 2), - new Translation2d(3, 1) - ), - new Pose2d(4, 2, Rotation2d.fromDegrees(180.0)), - trajectoryConfig); - - swerveDrivetrain.getField().getObject("traj").setTrajectory(trajectory); - - SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( - trajectory, - swerveDrivetrain::getSimPose, - swerveDrivetrain.getKinematics(), - swerveDrivetrain.xController, - swerveDrivetrain.yController, - swerveDrivetrain.thetaController, - swerveDrivetrain::setModuleStates, - swerveDrivetrain); - - return new SequentialCommandGroup( - new InstantCommand(() -> swerveDrivetrain.resetSimOdometry(trajectory.getInitialPose())), - swerveControllerCommand, - new InstantCommand(() -> swerveDrivetrain.stopModules()) - ); + return autonSelector.getSelected(); } } diff --git a/src/main/java/frc/robot/commands/AutonPathExample.java b/src/main/java/frc/robot/commands/AutonPathExample.java new file mode 100644 index 0000000..6a2cd5e --- /dev/null +++ b/src/main/java/frc/robot/commands/AutonPathExample.java @@ -0,0 +1,83 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import java.util.List; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +import frc.robot.subsystems.SwerveDrivetrain; + +public class AutonPathExample extends SequentialCommandGroup { + /** Creates a new AutonPathExample. */ + private final SwerveDrivetrain swerveDrivetrain; + private SwerveControllerCommand swerveControllerCommand; + private Trajectory trajectory; + + public AutonPathExample(SwerveDrivetrain drivetrain) { + // Use addRequirements() here to declare subsystem dependencies. + this.swerveDrivetrain = drivetrain; + addRequirements(drivetrain); + + trajectory = TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of( + new Translation2d(1, 2), + new Translation2d(3, 1) + ), + new Pose2d(4, 2, Rotation2d.fromDegrees(180.0)), + swerveDrivetrain.getTrajectoryConfig()); + + swerveDrivetrain.getField().getObject("traj").setTrajectory(trajectory); + + swerveControllerCommand = new SwerveControllerCommand( + trajectory, + swerveDrivetrain::getSimPose, + swerveDrivetrain.getKinematics(), + swerveDrivetrain.xController, + swerveDrivetrain.yController, + swerveDrivetrain.thetaController, + swerveDrivetrain::setModuleStates, + swerveDrivetrain); + + addCommands( + new InstantCommand(() -> swerveDrivetrain.resetSimOdometry(trajectory.getInitialPose())), + swerveControllerCommand, + new InstantCommand(() -> swerveDrivetrain.stopModules())); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + super.initialize(); + swerveDrivetrain.setAutonomous(); + } + + @Override + public void execute() { + super.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + super.end(interrupted); + swerveDrivetrain.setDisabled(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return super.isFinished(); + } +} diff --git a/src/main/java/frc/robot/commands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveJoystickCommand.java index 5937230..7bfd742 100644 --- a/src/main/java/frc/robot/commands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveJoystickCommand.java @@ -37,7 +37,9 @@ public SwerveJoystickCommand(SwerveDrivetrain drivetrain, Supplier xSpee // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + drivetrain.setJoystick(); + } // Called every time the scheduler runs while the command is scheduled. @Override @@ -60,7 +62,7 @@ public void execute() { ChassisSpeeds chassisSpeeds; if (fieldOrientedFunc.get()) { chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - vX, vY, vW, drivetrain.getRotation2d()); + vY, vX, vW, drivetrain.getSimRotation2d()); } else { chassisSpeeds = new ChassisSpeeds(vX, vY, vW); @@ -80,6 +82,7 @@ public void execute() { @Override public void end(boolean interrupted) { drivetrain.stopModules(); + drivetrain.setDisabled(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index fa36997..fa63275 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -16,18 +15,25 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; 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.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import frc.robot.Constants; import frc.robot.subsystems.SwerveModule; public class SwerveDrivetrain extends SubsystemBase { // state stuff public static enum DrivetrainState { - AUTON_PATH, JOYSTICK_DRIVE + AUTON_PATH, JOYSTICK_DRIVE, DISABLED } private DrivetrainState state; @@ -43,10 +49,10 @@ public static enum DrivetrainState { private SwerveDriveOdometry simOdometry; private SwerveDriveOdometry odometry; private Field2d field; - private SimpleMotorFeedforward feedforward; public PIDController xController; public PIDController yController; public ProfiledPIDController thetaController; + private TrajectoryConfig trajectoryConfig; // sensors private AHRS gyro; @@ -136,6 +142,12 @@ public SwerveDrivetrain() { Constants.SwerveDrivetrain.m_r_control_D, Constants.SwerveDrivetrain.kThetaControllerConstraints); thetaController.enableContinuousInput(-Math.PI, Math.PI); + + trajectoryConfig = new TrajectoryConfig( + Constants.SwerveDrivetrain.kDriveMaxSpeedMPS, + Constants.SwerveDrivetrain.kDriveMaxAcceleration); + trajectoryConfig.setKinematics(swerveKinematics); + state = DrivetrainState.JOYSTICK_DRIVE; } public void zeroHeading() { @@ -156,6 +168,10 @@ public Rotation2d getRotation2d() { return Rotation2d.fromDegrees(getHeading()); } + public Rotation2d getSimRotation2d() { + return new Rotation2d(simulationData.getHeading()); + } + @Override public void periodic() { // This method will be called once per scheduler run @@ -278,4 +294,29 @@ public int getRotationPointIdx() { public void setRotationPointIdx(int idx) { rotationPoint = idx; } + + public TrajectoryConfig getTrajectoryConfig() { + return trajectoryConfig; + } + + public SequentialCommandGroup getDriveSimCommand(SwerveControllerCommand swerveCommand, Trajectory trajectory) { + Command swerveControllerCommand; + return new SequentialCommandGroup( + new InstantCommand(() -> resetSimOdometry(trajectory.getInitialPose())), + swerveCommand, + new InstantCommand(() -> stopModules()) + ); + } + + public void setAutonomous() { + state = DrivetrainState.AUTON_PATH; + } + + public void setJoystick() { + state = DrivetrainState.JOYSTICK_DRIVE; + } + + public void setDisabled() { + state = DrivetrainState.DISABLED; + } } \ No newline at end of file