Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-08 16:28:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

formatting

Browse Source
This commit is contained in:
Connor Peach
2024-03-12 11:09:27 -06:00
parent 5aa9a10ca2
commit 88ba685239
30 changed files with 1602 additions and 1602 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+17 -17
View File
@@ -40,28 +40,28 @@ public final class Constants {
public static final double TURBO_SPEED = 4.0; public static final double TURBO_SPEED = 4.0;
public static final class DefaultSwerveRotOffsets { public static final class DefaultSwerveRotOffsets {
public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625; public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625;
public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
public static final double BACK_LEFT_ROT_OFFSET = -277.646484375; public static final double BACK_LEFT_ROT_OFFSET = -277.646484375;
public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625; public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625;
} }
public static final class IDs { public static final class IDs {
public static final int RIGHT_FRONT_WHEEL_ID = 2; public static final int RIGHT_FRONT_WHEEL_ID = 2;
public static final int RIGHT_FRONT_STEER_ID = 3; public static final int RIGHT_FRONT_STEER_ID = 3;
public static final int RIGHT_FRONT_ENCODER_ID = 10; public static final int RIGHT_FRONT_ENCODER_ID = 10;
public static final int LEFT_FRONT_WHEEL_ID = 4; public static final int LEFT_FRONT_WHEEL_ID = 4;
public static final int LEFT_FRONT_STEER_ID = 5; public static final int LEFT_FRONT_STEER_ID = 5;
public static final int LEFT_FRONT_ENCODER_ID = 11; public static final int LEFT_FRONT_ENCODER_ID = 11;
public static final int LEFT_BACK_WHEEL_ID = 6; public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7; public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12; public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int RIGHT_BACK_WHEEL_ID = 8; public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9; public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13; public static final int RIGHT_BACK_ENCODER_ID = 13;
} }
public static final class PIDConstants { public static final class PIDConstants {
@@ -120,7 +120,7 @@ public final class Constants {
// misc // misc
public static final int TIMEOUT_MS = 30; public static final int TIMEOUT_MS = 30;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
} }
public static final class VisionConstants { public static final class VisionConstants {
// public static final String NAME = "photonCamera"; // public static final String NAME = "photonCamera";
src/main/java/frc4388/robot/Main.java
+10 -10
View File
@@ -15,15 +15,15 @@ import edu.wpi.first.wpilibj.RobotBase;
* change the parameter class to the startRobot call. * change the parameter class to the startRobot call.
*/ */
public final class Main { public final class Main {
private Main() { private Main() {
} }
/** /**
* Main initialization function. Do not perform any initialization here. * Main initialization function. Do not perform any initialization here.
* *
* <p>If you change your main robot class, change the parameter type. * <p>If you change your main robot class, change the parameter type.
*/ */
public static void main(String... args) { public static void main(String... args) {
RobotBase.startRobot(Robot::new); RobotBase.startRobot(Robot::new);
} }
} }
src/main/java/frc4388/robot/Robot.java
+101 -101
View File
@@ -23,114 +23,114 @@ import frc4388.utility.RobotTime;
* project. * project.
*/ */
public class Robot extends TimedRobot { public class Robot extends TimedRobot {
Command m_autonomousCommand; Command m_autonomousCommand;
private RobotTime m_robotTime = RobotTime.getInstance(); private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer; private RobotContainer m_robotContainer;
//private LED mled = new LED(); //private LED mled = new LED();
/** /**
* This function is run when the robot is first started up and should be * This function is run when the robot is first started up and should be
* used for any initialization code. * used for any initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
/**
* This function is called every robot packet, no matter the mode. Use
* this for items like diagnostics that you want ran during disabled,
* autonomous, teleoperated and test.doubl
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
//System.out.println(m_robotContainer.limelight.isNearSpeaker());
//mled.updateLED();
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
* You can use it to reset any subsystem information you want to clear when
* the robot is disabled.
*/
@Override
public void disabledInit() {
m_robotTime.endMatchTime();
}
@Override
public void disabledPeriodic() {
}
@Override
public void disabledExit() {
DeferredBlock.execute();
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/ */
@Override
public void robotInit() {
// schedule the autonomous command (example) // Instantiate our RobotContainer. This will perform all our button bindings, and put our
if (m_autonomousCommand != null) { // autonomous chooser on the dashboard.
m_autonomousCommand.schedule(); m_robotContainer = new RobotContainer();
} }
m_robotTime.startMatchTime();
}
/** /**
* This function is called periodically during autonomous. * This function is called every robot packet, no matter the mode. Use
*/ * this for items like diagnostics that you want ran during disabled,
@Override * autonomous, teleoperated and test.doubl
public void autonomousPeriodic() { *
} * <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
@Override */
public void teleopInit() { @Override
// This makes sure that the autonomous stops running when public void robotPeriodic() {
// teleop starts running. If you want the autonomous to m_robotTime.updateTimes();
// continue until interrupted by another command, remove //System.out.println(m_robotContainer.limelight.isNearSpeaker());
// this line or comment it out. //mled.updateLED();
if (m_autonomousCommand != null) { // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
m_autonomousCommand.cancel(); // commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
} }
m_robotTime.startMatchTime();
}
/** /**
* This function is called periodically during operator control. * This function is called once each time the robot enters Disabled mode.
*/ * You can use it to reset any subsystem information you want to clear when
@Override * the robot is disabled.
public void teleopPeriodic() { */
@Override
public void disabledInit() {
m_robotTime.endMatchTime();
}
@Override
public void disabledPeriodic() {
}
@Override
public void disabledExit() {
DeferredBlock.execute();
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
m_robotTime.startMatchTime();
}
/**
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
}
@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
m_robotTime.startMatchTime();
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
} }
/** /**
* This function is called periodically during test mode. * This function is called periodically during test mode.
*/ */
@Override @Override
public void testPeriodic() { public void testPeriodic() {
} }
} }
src/main/java/frc4388/robot/RobotContainer.java
+158 -158
View File
@@ -86,11 +86,11 @@ public class RobotContainer {
private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup( private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup(
new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.idle()), new InstantCommand(() -> m_robotShooter.idle()),
new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0))) new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0)))
//new InstantCommand(() -> m_robotShooter.spin()) //new InstantCommand(() -> m_robotShooter.spin())
); );
// private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotShooter.spin()), // new InstantCommand(() -> m_robotShooter.spin()),
@@ -109,44 +109,44 @@ public class RobotContainer {
private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
private SequentialCommandGroup autoShoot = new SequentialCommandGroup( private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
// MoveToSpeaker, // MoveToSpeaker,
autoAlign, autoAlign,
new InstantCommand(() -> m_robotShooter.spin()), new InstantCommand(() -> m_robotShooter.spin()),
new WaitCommand(3.0), new WaitCommand(3.0),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(3.0), new WaitCommand(3.0),
new InstantCommand(() -> m_robotShooter.idle()), new InstantCommand(() -> m_robotShooter.idle()),
new InstantCommand(() -> autoAlign.reverse()), new InstantCommand(() -> autoAlign.reverse()),
autoAlign.asProxy() autoAlign.asProxy()
); );
private SequentialCommandGroup i = new SequentialCommandGroup( private SequentialCommandGroup i = new SequentialCommandGroup(
intakeToShootStuff, intakeToShoot, intakeToShootStuff, intakeToShoot,
new InstantCommand(() -> m_robotShooter.idle()) new InstantCommand(() -> m_robotShooter.idle())
); );
private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(0.75), new WaitCommand(0.75),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake)
); );
private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
); );
private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
interrupt, interrupt,
new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
); );
private SequentialCommandGroup ampShoot = new SequentialCommandGroup( private SequentialCommandGroup ampShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.ampPosition()), new InstantCommand(() -> m_robotIntake.ampPosition()),
new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed
); );
// ! /* Autos */ // ! /* Autos */
private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand(); private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand();
@@ -167,114 +167,114 @@ public class RobotContainer {
// ); // );
private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
); );
private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup ( private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
); );
private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup(
startLeftMoveRight.asProxy(), startLeftMoveRight.asProxy(),
ejectToShoot.asProxy(), ejectToShoot.asProxy(),
taxi.asProxy() taxi.asProxy()
); );
private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup( private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup(
startRightMoveLeft.asProxy(), startRightMoveLeft.asProxy(),
ejectToShoot.asProxy(), ejectToShoot.asProxy(),
taxi.asProxy() taxi.asProxy()
); );
private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(), new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5), new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
// new WaitCommand(1).asProxy(), // new WaitCommand(1).asProxy(),
// new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"), // new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
// new WaitCommand(0.5).asProxy(), // new WaitCommand(0.5).asProxy(),
// new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
// new WaitCommand(1).asProxy(), // new WaitCommand(1).asProxy(),
// new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), // new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
// new WaitCommand(1).asProxy(), // new WaitCommand(1).asProxy(),
// new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
// new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
); );
private SequentialCommandGroup stayTwoNoteStartingFromSpeaker = new SequentialCommandGroup( private SequentialCommandGroup stayTwoNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(), new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5), new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
); );
private SequentialCommandGroup threeNoteStartingFromSpeaker = new SequentialCommandGroup( private SequentialCommandGroup threeNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(), new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(), new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5), new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
//? Create Another Parallel Command Group :( //? Create Another Parallel Command Group :(
new InstantCommand(() -> m_robotIntake.talonPIDIn()), new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(), new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5), new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
); );
private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
.addOption("Taxi Auto", taxi.asProxy()) .addOption("Taxi Auto", taxi.asProxy())
.addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy())
.addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy())
// .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy())
// .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy())
.addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy())
.addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy())
.buildDisplay(); .buildDisplay();
@@ -287,19 +287,19 @@ public class RobotContainer {
new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
DriverStation.silenceJoystickConnectionWarning(true); DriverStation.silenceJoystickConnectionWarning(true);
CameraServer.startAutomaticCapture(); CameraServer.startAutomaticCapture();
/* Default Commands */ /* Default Commands */
// drives the robot with a two-axis input from the driver controller // drives the robot with a two-axis input from the driver controller
// ! Swerve Drive Default Command (Regular Rotation) // ! Swerve Drive Default Command (Regular Rotation)
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(), getDeadbandedDriverController().getRight(),
true); true);
}, m_robotSwerveDrive) }, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand")); .withName("SwerveDrive DefaultCommand"));
m_robotSwerveDrive.setToSlow(); m_robotSwerveDrive.setToSlow();
// ! Swerve Drive Default Command (Orientation Rotation) // ! Swerve Drive Default Command (Orientation Rotation)
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
@@ -356,25 +356,25 @@ public class RobotContainer {
// .onFalse(new InstantCommand()); // .onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
() -> autoplaybackName.get())) () -> autoplaybackName.get()))
.onFalse(new InstantCommand()); .onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive, .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
() -> autoplaybackName.get(), () -> autoplaybackName.get(),
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false)) true, false))
.onFalse(new InstantCommand()); .onFalse(new InstantCommand());
// ! /* Speed */ // ! /* Speed */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .whileTrue(new InstantCommand(() -> // .whileTrue(new InstantCommand(() ->
@@ -383,7 +383,7 @@ public class RobotContainer {
// true), m_robotSwerveDrive)); // true), m_robotSwerveDrive));
//? /* Operator Buttons */ //? /* Operator Buttons */
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn()))
@@ -398,7 +398,7 @@ public class RobotContainer {
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(emergencyRetract); .onTrue(emergencyRetract);
// Override Intake Position encoder: out // Override Intake Position encoder: out
@@ -443,7 +443,7 @@ public class RobotContainer {
} }
private void configureVirtualButtonBindings() { private void configureVirtualButtonBindings() {
/* Driver Buttons */ /* Driver Buttons */
// new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) // new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
@@ -462,7 +462,7 @@ public class RobotContainer {
// /* Operator Buttons */ // /* Operator Buttons */
new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor()));
@@ -475,7 +475,7 @@ public class RobotContainer {
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors()));
new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON) new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(emergencyRetract.asProxy()); .onTrue(emergencyRetract.asProxy());
// Override Intake Position encoder: out // Override Intake Position encoder: out
src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java
+12 -12
View File
@@ -16,16 +16,16 @@ import frc4388.robot.subsystems.SwerveDrive;
// information, see: // information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ArmIntakeInAuto extends ParallelCommandGroup { public class ArmIntakeInAuto extends ParallelCommandGroup {
private final Intake intake; private final Intake intake;
private final Shooter shooter; private final Shooter shooter;
private final SwerveDrive swerve; private final SwerveDrive swerve;
/** Creates a new ArmIntakeInAuto. */ /** Creates a new ArmIntakeInAuto. */
public ArmIntakeInAuto(Intake intake, Shooter shooter, SwerveDrive swerve) { public ArmIntakeInAuto(Intake intake, Shooter shooter, SwerveDrive swerve) {
// Add your commands in the addCommands() call, e.g. // Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand()); // addCommands(new FooCommand(), new BarCommand());
this.intake = intake; this.intake = intake;
this.shooter = shooter; this.shooter = shooter;
this.swerve = swerve; this.swerve = swerve;
addCommands((new ArmIntakeInTimeout(intake, shooter).withTimeout(3)), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt"))); addCommands((new ArmIntakeInTimeout(intake, shooter).withTimeout(3)), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt")));
} }
} }
src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java
+41 -41
View File
@@ -10,49 +10,49 @@ import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
public class ArmIntakeInTimeout extends Command { public class ArmIntakeInTimeout extends Command {
/** Creates a new ArmIntakeIn. */ /** Creates a new ArmIntakeIn. */
private Intake robotIntake; private Intake robotIntake;
private Shooter robotShooter; private Shooter robotShooter;
public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) { public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) {
// Use addRequirements() here to declare subsystem dependencies. // Use addRequirements() here to declare subsystem dependencies.
this.robotIntake = robotIntake; this.robotIntake = robotIntake;
this.robotShooter = robotShooter; this.robotShooter = robotShooter;
addRequirements(this.robotIntake, this.robotShooter); addRequirements(this.robotIntake, this.robotShooter);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
robotIntake.talonPIDOut();
robotIntake.talonSpinIntakeMotor();
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
if(interrupted) {
robotIntake.talonPIDIn();
robotIntake.talonStopIntakeMotors();
} }
}
// Returns true when the command should end. // Called when the command is initially scheduled.
@Override @Override
public boolean isFinished() { public void initialize() {}
return robotIntake.getTalonIntakeLimitSwitchState();
// if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) // Called every time the scheduler runs while the command is scheduled.
// { @Override
// return !true==true; public void execute() {
// } robotIntake.talonPIDOut();
// else robotIntake.talonSpinIntakeMotor();
// { }
// return !false==!(!(true));
// } // Called once the command ends or is interrupted.
} @Override
public void end(boolean interrupted) {
if(interrupted) {
robotIntake.talonPIDIn();
robotIntake.talonStopIntakeMotors();
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return robotIntake.getTalonIntakeLimitSwitchState();
// if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0)))
// {
// return !true==true;
// }
// else
// {
// return !false==!(!(true));
// }
}
} }
src/main/java/frc4388/robot/commands/Autos/AutoAlign.java
+15 -15
View File
@@ -118,18 +118,18 @@ public class AutoAlign extends Command {
private Translation2d calcRotStick(){ private Translation2d calcRotStick(){
double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
cumError += error * .02; // 20 ms cumError += error * .02; // 20 ms
double delta = error - prevError; double delta = error - prevError;
final double kP = 4; final double kP = 4;
final double kI = 4; final double kI = 4;
final double kD = 4; final double kD = 4;
final double kF = 4; final double kF = 4;
double output = error * kP; double output = error * kP;
output += cumError * kI; output += cumError * kI;
output += delta * kD; output += delta * kD;
output += kF; output += kF;
prevError = error; prevError = error;
return clamp(output, 0); return clamp(output, 0);
@@ -140,8 +140,8 @@ public class AutoAlign extends Command {
} }
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public final void initialize() { public final void initialize() {
isRed = alliance.get() == DriverStation.Alliance.Red; isRed = alliance.get() == DriverStation.Alliance.Red;
if(reverseAfterFinish){ if(reverseAfterFinish){
// isReverseFinished = false; // isReverseFinished = false;
@@ -150,10 +150,10 @@ public class AutoAlign extends Command {
moveStickReplayArr = new Translation2d[]{}; moveStickReplayArr = new Translation2d[]{};
rotStickReplayArr = new Translation2d[]{}; rotStickReplayArr = new Translation2d[]{};
} }
} }
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
// Update limelight pos // Update limelight pos
pose = limelight.getPose(); pose = limelight.getPose();
@@ -179,7 +179,7 @@ public class AutoAlign extends Command {
// } // }
isFinished = limelight.isNearSpeaker(); isFinished = limelight.isNearSpeaker();
// If reverseAfterFinish, then loop back over and replay in reverse // If reverseAfterFinish, then loop back over and replay in reverse
}else if(reverseAfterFinish && !isReverseFinished){ }else if(reverseAfterFinish && !isReverseFinished){
// Get reverse direction // Get reverse direction
moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1];
@@ -201,8 +201,8 @@ public class AutoAlign extends Command {
} }
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public final boolean isFinished() { public final boolean isFinished() {
return isFinished && (isReverseFinished || !reverseAfterFinish); return isFinished && (isReverseFinished || !reverseAfterFinish);
} }
} }
src/main/java/frc4388/robot/commands/Autos/AutoBalance.java
+23 -23
View File
@@ -17,33 +17,33 @@ import frc4388.utility.RobotGyro;
// information, see: // information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class AutoBalance extends PID { public class AutoBalance extends PID {
RobotGyro gyro; RobotGyro gyro;
Intake intake; Intake intake;
/** Creates a new AutoBalance. */ /** Creates a new AutoBalance. */
public AutoBalance(RobotGyro gyro, Intake intake) { public AutoBalance(RobotGyro gyro, Intake intake) {
super(0.6, 0, 0, 0, 0); super(0.6, 0, 0, 0, 0);
this.gyro = gyro; this.gyro = gyro;
this.intake = intake; this.intake = intake;
// Use addRequirements() here to declare subsystem dependencies. // Use addRequirements() here to declare subsystem dependencies.
// Configure additional PID options by calling `getController` here. // Configure additional PID options by calling `getController` here.
addRequirements(intake); addRequirements(intake);
} }
// Returns true when the command should end. // Returns true when the command should end.
public double getError() { public double getError() {
var pitch = gyro.getRoll(); var pitch = gyro.getRoll();
SmartDashboard.putNumber("pitch", pitch); SmartDashboard.putNumber("pitch", pitch);
return pitch; return pitch;
} }
@Override @Override
public void runWithOutput(double output) { public void runWithOutput(double output) {
double out2 = MathUtil.clamp(output / 40, -59, 0); double out2 = MathUtil.clamp(output / 40, -59, 0);
if (Math.abs(getError()) < 3) out2 = 0; if (Math.abs(getError()) < 3) out2 = 0;
intake.talonPIDPosition(out2); intake.talonPIDPosition(out2);
} }
} }
src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java
+34 -34
View File
@@ -10,44 +10,44 @@ import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
public class ArmIntakeIn extends Command { public class ArmIntakeIn extends Command {
/** Creates a new ArmIntakeIn. */ /** Creates a new ArmIntakeIn. */
private Intake robotIntake; private Intake robotIntake;
private Shooter robotShooter; private Shooter robotShooter;
public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) { public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) {
// Use addRequirements() here to declare subsystem dependencies. // Use addRequirements() here to declare subsystem dependencies.
this.robotIntake = robotIntake; this.robotIntake = robotIntake;
this.robotShooter = robotShooter; this.robotShooter = robotShooter;
addRequirements(this.robotIntake, this.robotShooter); addRequirements(this.robotIntake, this.robotShooter);
} }
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public void initialize() {} public void initialize() {}
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
robotIntake.talonPIDOut(); robotIntake.talonPIDOut();
robotIntake.talonSpinIntakeMotor(); robotIntake.talonSpinIntakeMotor();
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
@Override @Override
public void end(boolean interrupted) {} public void end(boolean interrupted) {}
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { public boolean isFinished() {
return robotIntake.getTalonIntakeLimitSwitchState(); return robotIntake.getTalonIntakeLimitSwitchState();
// if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0)))
// { // {
// return !true==true; // return !true==true;
// } // }
// else // else
// { // {
// return !false==!(!(true)); // return !false==!(!(true));
// } // }
} }
} }
src/main/java/frc4388/robot/commands/PID.java
+39 -39
View File
@@ -8,53 +8,53 @@ import edu.wpi.first.wpilibj2.command.Command;
import frc4388.utility.Gains; import frc4388.utility.Gains;
public abstract class PID extends Command { public abstract class PID extends Command {
protected Gains gains; protected Gains gains;
private double output = 0; private double output = 0;
private double tolerance = 0; private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */ /** Creates a new PelvicInflammatoryDisease. */
public PID(double kp, double ki, double kd, double kf, double tolerance) { public PID(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0); gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance; this.tolerance = tolerance;
} }
public PID(Gains gains, double tolerance) { public PID(Gains gains, double tolerance) {
this.gains = gains; this.gains = gains;
this.tolerance = tolerance; this.tolerance = tolerance;
} }
/** produces the error from the setpoint */ /** produces the error from the setpoint */
public abstract double getError(); public abstract double getError();
/** todo: javadoc */ /** todo: javadoc */
public abstract void runWithOutput(double output); public abstract void runWithOutput(double output);
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public final void initialize() { public final void initialize() {
output = 0; output = 0;
} }
private double prevError, cumError = 0; private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public final void execute() { public final void execute() {
double error = getError(); double error = getError();
cumError += error * .02; // 20 ms cumError += error * .02; // 20 ms
double delta = error - prevError; double delta = error - prevError;
output = error * gains.kP; output = error * gains.kP;
output += cumError * gains.kI; output += cumError * gains.kI;
output += delta * gains.kD; output += delta * gains.kD;
output += gains.kF; output += gains.kF;
runWithOutput(output); runWithOutput(output);
} }
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public final boolean isFinished() { public final boolean isFinished() {
return Math.abs(getError()) < tolerance; return Math.abs(getError()) < tolerance;
} }
} }
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
+105 -105
View File
@@ -14,132 +14,132 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput; import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends CommandBase { public class JoystickPlayback extends CommandBase {
private final SwerveDrive swerve; private final SwerveDrive swerve;
private String filename; private String filename;
private int mult = 1; private int mult = 1;
private Scanner input; private Scanner input;
private final ArrayList<TimedOutput> outputs = new ArrayList<>(); private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0; private int counter = 0;
private long startTime = 0; private long startTime = 0;
private long playbackTime = 0; private long playbackTime = 0;
private int lastIndex; private int lastIndex;
private boolean m_finished = false; // ! find a better way private boolean m_finished = false; // ! find a better way
/** Creates a new JoystickPlayback. */ /** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
this.swerve = swerve; this.swerve = swerve;
this.filename = filename; this.filename = filename;
this.mult = mult; this.mult = mult;
addRequirements(this.swerve); addRequirements(this.swerve);
} }
/** Creates a new JoystickPlayback. */ /** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename) { public JoystickPlayback(SwerveDrive swerve, String filename) {
this(swerve, filename, 1); this(swerve, filename, 1);
} }
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public void initialize() { public void initialize() {
outputs.clear(); outputs.clear();
m_finished = false; m_finished = false;
startTime = System.currentTimeMillis(); startTime = System.currentTimeMillis();
playbackTime = 0; playbackTime = 0;
lastIndex = 0; lastIndex = 0;
try { try {
input = new Scanner(new File("/home/lvuser/autos/" + filename)); input = new Scanner(new File("/home/lvuser/autos/" + filename));
String line = ""; String line = "";
while (input.hasNextLine()) { while (input.hasNextLine()) {
line = input.nextLine(); line = input.nextLine();
if (line.isEmpty() || line.isBlank() || line.equals("\n")) { if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
continue; continue;
} }
String[] values = line.split(","); String[] values = line.split(",");
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
var out = new TimedOutput(); var out = new TimedOutput();
out.leftX = Double.parseDouble(values[0]) * mult; out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]); out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]); out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]); out.rightY = Double.parseDouble(values[3]);
out.timedOffset = Long.parseLong(values[4]); out.timedOffset = Long.parseLong(values[4]);
outputs.add(out); outputs.add(out);
} }
input.close(); input.close();
} catch (FileNotFoundException e) { } catch (FileNotFoundException e) {
e.printStackTrace(); e.printStackTrace();
} }
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
startTime = System.currentTimeMillis();
playbackTime = 0;
} else {
playbackTime = System.currentTimeMillis() - startTime;
} }
// skip to reasonable time frame // Called every time the scheduler runs while the command is scheduled.
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing @Override
{ public void execute() {
int i = lastIndex == 0 ? 1 : lastIndex; if (counter == 0) {
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { startTime = System.currentTimeMillis();
i++; playbackTime = 0;
} } else {
playbackTime = System.currentTimeMillis() - startTime;
}
if (i >= outputs.size()) { // skip to reasonable time frame
m_finished = true; // ! kind of a hack // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
return; {
} int i = lastIndex == 0 ? 1 : lastIndex;
lastIndex = i; while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
} i++;
}
TimedOutput lastOut = outputs.get(lastIndex - 1); if (i >= outputs.size()) {
TimedOutput out = outputs.get(lastIndex); m_finished = true; // ! kind of a hack
return;
}
lastIndex = i;
}
double deltaTime = out.timedOffset - lastOut.timedOffset; TimedOutput lastOut = outputs.get(lastIndex - 1);
double playbackDelta = playbackTime - lastOut.timedOffset; TimedOutput out = outputs.get(lastIndex);
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); double deltaTime = out.timedOffset - lastOut.timedOffset;
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); double playbackDelta = playbackTime - lastOut.timedOffset;
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
// new Translation2d(out.rightX, out.rightY), double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
// true); double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY),
// new Translation2d(out.rightX, out.rightY),
// true);
// this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
// new Translation2d(lerpRX, lerpRY), // new Translation2d(lerpRX, lerpRY),
// true); // true);
this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY), new Translation2d(lerpRX, lerpRY),
true); true);
counter++; counter++;
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.
@Override @Override
public void end(boolean interrupted) { public void end(boolean interrupted) {
input.close(); input.close();
swerve.stopModules(); swerve.stopModules();
} }
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { public boolean isFinished() {
return m_finished; return m_finished;
} }
} }
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+73 -73
View File
@@ -16,82 +16,82 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput; import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickRecorder extends CommandBase { public class JoystickRecorder extends CommandBase {
public final SwerveDrive swerve; public final SwerveDrive swerve;
public final Supplier<Double> leftX; public final Supplier<Double> leftX;
public final Supplier<Double> leftY; public final Supplier<Double> leftY;
public final Supplier<Double> rightX; public final Supplier<Double> rightX;
public final Supplier<Double> rightY; public final Supplier<Double> rightY;
private String filename; private String filename;
public final ArrayList<TimedOutput> outputs = new ArrayList<>(); public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1; private long startTime = -1;
/** Creates a new JoystickRecorder. */ /** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY, public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> rightY, Supplier<Double> rightX, Supplier<Double> rightY,
String filename) String filename)
{ {
this.swerve = swerve; this.swerve = swerve;
this.leftX = leftX; this.leftX = leftX;
this.leftY = leftY; this.leftY = leftY;
this.rightX = rightX; this.rightX = rightX;
this.rightY = rightY; this.rightY = rightY;
this.filename = filename; this.filename = filename;
addRequirements(this.swerve); addRequirements(this.swerve);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
this.startTime = System.currentTimeMillis();
outputs.add(new TimedOutput());
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
var inputs = new TimedOutput();
inputs.leftX = leftX.get();
inputs.leftY = leftY.get();
inputs.rightX = rightX.get();
inputs.rightY = rightY.get();
inputs.timedOffset = System.currentTimeMillis() - startTime;
outputs.add(inputs);
swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
System.out.println("RECORDING");
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
File output = new File("/home/lvuser/autos/" + filename);
try (PrintWriter writer = new PrintWriter(output)) {
for (var input : outputs) {
writer.println( input.leftX + "," + input.leftY + "," +
input.rightX + "," + input.rightY + "," +
input.timedOffset);
}
writer.close();
} catch (IOException e) {
e.printStackTrace();
} }
}
// Returns true when the command should end. // Called when the command is initially scheduled.
@Override @Override
public boolean isFinished() { public void initialize() {
return false; outputs.clear();
}
} this.startTime = System.currentTimeMillis();
outputs.add(new TimedOutput());
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
var inputs = new TimedOutput();
inputs.leftX = leftX.get();
inputs.leftY = leftY.get();
inputs.rightX = rightX.get();
inputs.rightY = rightY.get();
inputs.timedOffset = System.currentTimeMillis() - startTime;
outputs.add(inputs);
swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
System.out.println("RECORDING");
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
File output = new File("/home/lvuser/autos/" + filename);
try (PrintWriter writer = new PrintWriter(output)) {
for (var input : outputs) {
writer.println( input.leftX + "," + input.leftY + "," +
input.rightX + "," + input.rightY + "," +
input.timedOffset);
}
writer.close();
} catch (IOException e) {
e.printStackTrace();
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
+17 -17
View File
@@ -10,26 +10,26 @@ import frc4388.robot.subsystems.SwerveDrive;
public class RotateToAngle extends PID { public class RotateToAngle extends PID {
SwerveDrive drive; SwerveDrive drive;
double targetAngle; double targetAngle;
/** Creates a new RotateToAngle. */ /** Creates a new RotateToAngle. */
public RotateToAngle(SwerveDrive drive, double targetAngle) { public RotateToAngle(SwerveDrive drive, double targetAngle) {
super(0.3, 0.0, 0.0, 0.0, 1); super(0.3, 0.0, 0.0, 0.0, 1);
this.drive = drive; this.drive = drive;
this.targetAngle = targetAngle; this.targetAngle = targetAngle;
addRequirements(drive); addRequirements(drive);
} }
@Override @Override
public double getError() { public double getError() {
return targetAngle - drive.getGyroAngle(); return targetAngle - drive.getGyroAngle();
} }
@Override @Override
public void runWithOutput(double output) { public void runWithOutput(double output) {
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
} }
} }
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
+4 -4
View File
@@ -65,7 +65,7 @@ public class neoJoystickPlayback extends Command {
if (m_numControllers > controllers.length) { if (m_numControllers > controllers.length) {
System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers
+ " virtual controllers but only " + controllers.length + " were given"); + " virtual controllers but only " + controllers.length + " were given");
return false; return false;
} }
@@ -133,9 +133,9 @@ public class neoJoystickPlayback extends Command {
controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
if (i == 0) { if (i == 0) {
this.swerve.driveWithInput( this.swerve.driveWithInput(
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
true); true);
} }
} }
frame_index++; frame_index++;
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
+5 -5
View File
@@ -55,8 +55,8 @@ public class neoJoystickRecorder extends Command {
XboxController controller = controllers[i]; XboxController controller = controllers[i];
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
double[] axes = {controller.getLeftX(), controller.getLeftY(), double[] axes = {controller.getLeftX(), controller.getLeftY(),
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
controller.getRightX(), controller.getRightY()}; controller.getRightX(), controller.getRightY()};
short button = 0; short button = 0;
for (int j = 0; j < 10; j++) for (int j = 0; j < 10; j++)
if (controller.getRawButton(j+1)) if (controller.getRawButton(j+1))
@@ -71,8 +71,8 @@ public class neoJoystickRecorder extends Command {
frames.add(frame); frames.add(frame);
swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]),
true); // Really jank way of doing this. true); // Really jank way of doing this.
} }
@Override @Override
@@ -105,7 +105,7 @@ public class neoJoystickRecorder extends Command {
} }
stream.write(DataUtils.intToByteArray(frame.timeStamp)); stream.write(DataUtils.intToByteArray(frame.timeStamp));
} }
System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
} catch (Exception e) { } catch (Exception e) {
e.printStackTrace(); e.printStackTrace();
} }
src/main/java/frc4388/robot/subsystems/Climber.java
+28 -28
View File
@@ -15,38 +15,38 @@ import frc4388.robot.Constants.ClimbConstants;
//! 6.5C Scoring Criteria for Stage //! 6.5C Scoring Criteria for Stage
public class Climber extends SubsystemBase { public class Climber extends SubsystemBase {
/** Creates a new Climber. */ /** Creates a new Climber. */
TalonFX climbMotor; TalonFX climbMotor;
public Climber(TalonFX climbMotor) { public Climber(TalonFX climbMotor) {
this.climbMotor = climbMotor; this.climbMotor = climbMotor;
this.climbMotor.setInverted(true); this.climbMotor.setInverted(true);
var slot0Configs = new Slot0Configs(); var slot0Configs = new Slot0Configs();
slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
slot0Configs.kI = 0.0; // no output for integrated error slot0Configs.kI = 0.0; // no output for integrated error
slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output
climbMotor.getConfigurator().apply(slot0Configs); climbMotor.getConfigurator().apply(slot0Configs);
} }
public void climbOut() { public void climbOut() {
PositionVoltage request = new PositionVoltage(0); PositionVoltage request = new PositionVoltage(0);
climbMotor.setControl(request.withPosition(-520)); climbMotor.setControl(request.withPosition(-520));
//climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); //climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
} }
public void climbIn() { public void climbIn() {
PositionVoltage request = new PositionVoltage(-520); PositionVoltage request = new PositionVoltage(-520);
climbMotor.setControl(request.withPosition(0)); climbMotor.setControl(request.withPosition(0));
// climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); // climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
} }
public void stopClimb() { public void stopClimb() {
climbMotor.set(0.d); climbMotor.set(0.d);
} }
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run // This method will be called once per scheduler run
} }
} }
src/main/java/frc4388/robot/subsystems/Intake.java
+239 -239
View File
@@ -38,303 +38,303 @@ import frc4388.utility.configurable.ConfigurableDouble;
public class Intake extends SubsystemBase { public class Intake extends SubsystemBase {
//NEO //NEO
private CANSparkMax intakeMotor; private CANSparkMax intakeMotor;
private CANSparkMax pivot; private CANSparkMax pivot;
private SparkPIDController m_spedController; private SparkPIDController m_spedController;
private SparkLimitSwitch forwardLimit; private SparkLimitSwitch forwardLimit;
private SparkLimitSwitch reverseLimit; private SparkLimitSwitch reverseLimit;
private SparkLimitSwitch intakeforwardLimit; private SparkLimitSwitch intakeforwardLimit;
private SparkLimitSwitch intakereverseLimit; private SparkLimitSwitch intakereverseLimit;
//Talon //Talon
private TalonFX talonIntake; private TalonFX talonIntake;
private TalonFX talonPivot; private TalonFX talonPivot;
private CANcoder encoder; private CANcoder encoder;
private boolean r; private boolean r;
private HardwareLimitSwitchConfigs l; private HardwareLimitSwitchConfigs l;
TalonFXConfiguration doodooController = new TalonFXConfiguration(); TalonFXConfiguration doodooController = new TalonFXConfiguration();
public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
private ConfigurableDouble outtakeSpeed = new ConfigurableDouble("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); private ConfigurableDouble outtakeSpeed = new ConfigurableDouble("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
private BooleanSupplier sup = () -> true; private BooleanSupplier sup = () -> true;
private BooleanSupplier dup = () -> false; private BooleanSupplier dup = () -> false;
private double smartDashboardOuttakeValue; private double smartDashboardOuttakeValue;
/** Creates a new Intake. */ /** Creates a new Intake. */
//For NEO //For NEO
// public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { // public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
// this.intakeMotor = intakeMotor; // this.intakeMotor = intakeMotor;
// this.pivot = pivot; // this.pivot = pivot;
// pivot.restoreFactoryDefaults(); // pivot.restoreFactoryDefaults();
// //pivot.setInverted(true); // //pivot.setInverted(true);
// forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// forwardLimit.enableLimitSwitch(true); // forwardLimit.enableLimitSwitch(true);
// reverseLimit.enableLimitSwitch(true); // reverseLimit.enableLimitSwitch(true);
// intakeMotor.restoreFactoryDefaults(); // intakeMotor.restoreFactoryDefaults();
// intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// intakeforwardLimit.enableLimitSwitch(true); // intakeforwardLimit.enableLimitSwitch(true);
// intakereverseLimit.enableLimitSwitch(false); // intakereverseLimit.enableLimitSwitch(false);
// //Arm PID // //Arm PID
// m_spedController = pivot.getPIDController(); // m_spedController = pivot.getPIDController();
// m_spedController.setP(armGains.kP); // m_spedController.setP(armGains.kP);
// m_spedController.setI(armGains.kI); // m_spedController.setI(armGains.kI);
// m_spedController.setD(armGains.kD); // m_spedController.setD(armGains.kD);
// SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); // SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
// } // }
//For Talon //For Talon
public Intake(TalonFX talonIntake, TalonFX talonPivot) { public Intake(TalonFX talonIntake, TalonFX talonPivot) {
this.talonIntake = talonIntake; this.talonIntake = talonIntake;
this.talonPivot = talonPivot; this.talonPivot = talonPivot;
talonIntake.getConfigurator().apply(new TalonFXConfiguration()); talonIntake.getConfigurator().apply(new TalonFXConfiguration());
talonPivot.getConfigurator().apply(new TalonFXConfiguration()); talonPivot.getConfigurator().apply(new TalonFXConfiguration());
talonIntake.setNeutralMode(NeutralModeValue.Brake); talonIntake.setNeutralMode(NeutralModeValue.Brake);
talonPivot.setNeutralMode(NeutralModeValue.Brake); talonPivot.setNeutralMode(NeutralModeValue.Brake);
// talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); // talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs());
// talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs()); // talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs());
// doodooController.Slot0.kP = armGains.kP; // doodooController.Slot0.kP = armGains.kP;
// doodooController.Slot1.kI = armGains.kI; // doodooController.Slot1.kI = armGains.kI;
// doodooController.Slot2.kD = armGains.kD; // doodooController.Slot2.kD = armGains.kD;
// in init function, set slot 0 gains // in init function, set slot 0 gains
var slot0Configs = new Slot0Configs(); var slot0Configs = new Slot0Configs();
slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
slot0Configs.kI = 0.0; // no output for integrated error slot0Configs.kI = 0.0; // no output for integrated error
slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output
talonPivot.getConfigurator().apply(slot0Configs); talonPivot.getConfigurator().apply(slot0Configs);
}
// ! Talon Methods
public void talonPIDIn() {
PositionVoltage request = new PositionVoltage(-59);
talonPivot.setControl(request.withPosition(0));
}
public void talonPIDOut() {
PositionVoltage request = new PositionVoltage(0);
talonPivot.setControl(request.withPosition(-59));
}
public void talonPIDPosition(double out2) {
PositionVoltage request = new PositionVoltage(out2);
talonPivot.setControl(request);
}
public void talonHandoff() {
talonIntake.set(-outtakeSpeed.get());
}
public void talonSpinIntakeMotor() {
talonIntake.set(IntakeConstants.INTAKE_SPEED);
}
public void talonSpinIntakeMotor(double speed) {
talonIntake.set(speed);
}
public boolean getTalonIntakeLimitSwitchState() {
if(r = talonIntake.getForwardLimit().getValue().value == 0) {
return true;
} }
return false;
}
public void talonSetPivotEncoderPosition(int val) { // ! Talon Methods
talonPivot.setPosition(val); public void talonPIDIn() {
} PositionVoltage request = new PositionVoltage(-59);
talonPivot.setControl(request.withPosition(0));
public void talonStopIntakeMotors() {
talonIntake.set(0);
}
public void talonStopArmMotor() {
talonPivot.set(0);
}
public double getArmPos() {
return talonPivot.getPosition().getValue();
}
public void resetArmPosition() {
if(getTalonIntakeLimitSwitchState()){
// talonPivot.setPosition(0);
} }
}
public void ampPosition() { public void talonPIDOut() {
PositionVoltage request = new PositionVoltage(-0); PositionVoltage request = new PositionVoltage(0);
talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value talonPivot.setControl(request.withPosition(-59));
} }
public void ampShoot(double speed) { public void talonPIDPosition(double out2) {
talonSpinIntakeMotor(speed); PositionVoltage request = new PositionVoltage(out2);
} talonPivot.setControl(request);
}
public void talonHandoff() {
talonIntake.set(-outtakeSpeed.get());
}
// ! NEO Methods public void talonSpinIntakeMotor() {
//hanoff talonIntake.set(IntakeConstants.INTAKE_SPEED);
// public void spinIntakeMotor() { }
// intakeMotor.set(IntakeConstants.INTAKE_SPEED);
// }
// //Rotate robot in for handoff public void talonSpinIntakeMotor(double speed) {
// public void rotateArmIn() { talonIntake.set(speed);
// pivot.set(IntakeConstants.PIVOT_SPEED); }
// }
// //Rotates robot out for intake public boolean getTalonIntakeLimitSwitchState() {
// public void rotateArmOut() { if(r = talonIntake.getForwardLimit().getValue().value == 0) {
// pivot.set(-IntakeConstants.PIVOT_SPEED); return true;
}
return false;
}
// } public void talonSetPivotEncoderPosition(int val) {
talonPivot.setPosition(val);
// public void pidIn() { }
// m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition);
// //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
// }
// public void pidOut() { public void talonStopIntakeMotors() {
// m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); talonIntake.set(0);
// } }
// public void limitNote() { public void talonStopArmMotor() {
// if (intakeforwardLimit.isPressed()) { talonPivot.set(0);
// rotateArmIn2(); }
// } else {
// spinIntakeMotor();
// }
// }
// public void rotateArmOut2() { public double getArmPos() {
// if(reverseLimit.isPressed()){ return talonPivot.getPosition().getValue();
// stopArmMotor(); }
// } else {
// pidOut();
// }
// }
// public void rotateArmIn2() { public void resetArmPosition() {
// if(forwardLimit.isPressed()){ if(getTalonIntakeLimitSwitchState()){
// stopArmMotor(); // talonPivot.setPosition(0);
// } else { }
// pidIn(); }
// }
// } public void ampPosition() {
PositionVoltage request = new PositionVoltage(-0);
talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value
}
public void ampShoot(double speed) {
talonSpinIntakeMotor(speed);
}
// ! NEO Methods
//hanoff
// public void spinIntakeMotor() {
// intakeMotor.set(IntakeConstants.INTAKE_SPEED);
// }
// //Rotate robot in for handoff
// public void rotateArmIn() {
// pivot.set(IntakeConstants.PIVOT_SPEED);
// }
// //Rotates robot out for intake
// public void rotateArmOut() {
// pivot.set(-IntakeConstants.PIVOT_SPEED);
// }
// public void pidIn() {
// m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition);
// //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
// }
// public void handoff() { // public void pidOut() {
// intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); // m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
// } // }
// public void handoff2() { // public void limitNote() {
// if(intakeforwardLimit.isPressed()) { // if (intakeforwardLimit.isPressed()) {
// intakeMotor.set(-smartDashboardOuttakeValue); // rotateArmIn2();
// } else { // } else {
// intakeMotor.set(-smartDashboardOuttakeValue); // spinIntakeMotor();
// } // }
// } // }
// public void stopIntakeMotors() { // public void rotateArmOut2() {
// intakeMotor.set(0); // if(reverseLimit.isPressed()){
// } // stopArmMotor();
// } else {
// pidOut();
// }
// }
// public void stopArmMotor() { // public void rotateArmIn2() {
// pivot.set(0); // if(forwardLimit.isPressed()){
// } // stopArmMotor();
// } else {
// pidIn();
// }
// }
// public void handoff() {
// intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
// }
// public RelativeEncoder getEncoder() { // public void handoff2() {
// return pivot.getEncoder(); // if(intakeforwardLimit.isPressed()) {
// } // intakeMotor.set(-smartDashboardOuttakeValue);
// } else {
// intakeMotor.set(-smartDashboardOuttakeValue);
// }
// }
// public boolean getForwardLimitSwitchState() { // public void stopIntakeMotors() {
// return forwardLimit.isPressed(); // intakeMotor.set(0);
// } // }
// public boolean getReverseLimitSwitchState() { // public void stopArmMotor() {
// return reverseLimit.isPressed(); // pivot.set(0);
// } // }
// public boolean getIntakeLimitSwtichState() { // public RelativeEncoder getEncoder() {
// return intakeforwardLimit.isPressed(); // return pivot.getEncoder();
// } // }
// public void setVoltage(double voltage) { // public boolean getForwardLimitSwitchState() {
// pivot.setVoltage(voltage); // return forwardLimit.isPressed();
// } // }
// public double getVelocity() { // public boolean getReverseLimitSwitchState() {
// return pivot.getEncoder().getVelocity(); // return reverseLimit.isPressed();
// } // }
// public void setPivotEncoderPosition(int val) { // public boolean getIntakeLimitSwtichState() {
// pivot.getEncoder().setPosition(val); // return intakeforwardLimit.isPressed();
// } // }
// public void resetPosition() { // public void setVoltage(double voltage) {
// if(forwardLimit.isPressed()) { // pivot.setVoltage(voltage);
// setPivotEncoderPosition(0); // }
// }
// }
// public double getPos() { // public double getVelocity() {
// return pivot.getEncoder().getPosition(); // return pivot.getEncoder().getVelocity();
// } // }
// public double getIntakeVelocity() { // public void setPivotEncoderPosition(int val) {
// return intakeMotor.getEncoder().getVelocity(); // pivot.getEncoder().setPosition(val);
// } // }
// public void rotateArm() { // public void resetPosition() {
// if(forwardLimit.isPressed()) {
// setPivotEncoderPosition(0);
// }
// }
// } // public double getPos() {
// return pivot.getEncoder().getPosition();
// }
// public BooleanSupplier getArmFowardLimitState() { // public double getIntakeVelocity() {
// if(forwardLimit.isPressed()) { // return intakeMotor.getEncoder().getVelocity();
// return sup; // }
// } else {
// return dup;
// }
// }
// public void changeIntakeNeutralState() { // public void rotateArm() {
// if(forwardLimit.isPressed()) {
// intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
// }
// }
@Override // }
public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Vel Output", getVelocity());
// SmartDashboard.putNumber("Position", getPos());
// resetPosition();
// changeIntakeNeutralState();
resetArmPosition(); // public BooleanSupplier getArmFowardLimitState() {
// if(forwardLimit.isPressed()) {
// return sup;
// } else {
// return dup;
// }
// }
SmartDashboard.putNumber("Pivot Position", getArmPos()); // public void changeIntakeNeutralState() {
// if(forwardLimit.isPressed()) {
// intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
// }
// }
smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); @Override
} public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Vel Output", getVelocity());
// SmartDashboard.putNumber("Position", getPos());
// resetPosition();
// changeIntakeNeutralState();
resetArmPosition();
SmartDashboard.putNumber("Pivot Position", getArmPos());
smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
} }
src/main/java/frc4388/robot/subsystems/LED.java
+64 -64
View File
@@ -22,69 +22,69 @@ import frc4388.utility.LEDPatterns;
*/ */
public class LED extends SubsystemBase { public class LED extends SubsystemBase {
static AddressableLED m_led; static AddressableLED m_led;
static AddressableLEDBuffer m_ledBuffer; static AddressableLEDBuffer m_ledBuffer;
static LED m_self; static LED m_self;
/** /**
* Add your docs here. * Add your docs here.
*/ */
public LED(){ public LED(){
if(m_self != null) if(m_self != null)
return; return;
m_led = new AddressableLED(9); m_led = new AddressableLED(9);
m_ledBuffer = new AddressableLEDBuffer(10); m_ledBuffer = new AddressableLEDBuffer(10);
m_led.setLength(m_ledBuffer.getLength()); m_led.setLength(m_ledBuffer.getLength());
m_led.setData(m_ledBuffer); m_led.setData(m_ledBuffer);
m_led.start(); m_led.start();
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
} }
public static LED getInstance() { public static LED getInstance() {
if(m_self == null) if(m_self == null)
m_self = new LED(); m_self = new LED();
return m_self; return m_self;
} }
@Override @Override
public void periodic(){ public void periodic(){
//gamermode(); //gamermode();
//SmartDashboard.putNumber("LED", m_currentPattern.getValue()); //SmartDashboard.putNumber("LED", m_currentPattern.getValue());
return; return;
} }
static int firstcolor = 0; static int firstcolor = 0;
static void gamermode() { static void gamermode() {
for(int i = 0; i < m_ledBuffer.getLength(); i++) { for(int i = 0; i < m_ledBuffer.getLength(); i++) {
final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
setLEDHSV(i, hue, 255, 128); setLEDHSV(i, hue, 255, 128);
} }
firstcolor +=3; firstcolor +=3;
firstcolor %= 180; firstcolor %= 180;
} }
/** /**
* Add your docs here. * Add your docs here.
*/ */
public static void updateLED (){ public static void updateLED (){
gamermode(); gamermode();
// m_LEDController.set(m_currentPattern.getValue()); // m_LEDController.set(m_currentPattern.getValue());
} }
/** /**
* Add your docs here. * Add your docs here.
*/ */
public static void setLEDRGB(int lednum, int r, int g, int b){ public static void setLEDRGB(int lednum, int r, int g, int b){
m_ledBuffer.setRGB(lednum, r, g, b); m_ledBuffer.setRGB(lednum, r, g, b);
//m_currentPattern = pattern; //m_currentPattern = pattern;
// m_LEDController.set(m_currentPattern.getValue()); // m_LEDController.set(m_currentPattern.getValue());
} }
public static void setLEDHSV(int lednum, int hue, int sat, int val){ public static void setLEDHSV(int lednum, int hue, int sat, int val){
m_ledBuffer.setRGB(lednum, hue, sat, val); m_ledBuffer.setRGB(lednum, hue, sat, val);
//m_currentPattern = pattern; //m_currentPattern = pattern;
// m_LEDController.set(m_currentPattern.getValue()); // m_LEDController.set(m_currentPattern.getValue());
} }
/** /**
* Add your docs here. * Add your docs here.
* @return * @return
*/ */
public AddressableLEDBuffer getBuffer() { public AddressableLEDBuffer getBuffer() {
return m_ledBuffer; return m_ledBuffer;
} }
} }
src/main/java/frc4388/robot/subsystems/Limelight.java
+50 -50
View File
@@ -18,65 +18,65 @@ import frc4388.robot.Constants.VisionConstants;
public class Limelight extends SubsystemBase { public class Limelight extends SubsystemBase {
// [X, Y, Z, Roll, Pitch, Yaw] // [X, Y, Z, Roll, Pitch, Yaw]
private double[] cameraPose; private double[] cameraPose;
private boolean isTag; private boolean isTag;
private Pose2d pose; private Pose2d pose;
private boolean isNearSpeaker; private boolean isNearSpeaker;
public boolean getIsTag() { public boolean getIsTag() {
return isTag; return isTag;
}
private void update() {
SmartDashboard.putBoolean("Apriltag", isTag);
if(!isTag){
return;
} }
double x = cameraPose[0]; private void update() {
double y = cameraPose[1]; SmartDashboard.putBoolean("Apriltag", isTag);
double yaw = cameraPose[5]; if(!isTag){
return;
}
Rotation2d rot = Rotation2d.fromDegrees(yaw); double x = cameraPose[0];
double y = cameraPose[1];
double yaw = cameraPose[5];
pose = new Pose2d(x, y, rot); Rotation2d rot = Rotation2d.fromDegrees(yaw);
boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; pose = new Pose2d(x, y, rot);
boolean isRed = DriverStation.getAlliance().get() == Alliance.Red;
double distance; double distance;
if(isRed){ if(isRed){
distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter);
}else{ }else{
distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter);
} }
isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
SmartDashboard.putNumber("speakerDistance", distance); SmartDashboard.putNumber("speakerDistance", distance);
}
public Pose2d getPose() {
return pose;
}
public boolean isNearSpeaker() {
return isNearSpeaker;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
if(newPose != cameraPose){
cameraPose = newPose;
update();
} }
}
} public Pose2d getPose() {
return pose;
}
public boolean isNearSpeaker() {
return isNearSpeaker;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
if(newPose != cameraPose){
cameraPose = newPose;
update();
}
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+82 -82
View File
@@ -21,94 +21,94 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
public class Shooter extends SubsystemBase { public class Shooter extends SubsystemBase {
private TalonFX leftShooter; private TalonFX leftShooter;
private TalonFX rightShooter; private TalonFX rightShooter;
private Limelight limelight; private Limelight limelight;
private int spinMode = 0; private int spinMode = 0;
// 0 = Stop / Coast // 0 = Stop / Coast
// 1 = Idle // 1 = Idle
// 2 = Idle Near Speaker // 2 = Idle Near Speaker
// 3 = Spin // 3 = Spin
// 4 = SingleSpin // 4 = SingleSpin
private double smartDashboardShooterSpeed; private double smartDashboardShooterSpeed;
/** Creates a new Shooter. */ /** Creates a new Shooter. */
public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) { public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) {
leftShooter = leftTalonFX; leftShooter = leftTalonFX;
rightShooter = rightTalonFX; rightShooter = rightTalonFX;
limelight = tmplimelight; limelight = tmplimelight;
leftShooter.setNeutralMode(NeutralModeValue.Coast); leftShooter.setNeutralMode(NeutralModeValue.Coast);
rightShooter.setNeutralMode(NeutralModeValue.Coast); rightShooter.setNeutralMode(NeutralModeValue.Coast);
SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
}
public Shooter(TalonFX leftShooter) {
this.leftShooter = leftShooter;
leftShooter.setNeutralMode(NeutralModeValue.Coast);
}
public void singleSpin() {
leftShooter.set(1.0);
spinMode = 4;
}
public void singleSpin(double speed) {
leftShooter.set(speed);
spinMode = 4;
}
public void spin() {
spin(smartDashboardShooterSpeed);
spinMode = 3;
}
public void spin(double speed) {
leftShooter.set(-speed);
rightShooter.set(-speed);
spinMode = 3;
}
public void spin(double leftSpeed, double rightSpeed) {
leftShooter.set(leftSpeed);
rightShooter.set(-rightSpeed);
spinMode = 3;
}
public void stop() {
spin(0.d);
spinMode = 0;
}
public void idle() {
spin(ShooterConstants.SHOOTER_IDLE);
spinMode = 1;
}
public int getMode(){
return spinMode;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
//SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
// If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed.
// Else if the robot is not near the speaker, then set the speed back to idle.
if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){
leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
spinMode = 2;
}else if(!limelight.isNearSpeaker() && spinMode == 2){
idle();
} }
}
public Shooter(TalonFX leftShooter) {
this.leftShooter = leftShooter;
leftShooter.setNeutralMode(NeutralModeValue.Coast);
}
public void singleSpin() {
leftShooter.set(1.0);
spinMode = 4;
}
public void singleSpin(double speed) {
leftShooter.set(speed);
spinMode = 4;
}
public void spin() {
spin(smartDashboardShooterSpeed);
spinMode = 3;
}
public void spin(double speed) {
leftShooter.set(-speed);
rightShooter.set(-speed);
spinMode = 3;
}
public void spin(double leftSpeed, double rightSpeed) {
leftShooter.set(leftSpeed);
rightShooter.set(-rightSpeed);
spinMode = 3;
}
public void stop() {
spin(0.d);
spinMode = 0;
}
public void idle() {
spin(ShooterConstants.SHOOTER_IDLE);
spinMode = 1;
}
public int getMode(){
return spinMode;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
//SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
// If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed.
// Else if the robot is not near the speaker, then set the speed back to idle.
if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){
leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
spinMode = 2;
}else if(!limelight.isNearSpeaker() && spinMode == 2){
idle();
}
}
} }
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+209 -209
View File
@@ -17,267 +17,267 @@ import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase { public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront; private SwerveModule leftFront;
private SwerveModule rightFront; private SwerveModule rightFront;
private SwerveModule leftBack; private SwerveModule leftBack;
private SwerveModule rightBack; private SwerveModule rightBack;
private SwerveModule[] modules; private SwerveModule[] modules;
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private RobotGyro gyro; private RobotGyro gyro;
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public double rotTarget = 0.0; public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d(); public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */ /** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
this.leftFront = leftFront; this.leftFront = leftFront;
this.rightFront = rightFront; this.rightFront = rightFront;
this.leftBack = leftBack; this.leftBack = leftBack;
this.rightBack = rightBack; this.rightBack = rightBack;
this.gyro = gyro; this.gyro = gyro;
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
// SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
} }
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { boolean stopped = false;
if (fieldRelative) { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0; double rot = 0;
// ! drift correction // ! drift correction
if (rightStick.getNorm() > 0.05) { if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle(); rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
// SmartDashboard.putBoolean("drift correction", false); // SmartDashboard.putBoolean("drift correction", false);
stopped = false; stopped = false;
} else if(leftStick.getNorm() > 0.05) { } else if(leftStick.getNorm() > 0.05) {
if (!stopped) { if (!stopped) {
stopModules(); stopModules();
stopped = true; stopped = true;
} }
// SmartDashboard.putBoolean("drift correction", true); // SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
} }
// Use the left joystick to set speed. Apply a cubic curve and the set max speed. // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds. // Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds. } else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
} }
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
Translation2d rightStick = new Translation2d(-rightX, rightY); double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
if(fieldRelative) { // SmartDashboard.putBoolean("drift correction", true);
double rot = 0;
if(rightStick.getNorm() > 0.5) {
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
double min = tmp.getDegrees();
min = Math.max(Math.abs(min), 2); }
if(tmp.getDegrees() < 0)
min*=-1; // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
tmp = new Rotation2d(min * Math.PI / 180); Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
rot = tmp.getRadians(); // x x - y/x // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
}
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
Translation2d rightStick = new Translation2d(-rightX, rightY);
if(fieldRelative) {
double rot = 0;
if(rightStick.getNorm() > 0.5) {
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5));
double min = tmp.getDegrees();
min = Math.max(Math.abs(min), 2);
if(tmp.getDegrees() < 0)
min*=-1;
tmp = new Rotation2d(min * Math.PI / 180);
rot = tmp.getRadians(); // x x - y/x
}
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1); chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1);
} else { // Create robot-relative speeds. } else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
} }
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
} }
/** /**
* Set each module of the swerve drive to the corresponding desired state. * Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set. * @param desiredStates Array of module states to set.
*/ */
public void setModuleStates(SwerveModuleState[] desiredStates) { public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) { for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i]; SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i]; SwerveModuleState state = desiredStates[i];
module.setDesiredState(state); module.setDesiredState(state);
} }
}
public boolean rotateToTarget(double angle) {
double currentAngle = getGyroAngle();
double error = angle - currentAngle;
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
if (Math.abs(angle - getGyroAngle()) < 5.0) {
return true;
} }
return false; public boolean rotateToTarget(double angle) {
} double currentAngle = getGyroAngle();
double error = angle - currentAngle;
public double getGyroAngle() { driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
return gyro.getAngle();
}
public void resetGyro() { if (Math.abs(angle - getGyroAngle()) < 5.0) {
gyro.reset(); return true;
rotTarget = 0.0; }
}
public void resetGyroFlip() { return false;
gyro.resetFlip(); }
rotTarget = 0.0;
}
public void resetGyroRightBlue() { public double getGyroAngle() {
gyro.resetRightSideBlue(); return gyro.getAngle();
rotTarget = 0.0; }
}
public void resetGyroRightAmp() { public void resetGyro() {
gyro.resetAmpSide(); gyro.reset();
rotTarget = 0.0; rotTarget = 0.0;
} }
public void resetGyroFlip() {
gyro.resetFlip();
rotTarget = 0.0;
}
public void resetGyroRightBlue() {
gyro.resetRightSideBlue();
rotTarget = 0.0;
}
public void resetGyroRightAmp() {
gyro.resetAmpSide();
rotTarget = 0.0;
}
public void stopModules() { public void stopModules() {
for (SwerveModule module : this.modules) { for (SwerveModule module : this.modules) {
module.stop(); module.stop();
}
} }
}
public SwerveDriveKinematics getKinematics() { public SwerveDriveKinematics getKinematics() {
return this.kinematics; return this.kinematics;
} }
public boolean getSpeedState() { public boolean getSpeedState() {
return false; return false;
} }
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run\ // This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle()); SmartDashboard.putNumber("Gyro", getGyroAngle());
} }
public void shiftDown() { public void shiftDown() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else { } else {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED; this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
}
} }
}
public void setToSlow() { public void setToSlow() {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
System.out.println("SLOW"); System.out.println("SLOW");
System.out.println("SLOW"); System.out.println("SLOW");
System.out.println("SLOW"); System.out.println("SLOW");
System.out.println("SLOW"); System.out.println("SLOW");
System.out.println("SLOW"); System.out.println("SLOW");
} }
public void setToFast() { public void setToFast() {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED; this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
System.out.println("FAST"); System.out.println("FAST");
System.out.println("FAST"); System.out.println("FAST");
System.out.println("FAST"); System.out.println("FAST");
System.out.println("FAST"); System.out.println("FAST");
System.out.println("FAST"); System.out.println("FAST");
} }
public void setToTurbo() { public void setToTurbo() {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
System.out.println("TURBO"); System.out.println("TURBO");
System.out.println("TURBO"); System.out.println("TURBO");
System.out.println("TURBO"); System.out.println("TURBO");
System.out.println("TURBO"); System.out.println("TURBO");
System.out.println("TURBO"); System.out.println("TURBO");
} }
public void shiftUp() { public void shiftUp() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED; this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
} else { } else {
}
} }
}
public void toggleGear(double angle) { public void toggleGear(double angle) {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
} else { } else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
}
} }
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+3 -3
View File
@@ -109,9 +109,9 @@ public class SwerveModule extends SubsystemBase {
*/ */
public SwerveModuleState getState() { public SwerveModuleState getState() {
return new SwerveModuleState( return new SwerveModuleState(
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
getAngle() getAngle()
); );
} }
/** /**
src/main/java/frc4388/utility/LEDPatterns.java
+19 -19
View File
@@ -4,8 +4,8 @@ package frc4388.utility;
* Add your docs here. * Add your docs here.
*/ */
public enum LEDPatterns { public enum LEDPatterns {
/* PALLETTE PATTERNS */ /* PALLETTE PATTERNS */
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f), RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f), PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f), PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
@@ -15,31 +15,31 @@ public enum LEDPatterns {
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f), BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f), GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
/* COLOR 1 PATTERNS */ /* COLOR 1 PATTERNS */
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f), C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
/* COLOR 2 PATTERNS */ /* COLOR 2 PATTERNS */
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f), C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
/* COLOR 1 AND 2 PATTERNS */ /* COLOR 1 AND 2 PATTERNS */
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f), C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
/* SOLID COLORS */ /* SOLID COLORS */
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f), SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f), SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f), SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f); SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
/* GETTERS/SETTERS */ /* GETTERS/SETTERS */
private final float id; private final float id;
LEDPatterns(float id) { LEDPatterns(float id) {
this.id = id; this.id = id;
} }
public float getValue() { public float getValue() {
return id; return id;
} }
} }
src/main/java/frc4388/utility/RobotGyro.java
+7 -7
View File
@@ -162,12 +162,12 @@ public class RobotGyro implements Gyro {
} }
/** /**
* Get Yaw, Pitch, and Roll data. * Get Yaw, Pitch, and Roll data.
* *
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees. * Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees. * Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees. * Roll is within [-90,+90] degrees.
*/ */
private double[] getPigeonAngles() { private double[] getPigeonAngles() {
double[] ypr = new double[3]; double[] ypr = new double[3];
@@ -186,7 +186,7 @@ public class RobotGyro implements Gyro {
} }
public double getYaw() { public double getYaw() {
return this.getAngle(); return this.getAngle();
} }
/** /**
src/main/java/frc4388/utility/RobotUnits.java
+11 -11
View File
@@ -8,20 +8,20 @@ package frc4388.utility;
* @author Aarav Shah */ * @author Aarav Shah */
public class RobotUnits { public class RobotUnits {
// constants // constants
// angle conversions // angle conversions
public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;} public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;} public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
// falcon conversions // falcon conversions
public static double falconTicksToRotations(final double ticks) {return ticks / 2048;} public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;} public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
// distance conversions // distance conversions
public static double metersToFeet(final double meters) {return meters * 3.28084;} public static double metersToFeet(final double meters) {return meters * 3.28084;}
public static double feetToMeters(final double feet) {return feet / 3.28084;} public static double feetToMeters(final double feet) {return feet / 3.28084;}
} }
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+14 -14
View File
@@ -6,22 +6,22 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController;
public class DeadbandedXboxController extends XboxController { public class DeadbandedXboxController extends XboxController {
public DeadbandedXboxController(int port) { super(port); } public DeadbandedXboxController(int port) { super(port); }
@Override public double getLeftX() { return getLeft().getX(); } @Override public double getLeftX() { return getLeft().getX(); }
@Override public double getLeftY() { return getLeft().getY(); } @Override public double getLeftY() { return getLeft().getY(); }
@Override public double getRightX() { return getRight().getX(); } @Override public double getRightX() { return getRight().getX(); }
@Override public double getRightY() { return getRight().getY(); } @Override public double getRightY() { return getRight().getY(); }
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); } public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
public static Translation2d skewToDeadzonedCircle(double x, double y) { public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y); Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm(); double magnitude = translation2d.getNorm();
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d; return translation2d;
} }
} }
src/main/java/frc4388/utility/controller/IHandController.java
+7 -7
View File
@@ -5,17 +5,17 @@ package frc4388.utility.controller;
*/ */
public interface IHandController { public interface IHandController {
public double getLeftXAxis(); public double getLeftXAxis();
public double getLeftYAxis(); public double getLeftYAxis();
public double getRightXAxis(); public double getRightXAxis();
public double getRightYAxis(); public double getRightYAxis();
public double getLeftTriggerAxis(); public double getLeftTriggerAxis();
public double getRightTriggerAxis(); public double getRightTriggerAxis();
public int getDpadAngle(); public int getDpadAngle();
} }
src/main/java/frc4388/utility/controller/VirtualController.java
+2 -2
View File
@@ -87,7 +87,7 @@ public class VirtualController extends GenericHID {
@Override @Override
public int getAxisType(int axis) { public int getAxisType(int axis) {
return 1; /* ! Warning, does not return accurate data. return 1; /* ! Warning, does not return accurate data.
Hopefully this isn't a problem */ Hopefully this isn't a problem */
} }
@Override @Override
@@ -107,4 +107,4 @@ public class VirtualController extends GenericHID {
public void setRumble(RumbleType type, double value) { public void setRumble(RumbleType type, double value) {
System.out.println("Why are you Setting rumble on an virtual controller?"); System.out.println("Why are you Setting rumble on an virtual controller?");
} }
} }
src/main/java/frc4388/utility/controller/XboxController.java
+160 -160
View File
@@ -9,210 +9,210 @@ import edu.wpi.first.wpilibj.Joystick;
*/ */
public class XboxController implements IHandController public class XboxController implements IHandController
{ {
public static final int LEFT_X_AXIS = 0; public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1; public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2; public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3; public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4; public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5; public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6; public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6; public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int A_BUTTON = 1; public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2; public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3; public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4; public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5; public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6; public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7; public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8; public static final int START_BUTTON = 8;
public static final int LEFT_JOYSTICK_BUTTON = 9; public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10; public static final int RIGHT_JOYSTICK_BUTTON = 10;
private static final double LEFT_DPAD_TOLERANCE = -0.9; private static final double LEFT_DPAD_TOLERANCE = -0.9;
private static final double RIGHT_DPAD_TOLERANCE = 0.9; private static final double RIGHT_DPAD_TOLERANCE = 0.9;
private static final double BOTTOM_DPAD_TOLERANCE = -0.9; private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
private static final double TOP_DPAD_TOLERANCE = 0.9; private static final double TOP_DPAD_TOLERANCE = 0.9;
private static final double LEFT_TRIGGER_TOLERANCE = 0.5; private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double DEADZONE = 0.1; private static final double DEADZONE = 0.1;
private Joystick m_stick; private Joystick m_stick;
/** /**
* Add your docs here. * Add your docs here.
*/ */
public XboxController(int portNumber){ public XboxController(int portNumber){
m_stick = new Joystick(portNumber); m_stick = new Joystick(portNumber);
} }
/** /**
* Add your docs here. * Add your docs here.
*/ */
public Joystick getJoyStick() { public Joystick getJoyStick() {
return m_stick; return m_stick;
} }
/** /**
* Checks if the input falls within the deadzone. * Checks if the input falls within the deadzone.
* @param input from an axis on the controller * @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone * @return true if input falls in deadzone, false if input falls outside deadzone
*/ */
private boolean inDeadZone(double input){ private boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE); return (Math.abs(input) < DEADZONE);
}
/**
* Updates an input to have a deadzone around the 0 position
* @param input from an axis on the controller
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
} }
}
/** public boolean getAButton(){
* Updates an input to have a deadzone around the 0 position return m_stick.getRawButton(A_BUTTON);
* @param input from an axis on the controller }
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
}
}
public boolean getAButton(){ public boolean getXButton(){
return m_stick.getRawButton(A_BUTTON); return m_stick.getRawButton(X_BUTTON);
} }
public boolean getXButton(){ public boolean getBButton(){
return m_stick.getRawButton(X_BUTTON); return m_stick.getRawButton(B_BUTTON);
} }
public boolean getBButton(){ public boolean getYButton(){
return m_stick.getRawButton(B_BUTTON); return m_stick.getRawButton(Y_BUTTON);
} }
public boolean getYButton(){ public boolean getBackButton(){
return m_stick.getRawButton(Y_BUTTON); return m_stick.getRawButton(BACK_BUTTON);
} }
public boolean getBackButton(){ public boolean getStartButton(){
return m_stick.getRawButton(BACK_BUTTON); return m_stick.getRawButton(START_BUTTON);
} }
public boolean getStartButton(){ public boolean getLeftBumperButton(){
return m_stick.getRawButton(START_BUTTON); return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
} }
public boolean getLeftBumperButton(){ public boolean getRightBumperButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON); return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
} }
public boolean getRightBumperButton(){ public boolean getLeftJoystickButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
} }
public boolean getLeftJoystickButton(){ public boolean getRightJoystickButton(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
} }
public boolean getRightJoystickButton(){ public double getLeftXAxis(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
} }
public double getLeftXAxis(){ public double getLeftYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
} }
public double getLeftYAxis(){ public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
} }
public double getRightXAxis(){ public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
} }
public double getRightYAxis(){ public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
} }
public double getLeftTriggerAxis(){ public double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
} }
public double getRightTriggerAxis(){ /**
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); * Get the angle input from the dpad.
} * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
*/
public int getDpadAngle() {
return m_stick.getPOV(0);
}
/** public boolean getDPadLeft(){
* Get the angle input from the dpad. return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc. }
*/
public int getDpadAngle() {
return m_stick.getPOV(0);
}
public boolean getDPadLeft(){ public boolean getDPadRight(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
} }
public boolean getDPadRight(){ public boolean getDPadTop(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
} }
public boolean getDPadTop(){ public boolean getDPadBottom(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
} }
public boolean getDPadBottom(){ public boolean getLeftTrigger(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
} }
public boolean getLeftTrigger(){ public boolean getRightTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
} }
public boolean getRightTrigger(){ public boolean getRightAxisUpTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
} }
public boolean getRightAxisUpTrigger(){ public boolean getRightAxisDownTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
} }
public boolean getRightAxisDownTrigger(){ public boolean getRightAxisLeftTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
} }
public boolean getRightAxisLeftTrigger(){ public boolean getRightAxisRightTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
} }
public boolean getRightAxisRightTrigger(){ public boolean getLeftAxisUpTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
} }
public boolean getLeftAxisUpTrigger(){ public boolean getLeftAxisDownTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
} }
public boolean getLeftAxisDownTrigger(){ public boolean getLeftAxisLeftTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
} }
public boolean getLeftAxisLeftTrigger(){ public boolean getLeftAxisRightTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
} }
}
public boolean getLeftAxisRightTrigger(){
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
}
}
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+53 -53
View File
@@ -8,61 +8,61 @@ package frc4388.utility.controller;
* exceeds a tolerance defined in {@link XboxController}. * exceeds a tolerance defined in {@link XboxController}.
*/ */
public class XboxTriggerButton {//extends Trigger { public class XboxTriggerButton {//extends Trigger {
public static final int RIGHT_TRIGGER = 0; public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1; public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2; public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6; public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7; public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9; public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
private XboxController m_controller; private XboxController m_controller;
private int m_trigger; private int m_trigger;
/** /**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger * Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/ */
public XboxTriggerButton(XboxController controller, int trigger) { public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller; m_controller = controller;
m_trigger = trigger; m_trigger = trigger;
}
/** {@inheritDoc} */
// @Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();
} }
else if (m_trigger == LEFT_TRIGGER) {
/** {@inheritDoc} */ return m_controller.getLeftTrigger();
// @Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();
}
else if (m_trigger == LEFT_TRIGGER) {
return m_controller.getLeftTrigger();
}
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
return m_controller.getRightAxisUpTrigger();
}
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
return m_controller.getRightAxisDownTrigger();
}
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
return m_controller.getRightAxisRightTrigger();
}
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
return m_controller.getRightAxisLeftTrigger();
}
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
return m_controller.getLeftAxisUpTrigger();
}
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
return m_controller.getLeftAxisDownTrigger();
}
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
return m_controller.getLeftAxisRightTrigger();
}
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
return m_controller.getLeftAxisLeftTrigger();
}
return false;
} }
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
return m_controller.getRightAxisUpTrigger();
}
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
return m_controller.getRightAxisDownTrigger();
}
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
return m_controller.getRightAxisRightTrigger();
}
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
return m_controller.getRightAxisLeftTrigger();
}
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
return m_controller.getLeftAxisUpTrigger();
}
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
return m_controller.getLeftAxisDownTrigger();
}
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
return m_controller.getLeftAxisRightTrigger();
}
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
return m_controller.getLeftAxisLeftTrigger();
}
return false;
}
} }