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 class DefaultSwerveRotOffsets {
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 BACK_LEFT_ROT_OFFSET = -277.646484375;
public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625;
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 BACK_LEFT_ROT_OFFSET = -277.646484375;
public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625;
}
public static final class IDs {
public static final int RIGHT_FRONT_WHEEL_ID = 2;
public static final int RIGHT_FRONT_STEER_ID = 3;
public static final int RIGHT_FRONT_ENCODER_ID = 10;
public static final int RIGHT_FRONT_WHEEL_ID = 2;
public static final int RIGHT_FRONT_STEER_ID = 3;
public static final int RIGHT_FRONT_ENCODER_ID = 10;
public static final int LEFT_FRONT_WHEEL_ID = 4;
public static final int LEFT_FRONT_STEER_ID = 5;
public static final int LEFT_FRONT_ENCODER_ID = 11;
public static final int LEFT_FRONT_WHEEL_ID = 4;
public static final int LEFT_FRONT_STEER_ID = 5;
public static final int LEFT_FRONT_ENCODER_ID = 11;
public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13;
public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13;
}
public static final class PIDConstants {
@@ -120,7 +120,7 @@ public final class Constants {
// misc
public static final int TIMEOUT_MS = 30;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
}
public static final class VisionConstants {
// 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.
*/
public final class Main {
private Main() {
}
private Main() {
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}
src/main/java/frc4388/robot/Robot.java
+101 -101
View File
@@ -23,114 +23,114 @@ import frc4388.utility.RobotTime;
* project.
*/
public class Robot extends TimedRobot {
Command m_autonomousCommand;
Command m_autonomousCommand;
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
//private LED mled = new LED();
/**
* This function is run when the robot is first started up and should be
* 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; }
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
//private LED mled = new LED();
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}
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();
/**
* 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();
}
m_robotTime.startMatchTime();
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
/**
* 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() {
}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
@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.
*/
@Override
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 ParallelCommandGroup intakeToShoot = new ParallelCommandGroup(
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
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_robotShooter.spin())
);
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
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_robotShooter.spin())
);
// private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup(
// new InstantCommand(() -> m_robotShooter.spin()),
@@ -109,44 +109,44 @@ public class RobotContainer {
private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
// MoveToSpeaker,
autoAlign,
new InstantCommand(() -> m_robotShooter.spin()),
new WaitCommand(3.0),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(3.0),
new InstantCommand(() -> m_robotShooter.idle()),
new InstantCommand(() -> autoAlign.reverse()),
autoAlign.asProxy()
);
// MoveToSpeaker,
autoAlign,
new InstantCommand(() -> m_robotShooter.spin()),
new WaitCommand(3.0),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(3.0),
new InstantCommand(() -> m_robotShooter.idle()),
new InstantCommand(() -> autoAlign.reverse()),
autoAlign.asProxy()
);
private SequentialCommandGroup i = new SequentialCommandGroup(
intakeToShootStuff, intakeToShoot,
new InstantCommand(() -> m_robotShooter.idle())
);
intakeToShootStuff, intakeToShoot,
new InstantCommand(() -> m_robotShooter.idle())
);
private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(0.75),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake)
);
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(0.75),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake)
);
private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
interrupt,
new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
interrupt,
new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup ampShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.ampPosition()),
new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed
);
new InstantCommand(() -> m_robotIntake.ampPosition()),
new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed
);
// ! /* Autos */
private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand();
@@ -167,114 +167,114 @@ public class RobotContainer {
// );
private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
);
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
);
private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup(
startLeftMoveRight.asProxy(),
ejectToShoot.asProxy(),
taxi.asProxy()
);
startLeftMoveRight.asProxy(),
ejectToShoot.asProxy(),
taxi.asProxy()
);
private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup(
startRightMoveLeft.asProxy(),
ejectToShoot.asProxy(),
taxi.asProxy()
);
startRightMoveLeft.asProxy(),
ejectToShoot.asProxy(),
taxi.asProxy()
);
private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
// new WaitCommand(1).asProxy(),
// new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
// new WaitCommand(0.5).asProxy(),
// new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
// new WaitCommand(1).asProxy(),
// new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
// new WaitCommand(1).asProxy(),
// new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
// new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
// new WaitCommand(1).asProxy(),
// new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
// new WaitCommand(0.5).asProxy(),
// new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
// new WaitCommand(1).asProxy(),
// new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
// new WaitCommand(1).asProxy(),
// new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
// new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup stayTwoNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup threeNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
//? Create Another Parallel Command Group :(
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
);
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
//? Create Another Parallel Command Group :(
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1.4).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(0.5),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
);
private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
.addOption("Taxi Auto", taxi.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("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy())
// .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy())
.addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy())
.addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy())
.buildDisplay();
.addOption("Taxi Auto", taxi.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("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy())
// .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy())
.addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy())
.addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy())
.buildDisplay();
@@ -287,19 +287,19 @@ public class RobotContainer {
new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
DriverStation.silenceJoystickConnectionWarning(true);
CameraServer.startAutomaticCapture();
DriverStation.silenceJoystickConnectionWarning(true);
CameraServer.startAutomaticCapture();
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
// ! Swerve Drive Default Command (Regular Rotation)
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive)
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
// ! Swerve Drive Default Command (Regular Rotation)
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(),
true);
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
m_robotSwerveDrive.setToSlow();
m_robotSwerveDrive.setToSlow();
// ! Swerve Drive Default Command (Orientation Rotation)
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
@@ -356,25 +356,25 @@ public class RobotContainer {
// .onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
() -> autoplaybackName.get()))
.onFalse(new InstantCommand());
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
() -> autoplaybackName.get()))
.onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
() -> autoplaybackName.get(),
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false))
.onFalse(new InstantCommand());
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
() -> autoplaybackName.get(),
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false))
.onFalse(new InstantCommand());
// ! /* Speed */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .whileTrue(new InstantCommand(() ->
@@ -383,7 +383,7 @@ public class RobotContainer {
// true), m_robotSwerveDrive));
//? /* Operator Buttons */
//? /* Operator Buttons */
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn()))
@@ -398,7 +398,7 @@ public class RobotContainer {
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(emergencyRetract);
.onTrue(emergencyRetract);
// Override Intake Position encoder: out
@@ -443,7 +443,7 @@ public class RobotContainer {
}
private void configureVirtualButtonBindings() {
private void configureVirtualButtonBindings() {
/* Driver Buttons */
// new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
@@ -462,7 +462,7 @@ public class RobotContainer {
// /* Operator Buttons */
new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON)
new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor()));
@@ -475,7 +475,7 @@ public class RobotContainer {
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors()));
new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON)
.onTrue(emergencyRetract.asProxy());
.onTrue(emergencyRetract.asProxy());
// 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:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class ArmIntakeInAuto extends ParallelCommandGroup {
private final Intake intake;
private final Shooter shooter;
private final SwerveDrive swerve;
/** Creates a new ArmIntakeInAuto. */
public ArmIntakeInAuto(Intake intake, Shooter shooter, SwerveDrive swerve) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
this.intake = intake;
this.shooter = shooter;
this.swerve = swerve;
addCommands((new ArmIntakeInTimeout(intake, shooter).withTimeout(3)), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt")));
}
private final Intake intake;
private final Shooter shooter;
private final SwerveDrive swerve;
/** Creates a new ArmIntakeInAuto. */
public ArmIntakeInAuto(Intake intake, Shooter shooter, SwerveDrive swerve) {
// Add your commands in the addCommands() call, e.g.
// addCommands(new FooCommand(), new BarCommand());
this.intake = intake;
this.shooter = shooter;
this.swerve = swerve;
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;
public class ArmIntakeInTimeout extends Command {
/** Creates a new ArmIntakeIn. */
private Intake robotIntake;
private Shooter robotShooter;
/** Creates a new ArmIntakeIn. */
private Intake robotIntake;
private Shooter robotShooter;
public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) {
// Use addRequirements() here to declare subsystem dependencies.
this.robotIntake = robotIntake;
this.robotShooter = robotShooter;
public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) {
// Use addRequirements() here to declare subsystem dependencies.
this.robotIntake = robotIntake;
this.robotShooter = 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();
addRequirements(this.robotIntake, this.robotShooter);
}
}
// 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));
// }
}
// 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.
@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
+14 -14
View File
@@ -118,18 +118,18 @@ public class AutoAlign extends Command {
private Translation2d calcRotStick(){
double error = pose.getRotation().getDegrees() - targetRot.getDegrees();
cumError += error * .02; // 20 ms
double delta = error - prevError;
cumError += error * .02; // 20 ms
double delta = error - prevError;
final double kP = 4;
final double kI = 4;
final double kD = 4;
final double kF = 4;
double output = error * kP;
output += cumError * kI;
output += delta * kD;
output += kF;
double output = error * kP;
output += cumError * kI;
output += delta * kD;
output += kF;
prevError = error;
return clamp(output, 0);
@@ -140,8 +140,8 @@ public class AutoAlign extends Command {
}
// Called when the command is initially scheduled.
@Override
public final void initialize() {
@Override
public final void initialize() {
isRed = alliance.get() == DriverStation.Alliance.Red;
if(reverseAfterFinish){
// isReverseFinished = false;
@@ -150,10 +150,10 @@ public class AutoAlign extends Command {
moveStickReplayArr = new Translation2d[]{};
rotStickReplayArr = new Translation2d[]{};
}
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
@Override
public void execute() {
// Update limelight pos
pose = limelight.getPose();
@@ -179,7 +179,7 @@ public class AutoAlign extends Command {
// }
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){
// Get reverse direction
moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1];
@@ -201,8 +201,8 @@ public class AutoAlign extends Command {
}
// Returns true when the command should end.
@Override
public final boolean isFinished() {
@Override
public final boolean isFinished() {
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:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class AutoBalance extends PID {
RobotGyro gyro;
Intake intake;
/** Creates a new AutoBalance. */
public AutoBalance(RobotGyro gyro, Intake intake) {
super(0.6, 0, 0, 0, 0);
RobotGyro gyro;
Intake intake;
/** Creates a new AutoBalance. */
public AutoBalance(RobotGyro gyro, Intake intake) {
super(0.6, 0, 0, 0, 0);
this.gyro = gyro;
this.intake = intake;
this.gyro = gyro;
this.intake = intake;
// Use addRequirements() here to declare subsystem dependencies.
// Configure additional PID options by calling `getController` here.
addRequirements(intake);
}
// Use addRequirements() here to declare subsystem dependencies.
// Configure additional PID options by calling `getController` here.
addRequirements(intake);
}
// Returns true when the command should end.
// Returns true when the command should end.
public double getError() {
var pitch = gyro.getRoll();
SmartDashboard.putNumber("pitch", pitch);
return pitch;
}
public double getError() {
var pitch = gyro.getRoll();
SmartDashboard.putNumber("pitch", pitch);
return pitch;
}
@Override
public void runWithOutput(double output) {
double out2 = MathUtil.clamp(output / 40, -59, 0);
if (Math.abs(getError()) < 3) out2 = 0;
intake.talonPIDPosition(out2);
}
@Override
public void runWithOutput(double output) {
double out2 = MathUtil.clamp(output / 40, -59, 0);
if (Math.abs(getError()) < 3) out2 = 0;
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;
public class ArmIntakeIn extends Command {
/** Creates a new ArmIntakeIn. */
private Intake robotIntake;
private Shooter robotShooter;
/** Creates a new ArmIntakeIn. */
private Intake robotIntake;
private Shooter robotShooter;
public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) {
// Use addRequirements() here to declare subsystem dependencies.
this.robotIntake = robotIntake;
this.robotShooter = robotShooter;
public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) {
// Use addRequirements() here to declare subsystem dependencies.
this.robotIntake = robotIntake;
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 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 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) {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// 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));
// }
}
// 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/PID.java
+39 -39
View File
@@ -8,53 +8,53 @@ import edu.wpi.first.wpilibj2.command.Command;
import frc4388.utility.Gains;
public abstract class PID extends Command {
protected Gains gains;
private double output = 0;
private double tolerance = 0;
protected Gains gains;
private double output = 0;
private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PID(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance;
}
/** Creates a new PelvicInflammatoryDisease. */
public PID(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance;
}
public PID(Gains gains, double tolerance) {
this.gains = gains;
this.tolerance = tolerance;
}
public PID(Gains gains, double tolerance) {
this.gains = gains;
this.tolerance = tolerance;
}
/** produces the error from the setpoint */
public abstract double getError();
/** produces the error from the setpoint */
public abstract double getError();
/** todo: javadoc */
public abstract void runWithOutput(double output);
/** todo: javadoc */
public abstract void runWithOutput(double output);
// Called when the command is initially scheduled.
@Override
public final void initialize() {
output = 0;
}
// Called when the command is initially scheduled.
@Override
public final void initialize() {
output = 0;
}
private double prevError, cumError = 0;
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
@Override
public final void execute() {
double error = getError();
cumError += error * .02; // 20 ms
double delta = error - prevError;
// Called every time the scheduler runs while the command is scheduled.
@Override
public final void execute() {
double error = getError();
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
runWithOutput(output);
}
runWithOutput(output);
}
// Returns true when the command should end.
@Override
public final boolean isFinished() {
return Math.abs(getError()) < tolerance;
}
// Returns true when the command should end.
@Override
public final boolean isFinished() {
return Math.abs(getError()) < tolerance;
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
+114 -114
View File
@@ -14,132 +14,132 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends CommandBase {
private final SwerveDrive swerve;
private String filename;
private int mult = 1;
private Scanner input;
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0;
private long startTime = 0;
private long playbackTime = 0;
private int lastIndex;
private boolean m_finished = false; // ! find a better way
private final SwerveDrive swerve;
private String filename;
private int mult = 1;
private Scanner input;
private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0;
private long startTime = 0;
private long playbackTime = 0;
private int lastIndex;
private boolean m_finished = false; // ! find a better way
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
this.swerve = swerve;
this.filename = filename;
this.mult = mult;
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
this.swerve = swerve;
this.filename = filename;
this.mult = mult;
addRequirements(this.swerve);
}
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename) {
this(swerve, filename, 1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
m_finished = false;
startTime = System.currentTimeMillis();
playbackTime = 0;
lastIndex = 0;
try {
input = new Scanner(new File("/home/lvuser/autos/" + filename));
String line = "";
while (input.hasNextLine()) {
line = input.nextLine();
if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
continue;
}
String[] values = line.split(",");
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
var out = new TimedOutput();
out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]);
out.timedOffset = Long.parseLong(values[4]);
outputs.add(out);
}
input.close();
} catch (FileNotFoundException e) {
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;
addRequirements(this.swerve);
}
// skip to reasonable time frame
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
{
int i = lastIndex == 0 ? 1 : lastIndex;
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
i++;
}
if (i >= outputs.size()) {
m_finished = true; // ! kind of a hack
return;
}
lastIndex = i;
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename) {
this(swerve, filename, 1);
}
TimedOutput lastOut = outputs.get(lastIndex - 1);
TimedOutput out = outputs.get(lastIndex);
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
m_finished = false;
double deltaTime = out.timedOffset - lastOut.timedOffset;
double playbackDelta = playbackTime - lastOut.timedOffset;
startTime = System.currentTimeMillis();
playbackTime = 0;
lastIndex = 0;
try {
input = new Scanner(new File("/home/lvuser/autos/" + filename));
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
String line = "";
while (input.hasNextLine()) {
line = input.nextLine();
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY),
// new Translation2d(out.rightX, out.rightY),
// true);
if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
continue;
}
// this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
// new Translation2d(lerpRX, lerpRY),
// true);
String[] values = line.split(",");
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY),
true);
var out = new TimedOutput();
out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]);
counter++;
}
out.timedOffset = Long.parseLong(values[4]);
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
input.close();
swerve.stopModules();
}
outputs.add(out);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return m_finished;
}
input.close();
} catch (FileNotFoundException e) {
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
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
{
int i = lastIndex == 0 ? 1 : lastIndex;
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
i++;
}
if (i >= outputs.size()) {
m_finished = true; // ! kind of a hack
return;
}
lastIndex = i;
}
TimedOutput lastOut = outputs.get(lastIndex - 1);
TimedOutput out = outputs.get(lastIndex);
double deltaTime = out.timedOffset - lastOut.timedOffset;
double playbackDelta = playbackTime - lastOut.timedOffset;
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
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),
// new Translation2d(lerpRX, lerpRY),
// true);
this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY),
true);
counter++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
input.close();
swerve.stopModules();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return m_finished;
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+72 -72
View File
@@ -16,82 +16,82 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickRecorder extends CommandBase {
public final SwerveDrive swerve;
public final SwerveDrive swerve;
public final Supplier<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
private String filename;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1;
public final Supplier<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
private String filename;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1;
/** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> rightY,
String filename)
{
this.swerve = swerve;
this.leftX = leftX;
this.leftY = leftY;
this.rightX = rightX;
this.rightY = rightY;
this.filename = filename;
/** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> rightY,
String filename)
{
this.swerve = swerve;
this.leftX = leftX;
this.leftY = leftY;
this.rightX = rightX;
this.rightY = rightY;
this.filename = filename;
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();
addRequirements(this.swerve);
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
// 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.
@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 {
SwerveDrive drive;
double targetAngle;
SwerveDrive drive;
double targetAngle;
/** Creates a new RotateToAngle. */
public RotateToAngle(SwerveDrive drive, double targetAngle) {
super(0.3, 0.0, 0.0, 0.0, 1);
/** Creates a new RotateToAngle. */
public RotateToAngle(SwerveDrive drive, double targetAngle) {
super(0.3, 0.0, 0.0, 0.0, 1);
this.drive = drive;
this.targetAngle = targetAngle;
this.drive = drive;
this.targetAngle = targetAngle;
addRequirements(drive);
}
addRequirements(drive);
}
@Override
public double getError() {
return targetAngle - drive.getGyroAngle();
}
@Override
public double getError() {
return targetAngle - drive.getGyroAngle();
}
@Override
public void runWithOutput(double output) {
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
}
@Override
public void runWithOutput(double output) {
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) {
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;
}
@@ -133,9 +133,9 @@ public class neoJoystickPlayback extends Command {
controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
if (i == 0) {
this.swerve.driveWithInput(
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
true);
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
true);
}
}
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];
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
double[] axes = {controller.getLeftX(), controller.getLeftY(),
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
controller.getRightX(), controller.getRightY()};
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
controller.getRightX(), controller.getRightY()};
short button = 0;
for (int j = 0; j < 10; j++)
if (controller.getRawButton(j+1))
@@ -71,8 +71,8 @@ public class neoJoystickRecorder extends Command {
frames.add(frame);
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]),
true); // Really jank way of doing this.
new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]),
true); // Really jank way of doing this.
}
@Override
@@ -105,7 +105,7 @@ public class neoJoystickRecorder extends Command {
}
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) {
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
public class Climber extends SubsystemBase {
/** Creates a new Climber. */
TalonFX climbMotor;
/** Creates a new Climber. */
TalonFX climbMotor;
public Climber(TalonFX climbMotor) {
this.climbMotor = climbMotor;
this.climbMotor.setInverted(true);
var slot0Configs = new Slot0Configs();
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.kD = 0.06; // A velocity of 1 rps results in 0.1 V output
public Climber(TalonFX climbMotor) {
this.climbMotor = climbMotor;
this.climbMotor.setInverted(true);
var slot0Configs = new Slot0Configs();
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.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() {
PositionVoltage request = new PositionVoltage(0);
climbMotor.setControl(request.withPosition(-520));
//climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
}
public void climbOut() {
PositionVoltage request = new PositionVoltage(0);
climbMotor.setControl(request.withPosition(-520));
//climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
}
public void climbIn() {
PositionVoltage request = new PositionVoltage(-520);
climbMotor.setControl(request.withPosition(0));
// climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
}
public void climbIn() {
PositionVoltage request = new PositionVoltage(-520);
climbMotor.setControl(request.withPosition(0));
// climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
}
public void stopClimb() {
climbMotor.set(0.d);
}
public void stopClimb() {
climbMotor.set(0.d);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/Intake.java
+238 -238
View File
@@ -38,303 +38,303 @@ import frc4388.utility.configurable.ConfigurableDouble;
public class Intake extends SubsystemBase {
//NEO
private CANSparkMax intakeMotor;
private CANSparkMax pivot;
private SparkPIDController m_spedController;
private SparkLimitSwitch forwardLimit;
private SparkLimitSwitch reverseLimit;
private SparkLimitSwitch intakeforwardLimit;
private SparkLimitSwitch intakereverseLimit;
//NEO
private CANSparkMax intakeMotor;
private CANSparkMax pivot;
private SparkPIDController m_spedController;
private SparkLimitSwitch forwardLimit;
private SparkLimitSwitch reverseLimit;
private SparkLimitSwitch intakeforwardLimit;
private SparkLimitSwitch intakereverseLimit;
//Talon
private TalonFX talonIntake;
private TalonFX talonPivot;
private CANcoder encoder;
//Talon
private TalonFX talonIntake;
private TalonFX talonPivot;
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;
private ConfigurableDouble outtakeSpeed = new ConfigurableDouble("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
private BooleanSupplier sup = () -> true;
private BooleanSupplier dup = () -> false;
public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
private ConfigurableDouble outtakeSpeed = new ConfigurableDouble("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
private BooleanSupplier sup = () -> true;
private BooleanSupplier dup = () -> false;
private double smartDashboardOuttakeValue;
private double smartDashboardOuttakeValue;
/** Creates a new Intake. */
//For NEO
// public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
// this.intakeMotor = intakeMotor;
// this.pivot = pivot;
/** Creates a new Intake. */
//For NEO
// public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
// this.intakeMotor = intakeMotor;
// this.pivot = pivot;
// pivot.restoreFactoryDefaults();
// //pivot.setInverted(true);
// pivot.restoreFactoryDefaults();
// //pivot.setInverted(true);
// forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// forwardLimit.enableLimitSwitch(true);
// reverseLimit.enableLimitSwitch(true);
// forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// forwardLimit.enableLimitSwitch(true);
// reverseLimit.enableLimitSwitch(true);
// intakeMotor.restoreFactoryDefaults();
// intakeMotor.restoreFactoryDefaults();
// intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// intakeforwardLimit.enableLimitSwitch(true);
// intakereverseLimit.enableLimitSwitch(false);
// intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
// intakeforwardLimit.enableLimitSwitch(true);
// intakereverseLimit.enableLimitSwitch(false);
// //Arm PID
// m_spedController = pivot.getPIDController();
// m_spedController.setP(armGains.kP);
// m_spedController.setI(armGains.kI);
// m_spedController.setD(armGains.kD);
// //Arm PID
// m_spedController = pivot.getPIDController();
// m_spedController.setP(armGains.kP);
// m_spedController.setI(armGains.kI);
// 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
public Intake(TalonFX talonIntake, TalonFX talonPivot) {
this.talonIntake = talonIntake;
this.talonPivot = talonPivot;
//For Talon
public Intake(TalonFX talonIntake, TalonFX talonPivot) {
this.talonIntake = talonIntake;
this.talonPivot = talonPivot;
talonIntake.getConfigurator().apply(new TalonFXConfiguration());
talonPivot.getConfigurator().apply(new TalonFXConfiguration());
talonIntake.getConfigurator().apply(new TalonFXConfiguration());
talonPivot.getConfigurator().apply(new TalonFXConfiguration());
talonIntake.setNeutralMode(NeutralModeValue.Brake);
talonPivot.setNeutralMode(NeutralModeValue.Brake);
talonIntake.setNeutralMode(NeutralModeValue.Brake);
talonPivot.setNeutralMode(NeutralModeValue.Brake);
// talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs());
// talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs());
// talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs());
// talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs());
// doodooController.Slot0.kP = armGains.kP;
// doodooController.Slot1.kI = armGains.kI;
// doodooController.Slot2.kD = armGains.kD;
// doodooController.Slot0.kP = armGains.kP;
// doodooController.Slot1.kI = armGains.kI;
// doodooController.Slot2.kD = armGains.kD;
// in init function, set slot 0 gains
var slot0Configs = new Slot0Configs();
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.kD = 0.06; // A velocity of 1 rps results in 0.1 V output
// in init function, set slot 0 gains
var slot0Configs = new Slot0Configs();
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.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) {
talonPivot.setPosition(val);
}
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);
// ! Talon Methods
public void talonPIDIn() {
PositionVoltage request = new PositionVoltage(-59);
talonPivot.setControl(request.withPosition(0));
}
}
public void ampPosition() {
PositionVoltage request = new PositionVoltage(-0);
talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value
}
public void talonPIDOut() {
PositionVoltage request = new PositionVoltage(0);
talonPivot.setControl(request.withPosition(-59));
}
public void ampShoot(double speed) {
talonSpinIntakeMotor(speed);
}
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) {
talonPivot.setPosition(val);
}
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() {
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);
// }
// ! NEO Methods
//hanoff
// public void spinIntakeMotor() {
// intakeMotor.set(IntakeConstants.INTAKE_SPEED);
// }
// //Rotate robot in for handoff
// public void rotateArmIn() {
// pivot.set(IntakeConstants.PIVOT_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);
// //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 pidIn() {
// m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition);
// //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
// }
// public void pidOut() {
// m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
// }
// public void pidOut() {
// m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
// }
// public void limitNote() {
// if (intakeforwardLimit.isPressed()) {
// rotateArmIn2();
// } else {
// spinIntakeMotor();
// }
// }
// public void limitNote() {
// if (intakeforwardLimit.isPressed()) {
// rotateArmIn2();
// } else {
// spinIntakeMotor();
// }
// }
// public void rotateArmOut2() {
// if(reverseLimit.isPressed()){
// stopArmMotor();
// } else {
// pidOut();
// }
// }
// public void rotateArmOut2() {
// if(reverseLimit.isPressed()){
// stopArmMotor();
// } else {
// pidOut();
// }
// }
// public void rotateArmIn2() {
// if(forwardLimit.isPressed()){
// stopArmMotor();
// } else {
// pidIn();
// }
// }
// public void rotateArmIn2() {
// if(forwardLimit.isPressed()){
// stopArmMotor();
// } else {
// pidIn();
// }
// }
// public void handoff() {
// intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
// }
// public void handoff() {
// intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
// }
// public void handoff2() {
// if(intakeforwardLimit.isPressed()) {
// intakeMotor.set(-smartDashboardOuttakeValue);
// } else {
// intakeMotor.set(-smartDashboardOuttakeValue);
// }
// }
// public void handoff2() {
// if(intakeforwardLimit.isPressed()) {
// intakeMotor.set(-smartDashboardOuttakeValue);
// } else {
// intakeMotor.set(-smartDashboardOuttakeValue);
// }
// }
// public void stopIntakeMotors() {
// intakeMotor.set(0);
// }
// public void stopIntakeMotors() {
// intakeMotor.set(0);
// }
// public void stopArmMotor() {
// pivot.set(0);
// }
// public void stopArmMotor() {
// pivot.set(0);
// }
// public RelativeEncoder getEncoder() {
// return pivot.getEncoder();
// }
// public RelativeEncoder getEncoder() {
// return pivot.getEncoder();
// }
// public boolean getForwardLimitSwitchState() {
// return forwardLimit.isPressed();
// }
// public boolean getForwardLimitSwitchState() {
// return forwardLimit.isPressed();
// }
// public boolean getReverseLimitSwitchState() {
// return reverseLimit.isPressed();
// }
// public boolean getReverseLimitSwitchState() {
// return reverseLimit.isPressed();
// }
// public boolean getIntakeLimitSwtichState() {
// return intakeforwardLimit.isPressed();
// }
// public boolean getIntakeLimitSwtichState() {
// return intakeforwardLimit.isPressed();
// }
// public void setVoltage(double voltage) {
// pivot.setVoltage(voltage);
// }
// public void setVoltage(double voltage) {
// pivot.setVoltage(voltage);
// }
// public double getVelocity() {
// return pivot.getEncoder().getVelocity();
// }
// public double getVelocity() {
// return pivot.getEncoder().getVelocity();
// }
// public void setPivotEncoderPosition(int val) {
// pivot.getEncoder().setPosition(val);
// }
// public void setPivotEncoderPosition(int val) {
// pivot.getEncoder().setPosition(val);
// }
// public void resetPosition() {
// if(forwardLimit.isPressed()) {
// setPivotEncoderPosition(0);
// }
// }
// public void resetPosition() {
// if(forwardLimit.isPressed()) {
// setPivotEncoderPosition(0);
// }
// }
// public double getPos() {
// return pivot.getEncoder().getPosition();
// }
// public double getPos() {
// return pivot.getEncoder().getPosition();
// }
// public double getIntakeVelocity() {
// return intakeMotor.getEncoder().getVelocity();
// }
// public double getIntakeVelocity() {
// return intakeMotor.getEncoder().getVelocity();
// }
// public void rotateArm() {
// public void rotateArm() {
// }
// }
// public BooleanSupplier getArmFowardLimitState() {
// if(forwardLimit.isPressed()) {
// return sup;
// } else {
// return dup;
// }
// }
// public BooleanSupplier getArmFowardLimitState() {
// if(forwardLimit.isPressed()) {
// return sup;
// } else {
// return dup;
// }
// }
// public void changeIntakeNeutralState() {
// if(forwardLimit.isPressed()) {
// intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
// }
// }
// public void changeIntakeNeutralState() {
// 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();
@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();
resetArmPosition();
SmartDashboard.putNumber("Pivot Position", getArmPos());
SmartDashboard.putNumber("Pivot Position", getArmPos());
smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+63 -63
View File
@@ -22,69 +22,69 @@ import frc4388.utility.LEDPatterns;
*/
public class LED extends SubsystemBase {
static AddressableLED m_led;
static AddressableLEDBuffer m_ledBuffer;
static LED m_self;
/**
* Add your docs here.
*/
static AddressableLED m_led;
static AddressableLEDBuffer m_ledBuffer;
static LED m_self;
/**
* Add your docs here.
*/
public LED(){
if(m_self != null)
return;
m_led = new AddressableLED(9);
m_ledBuffer = new AddressableLEDBuffer(10);
m_led.setLength(m_ledBuffer.getLength());
m_led.setData(m_ledBuffer);
m_led.start();
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
}
public static LED getInstance() {
if(m_self == null)
m_self = new LED();
return m_self;
}
@Override
public void periodic(){
//gamermode();
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
return;
}
static int firstcolor = 0;
static void gamermode() {
for(int i = 0; i < m_ledBuffer.getLength(); i++) {
final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
setLEDHSV(i, hue, 255, 128);
}
firstcolor +=3;
firstcolor %= 180;
}
/**
* Add your docs here.
*/
public static void updateLED (){
gamermode();
// m_LEDController.set(m_currentPattern.getValue());
}
public LED(){
if(m_self != null)
return;
m_led = new AddressableLED(9);
m_ledBuffer = new AddressableLEDBuffer(10);
m_led.setLength(m_ledBuffer.getLength());
m_led.setData(m_ledBuffer);
m_led.start();
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
}
public static LED getInstance() {
if(m_self == null)
m_self = new LED();
return m_self;
}
@Override
public void periodic(){
//gamermode();
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
return;
}
static int firstcolor = 0;
static void gamermode() {
for(int i = 0; i < m_ledBuffer.getLength(); i++) {
final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180;
setLEDHSV(i, hue, 255, 128);
}
firstcolor +=3;
firstcolor %= 180;
}
/**
* Add your docs here.
*/
public static void updateLED (){
gamermode();
// m_LEDController.set(m_currentPattern.getValue());
}
/**
* Add your docs here.
*/
public static void setLEDRGB(int lednum, int r, int g, int b){
m_ledBuffer.setRGB(lednum, r, g, b);
//m_currentPattern = pattern;
// m_LEDController.set(m_currentPattern.getValue());
}
public static void setLEDHSV(int lednum, int hue, int sat, int val){
m_ledBuffer.setRGB(lednum, hue, sat, val);
//m_currentPattern = pattern;
// m_LEDController.set(m_currentPattern.getValue());
}
/**
* Add your docs here.
* @return
*/
public AddressableLEDBuffer getBuffer() {
return m_ledBuffer;
}
/**
* Add your docs here.
*/
public static void setLEDRGB(int lednum, int r, int g, int b){
m_ledBuffer.setRGB(lednum, r, g, b);
//m_currentPattern = pattern;
// m_LEDController.set(m_currentPattern.getValue());
}
public static void setLEDHSV(int lednum, int hue, int sat, int val){
m_ledBuffer.setRGB(lednum, hue, sat, val);
//m_currentPattern = pattern;
// m_LEDController.set(m_currentPattern.getValue());
}
/**
* Add your docs here.
* @return
*/
public AddressableLEDBuffer getBuffer() {
return m_ledBuffer;
}
}
src/main/java/frc4388/robot/subsystems/Limelight.java
+49 -49
View File
@@ -18,65 +18,65 @@ import frc4388.robot.Constants.VisionConstants;
public class Limelight extends SubsystemBase {
// [X, Y, Z, Roll, Pitch, Yaw]
private double[] cameraPose;
private boolean isTag;
// [X, Y, Z, Roll, Pitch, Yaw]
private double[] cameraPose;
private boolean isTag;
private Pose2d pose;
private boolean isNearSpeaker;
private Pose2d pose;
private boolean isNearSpeaker;
public boolean getIsTag() {
return isTag;
}
private void update() {
SmartDashboard.putBoolean("Apriltag", isTag);
if(!isTag){
return;
public boolean getIsTag() {
return isTag;
}
double x = cameraPose[0];
double y = cameraPose[1];
double yaw = cameraPose[5];
private void update() {
SmartDashboard.putBoolean("Apriltag", isTag);
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);
double distance;
boolean isRed = DriverStation.getAlliance().get() == Alliance.Red;
if(isRed){
distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter);
}else{
distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter);
double distance;
if(isRed){
distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter);
}else{
distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter);
}
isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
SmartDashboard.putNumber("speakerDistance", distance);
}
isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
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 {
private TalonFX leftShooter;
private TalonFX rightShooter;
private TalonFX leftShooter;
private TalonFX rightShooter;
private Limelight limelight;
private Limelight limelight;
private int spinMode = 0;
// 0 = Stop / Coast
// 1 = Idle
// 2 = Idle Near Speaker
// 3 = Spin
// 4 = SingleSpin
private int spinMode = 0;
// 0 = Stop / Coast
// 1 = Idle
// 2 = Idle Near Speaker
// 3 = Spin
// 4 = SingleSpin
private double smartDashboardShooterSpeed;
private double smartDashboardShooterSpeed;
/** Creates a new Shooter. */
public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) {
leftShooter = leftTalonFX;
rightShooter = rightTalonFX;
/** Creates a new Shooter. */
public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) {
leftShooter = leftTalonFX;
rightShooter = rightTalonFX;
limelight = tmplimelight;
limelight = tmplimelight;
leftShooter.setNeutralMode(NeutralModeValue.Coast);
rightShooter.setNeutralMode(NeutralModeValue.Coast);
SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
leftShooter.setNeutralMode(NeutralModeValue.Coast);
rightShooter.setNeutralMode(NeutralModeValue.Coast);
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
+227 -227
View File
@@ -17,267 +17,267 @@ import frc4388.utility.RobotGyro;
public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront;
private SwerveModule rightFront;
private SwerveModule leftBack;
private SwerveModule rightBack;
private SwerveModule leftFront;
private SwerveModule rightFront;
private SwerveModule leftBack;
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 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 rightBackLocation = 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 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 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 autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
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 rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.gyro = gyro;
this.gyro = gyro;
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) {
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() * autoSpeedAdjust);
// 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);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rot * 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));
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
}
}
boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
public boolean rotateToTarget(double angle) {
double currentAngle = getGyroAngle();
double error = angle - currentAngle;
double rot = 0;
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
// ! 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 (Math.abs(angle - getGyroAngle()) < 5.0) {
return 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));
}
return false;
}
public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
public double getGyroAngle() {
return gyro.getAngle();
}
double rot = 0;
public void resetGyro() {
gyro.reset();
rotTarget = 0.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;
}
public void resetGyroFlip() {
gyro.resetFlip();
rotTarget = 0.0;
}
// SmartDashboard.putBoolean("drift correction", true);
public void resetGyroRightBlue() {
gyro.resetRightSideBlue();
rotTarget = 0.0;
}
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
public void resetGyroRightAmp() {
gyro.resetAmpSide();
rotTarget = 0.0;
}
}
public void stopModules() {
for (SwerveModule module : this.modules) {
module.stop();
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust);
// 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 SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) {
public boolean getSpeedState() {
Translation2d rightStick = new Translation2d(-rightX, rightY);
return false;
}
if(fieldRelative) {
double rot = 0;
if(rightStick.getNorm() > 0.5) {
orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1));
@Override
public void periodic() {
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
}
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
}
public void shiftDown() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
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);
} 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 setToSlow() {
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");
}
public void setToFast() {
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");
}
public void setToTurbo() {
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");
}
public void shiftUp() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
} else {
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
}
}
}
public void toggleGear(double angle) {
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;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
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 double getGyroAngle() {
return gyro.getAngle();
}
public void resetGyro() {
gyro.reset();
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() {
for (SwerveModule module : this.modules) {
module.stop();
}
}
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
public boolean getSpeedState() {
return false;
}
@Override
public void periodic() {
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
}
public void shiftDown() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
}
}
public void setToSlow() {
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");
}
public void setToFast() {
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");
}
public void setToTurbo() {
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");
}
public void shiftUp() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
} else {
}
}
public void toggleGear(double angle) {
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;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
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() {
return new SwerveModuleState(
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
getAngle()
);
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
getAngle()
);
}
/**
src/main/java/frc4388/utility/LEDPatterns.java
+18 -18
View File
@@ -4,8 +4,8 @@ package frc4388.utility;
* Add your docs here.
*/
public enum LEDPatterns {
/* PALLETTE PATTERNS */
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
/* PALLETTE PATTERNS */
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),
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),
@@ -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),
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
/* 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),
/* 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_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 */
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
/* 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_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 */
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
/* 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_WAVES(0.53f), C1C2_SINELON(0.55f),
/* 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 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_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_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);
/* GETTERS/SETTERS */
private final float id;
LEDPatterns(float id) {
this.id = id;
}
public float getValue() {
return id;
}
/* GETTERS/SETTERS */
private final float id;
LEDPatterns(float id) {
this.id = id;
}
public float getValue() {
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.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] ypr = new double[3];
@@ -186,7 +186,7 @@ public class RobotGyro implements Gyro {
}
public double getYaw() {
return this.getAngle();
return this.getAngle();
}
/**
src/main/java/frc4388/utility/RobotUnits.java
+10 -10
View File
@@ -8,20 +8,20 @@ package frc4388.utility;
* @author Aarav Shah */
public class RobotUnits {
// constants
// constants
// angle conversions
public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
// angle conversions
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
public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
// falcon conversions
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
public static double metersToFeet(final double meters) {return meters * 3.28084;}
// distance conversions
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
+13 -13
View File
@@ -6,22 +6,22 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.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 getLeftY() { return getLeft().getY(); }
@Override public double getRightX() { return getRight().getX(); }
@Override public double getRightY() { return getRight().getY(); }
@Override public double getLeftX() { return getLeft().getX(); }
@Override public double getLeftY() { return getLeft().getY(); }
@Override public double getRightX() { return getRight().getX(); }
@Override public double getRightY() { return getRight().getY(); }
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
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 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
+1 -1
View File
@@ -87,7 +87,7 @@ public class VirtualController extends GenericHID {
@Override
public int getAxisType(int axis) {
return 1; /* ! Warning, does not return accurate data.
Hopefully this isn't a problem */
Hopefully this isn't a problem */
}
@Override
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 static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8;
public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8;
public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10;
public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10;
private static final double LEFT_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 TOP_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 BOTTOM_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 RIGHT_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_AXIS_UP_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_LEFT_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_RIGHT_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_DOWN_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_UP_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_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.
*/
public XboxController(int portNumber){
m_stick = new Joystick(portNumber);
}
public XboxController(int portNumber){
m_stick = new Joystick(portNumber);
}
/**
/**
* Add your docs here.
*/
public Joystick getJoyStick() {
return m_stick;
public Joystick getJoyStick() {
return m_stick;
}
/**
* Checks if the input falls within the deadzone.
* @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
private boolean inDeadZone(double input){
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;
}
}
/**
* Checks if the input falls within the deadzone.
* @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
private boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE);
}
public boolean getAButton(){
return m_stick.getRawButton(A_BUTTON);
}
/**
* 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 getXButton(){
return m_stick.getRawButton(X_BUTTON);
}
public boolean getAButton(){
return m_stick.getRawButton(A_BUTTON);
}
public boolean getBButton(){
return m_stick.getRawButton(B_BUTTON);
}
public boolean getXButton(){
return m_stick.getRawButton(X_BUTTON);
}
public boolean getYButton(){
return m_stick.getRawButton(Y_BUTTON);
}
public boolean getBButton(){
return m_stick.getRawButton(B_BUTTON);
}
public boolean getBackButton(){
return m_stick.getRawButton(BACK_BUTTON);
}
public boolean getYButton(){
return m_stick.getRawButton(Y_BUTTON);
}
public boolean getStartButton(){
return m_stick.getRawButton(START_BUTTON);
}
public boolean getBackButton(){
return m_stick.getRawButton(BACK_BUTTON);
}
public boolean getLeftBumperButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
}
public boolean getStartButton(){
return m_stick.getRawButton(START_BUTTON);
}
public boolean getRightBumperButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
}
public boolean getLeftBumperButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
}
public boolean getLeftJoystickButton(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
}
public boolean getRightBumperButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
}
public boolean getRightJoystickButton(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
}
public boolean getLeftJoystickButton(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
}
public double getLeftXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
}
public boolean getRightJoystickButton(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
}
public double getLeftYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
}
public double getLeftXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
}
public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
}
public double getLeftYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
}
public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
}
public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
}
public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
}
public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
}
public double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
}
public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_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 double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
}
public boolean getDPadLeft(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
}
/**
* 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 getDPadRight(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
}
public boolean getDPadLeft(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
}
public boolean getDPadTop(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
}
public boolean getDPadRight(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
}
public boolean getDPadBottom(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
}
public boolean getDPadTop(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
}
public boolean getLeftTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
}
public boolean getDPadBottom(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
}
public boolean getRightTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
}
public boolean getLeftTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
}
public boolean getRightAxisUpTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
}
public boolean getRightTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
}
public boolean getRightAxisDownTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
}
public boolean getRightAxisUpTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
}
public boolean getRightAxisLeftTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
}
public boolean getRightAxisDownTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
}
public boolean getRightAxisRightTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
}
public boolean getRightAxisLeftTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
}
public boolean getLeftAxisUpTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
}
public boolean getRightAxisRightTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
}
public boolean getLeftAxisDownTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
}
public boolean getLeftAxisUpTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
}
public boolean getLeftAxisLeftTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
}
public boolean getLeftAxisDownTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
}
public boolean getLeftAxisLeftTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
}
public boolean getLeftAxisRightTrigger(){
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}.
*/
public class XboxTriggerButton {//extends Trigger {
public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
private XboxController m_controller;
private int m_trigger;
private XboxController m_controller;
private int m_trigger;
/**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/
public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller;
m_trigger = trigger;
/**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/
public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller;
m_trigger = trigger;
}
/** {@inheritDoc} */
// @Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();
}
/** {@inheritDoc} */
// @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 == 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;
}
}