diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt
index 70c79b6..f022c09 100644
--- a/src/main/deploy/example.txt
+++ b/src/main/deploy/example.txt
@@ -1,3 +1,3 @@
-Files placed in this directory will be deployed to the RoboRIO into the
-'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function
+Files placed in this directory will be deployed to the RoboRIO into the
+'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function
to get a proper path relative to the deploy directory.
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index af39c6a..e894a0f 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -1,178 +1,178 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot;
-
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.util.Units;
-import frc4388.utility.Gains;
-import frc4388.utility.LEDPatterns;
-import frc4388.utility.RobotUnits;
-
-/**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be
- * declared globally (i.e. public static). Do not put anything functional in this class.
- *
- * It is advised to statically import this class (or one of its inner classes) wherever the
- * constants are needed, to reduce verbosity.
- */
-public final class Constants {
- public static final class SwerveDriveConstants {
-
- public static final double MAX_ROT_SPEED = 1.5;
- public static final double MIN_ROT_SPEED = 0.8;
- public static double ROTATION_SPEED = MAX_ROT_SPEED;
- public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
-
- public static final double CORRECTION_MIN = 10;
- public static final double CORRECTION_MAX = 50;
-
- public static final double SLOW_SPEED = 0.8;
- public static final double FAST_SPEED = 1.0;
- public static final double TURBO_SPEED = 4.0;
-
- public static final class IDs {
- public static final int LEFT_FRONT_WHEEL_ID = 2;
- public static final int LEFT_FRONT_STEER_ID = 3;
- public static final int LEFT_FRONT_ENCODER_ID = 10;
-
- public static final int RIGHT_FRONT_WHEEL_ID = 4;
- public static final int RIGHT_FRONT_STEER_ID = 5;
- public static final int RIGHT_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 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 {
- public static final int SWERVE_SLOT_IDX = 0;
- public static final int SWERVE_PID_LOOP_IDX = 1;
- public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
- }
-
- public static final class AutoConstants {
- public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
- public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
- public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
- public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
-
- public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
- public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
- }
-
- public static final class Conversions {
- public static final int CANCODER_TICKS_PER_ROTATION = 4096;
-
- public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
- public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
-
- public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
- public static final double MOTOR_REV_PER_STEER_REV = 12.8;
-
- public static final double TICKS_PER_MOTOR_REV = 2048;
- public static final double WHEEL_DIAMETER_INCHES = 3.9;
- public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
-
- public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
- public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
- public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
- public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
-
- public static final double TICK_TIME_TO_SECONDS = 10;
- public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
- }
-
- public static final class Configurations {
- public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
- public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
- public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
- }
-
- public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
- public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
-
- // dimensions
- public static final double WIDTH = 18.5;
- public static final double HEIGHT = 18.5;
- public static final double HALF_WIDTH = WIDTH / 2.d;
- public static final double HALF_HEIGHT = HEIGHT / 2.d;
-
- // misc
- public static final int TIMEOUT_MS = 30;
- public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
- }
-
- public static final class GyroConstants {
- public static final int ID = 14;
- }
-
- public static final class ArmConstants {
- public static final double MIN_ARM_LEN = 1;
- public static final double MAX_ARM_LEN = 2;
- public static final double ARM_HEIGHT = 1;
- public static final double CURVE_POWER = 2;
-
- public static final double TELE_TICKS_PER_SECOND = (-5);
-
- public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value
- public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value
-
- public static final double TELE_FORWARD_SOFT_LIMIT = 100;
- public static final double TELE_REVERSE_SOFT_LIMIT = 0;
-
- public static final double kP = 0;
- public static final double kI = 0;
- public static final double kD = 0;
-
- public static final double OFFSET = 0;
- }
-
- public static final class LEDConstants {
- // public static final int LED_SPARK_ID = 0;
-
- public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
- }
-
- public static final class OIConstants {
- public static final int XBOX_DRIVER_ID = 0;
- public static final int XBOX_OPERATOR_ID = 1;
- public static final double LEFT_AXIS_DEADBAND = 0.1;
- public static final double RIGHT_AXIS_DEADBAND = 0.6;
- public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
- }
-
- public static final class VisionConstants {
- public static final String NAME = "photonCamera";
-
- public static final int LIME_HIXELS = 640;
- public static final int LIME_VIXELS = 480;
-
- public static final double H_FOV = 59.6;
- public static final double V_FOV = 45.7;
-
- public static final double LIME_HEIGHT = 6.0;
- public static final double LIME_ANGLE = 55.0;
-
- // public static final double HIGH_TARGET_HEIGHT = 46.0;
- public static final double HIGH_TAPE_HEIGHT = 44.0;
-
- // public static final double MID_TARGET_HEIGHT = 34.0;
- public static final double MID_TAPE_HEIGHT = 24.0;
-
- public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
-
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import frc4388.utility.Gains;
+import frc4388.utility.LEDPatterns;
+import frc4388.utility.RobotUnits;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be
+ * declared globally (i.e. public static). Do not put anything functional in this class.
+ *
+ *
It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+ public static final class SwerveDriveConstants {
+
+ public static final double MAX_ROT_SPEED = 1.5;
+ public static final double MIN_ROT_SPEED = 0.8;
+ public static double ROTATION_SPEED = MAX_ROT_SPEED;
+ public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
+
+ public static final double CORRECTION_MIN = 10;
+ public static final double CORRECTION_MAX = 50;
+
+ public static final double SLOW_SPEED = 0.8;
+ public static final double FAST_SPEED = 1.0;
+ public static final double TURBO_SPEED = 4.0;
+
+ public static final class IDs {
+ public static final int LEFT_FRONT_WHEEL_ID = 2;
+ public static final int LEFT_FRONT_STEER_ID = 3;
+ public static final int LEFT_FRONT_ENCODER_ID = 10;
+
+ public static final int RIGHT_FRONT_WHEEL_ID = 4;
+ public static final int RIGHT_FRONT_STEER_ID = 5;
+ public static final int RIGHT_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 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 {
+ public static final int SWERVE_SLOT_IDX = 0;
+ public static final int SWERVE_PID_LOOP_IDX = 1;
+ public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
+ }
+
+ public static final class AutoConstants {
+ public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+ public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
+ public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
+ public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
+
+ public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
+ public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
+ }
+
+ public static final class Conversions {
+ public static final int CANCODER_TICKS_PER_ROTATION = 4096;
+
+ public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
+ public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
+
+ public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
+ public static final double MOTOR_REV_PER_STEER_REV = 12.8;
+
+ public static final double TICKS_PER_MOTOR_REV = 2048;
+ public static final double WHEEL_DIAMETER_INCHES = 3.9;
+ public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
+
+ public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
+ public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
+ public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
+ public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
+
+ public static final double TICK_TIME_TO_SECONDS = 10;
+ public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
+ }
+
+ public static final class Configurations {
+ public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
+ public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
+ public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
+ }
+
+ public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
+ public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
+
+ // dimensions
+ public static final double WIDTH = 18.5;
+ public static final double HEIGHT = 18.5;
+ public static final double HALF_WIDTH = WIDTH / 2.d;
+ public static final double HALF_HEIGHT = HEIGHT / 2.d;
+
+ // misc
+ public static final int TIMEOUT_MS = 30;
+ public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
+ }
+
+ public static final class GyroConstants {
+ public static final int ID = 14;
+ }
+
+ public static final class ArmConstants {
+ public static final double MIN_ARM_LEN = 1;
+ public static final double MAX_ARM_LEN = 2;
+ public static final double ARM_HEIGHT = 1;
+ public static final double CURVE_POWER = 2;
+
+ public static final double TELE_TICKS_PER_SECOND = (-5);
+
+ public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value
+ public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value
+
+ public static final double TELE_FORWARD_SOFT_LIMIT = 100;
+ public static final double TELE_REVERSE_SOFT_LIMIT = 0;
+
+ public static final double kP = 0;
+ public static final double kI = 0;
+ public static final double kD = 0;
+
+ public static final double OFFSET = 0;
+ }
+
+ public static final class LEDConstants {
+ // public static final int LED_SPARK_ID = 0;
+
+ public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
+ }
+
+ public static final class OIConstants {
+ public static final int XBOX_DRIVER_ID = 0;
+ public static final int XBOX_OPERATOR_ID = 1;
+ public static final double LEFT_AXIS_DEADBAND = 0.1;
+ public static final double RIGHT_AXIS_DEADBAND = 0.6;
+ public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
+ }
+
+ public static final class VisionConstants {
+ public static final String NAME = "photonCamera";
+
+ public static final int LIME_HIXELS = 640;
+ public static final int LIME_VIXELS = 480;
+
+ public static final double H_FOV = 59.6;
+ public static final double V_FOV = 45.7;
+
+ public static final double LIME_HEIGHT = 6.0;
+ public static final double LIME_ANGLE = 55.0;
+
+ // public static final double HIGH_TARGET_HEIGHT = 46.0;
+ public static final double HIGH_TAPE_HEIGHT = 44.0;
+
+ // public static final double MID_TARGET_HEIGHT = 34.0;
+ public static final double MID_TAPE_HEIGHT = 24.0;
+
+ public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
+
+ }
+}
diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java
index ad2d494..9cfbc11 100644
--- a/src/main/java/frc4388/robot/Main.java
+++ b/src/main/java/frc4388/robot/Main.java
@@ -1,29 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
- private Main() {
- }
-
- /**
- * Main initialization function. Do not perform any initialization here.
- *
- *
If you change your main robot class, change the parameter type.
- */
- public static void main(String... args) {
- RobotBase.startRobot(Robot::new);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+ private Main() {
+ }
+
+ /**
+ * Main initialization function. Do not perform any initialization here.
+ *
+ *
If you change your main robot class, change the parameter type.
+ */
+ public static void main(String... args) {
+ RobotBase.startRobot(Robot::new);
+ }
+}
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index a63c659..e7b8afd 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -1,134 +1,134 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot;
-
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc4388.utility.DeferredBlock;
-import frc4388.utility.RobotTime;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
- * project.
- */
-public class Robot extends TimedRobot {
- Command m_autonomousCommand;
-
- private RobotTime m_robotTime = RobotTime.getInstance();
- private RobotContainer m_robotContainer;
-
- /**
- * 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();
-
- SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
- }
-
- /**
- * 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.
- *
- *
This runs after the mode specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
- */
- @Override
- public void robotPeriodic() {
- m_robotTime.updateTimes();
-
- // 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();
- super.disabledExit();
- }
-
- /**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
- */
- @Override
- public void autonomousInit() {
- m_robotContainer.m_robotSwerveDrive.resetGyro();
- m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
- // 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() {
- m_robotContainer.m_robotSwerveDrive.resetGyro();
-
- // 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_robotContainer.m_robotSwerveDrive.resetGyro();
- m_robotTime.startMatchTime();
-
- m_robotContainer.m_robotMap.restart_motor_tests();
- }
-
- /**
- * This function is called periodically during operator control.
- */
- @Override
- public void teleopPeriodic() {}
-
- /**
- * This function is called periodically during test mode.
- */
- @Override
- public void testPeriodic() {
- m_robotContainer.m_robotMap.run_periodic_tests();
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.DeferredBlock;
+import frc4388.utility.RobotTime;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ Command m_autonomousCommand;
+
+ private RobotTime m_robotTime = RobotTime.getInstance();
+ private RobotContainer m_robotContainer;
+
+ /**
+ * 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();
+
+ SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
+ }
+
+ /**
+ * 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.
+ *
+ *
This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+ @Override
+ public void robotPeriodic() {
+ m_robotTime.updateTimes();
+
+ // 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();
+ super.disabledExit();
+ }
+
+ /**
+ * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ */
+ @Override
+ public void autonomousInit() {
+ m_robotContainer.m_robotSwerveDrive.resetGyro();
+ m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+ // 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() {
+ m_robotContainer.m_robotSwerveDrive.resetGyro();
+
+ // 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_robotContainer.m_robotSwerveDrive.resetGyro();
+ m_robotTime.startMatchTime();
+
+ m_robotContainer.m_robotMap.restart_motor_tests();
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {}
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ m_robotContainer.m_robotMap.run_periodic_tests();
+ }
+}
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index cd728ce..9a0b737 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -1,350 +1,350 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot;
-
-import java.util.function.Consumer;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.REVLibError;
-import com.revrobotics.CANSparkMax.FaultID;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import edu.wpi.first.wpilibj2.command.button.POVButton;
-import frc4388.robot.Constants.*;
-import frc4388.robot.subsystems.Arm;
-import frc4388.robot.subsystems.Claw;
-import frc4388.robot.subsystems.Limelight;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.robot.commands.BooleanCommand;
-import frc4388.robot.commands.Arm.PivotCommand;
-import frc4388.robot.commands.Arm.TeleCommand;
-import frc4388.robot.commands.Autos.AutoBalance;
-import frc4388.robot.commands.Autos.PlaybackChooser;
-import frc4388.robot.commands.Placement.AprilRotAlign;
-import frc4388.robot.commands.Placement.LimeAlign;
-import frc4388.robot.commands.Swerve.RotateToAngle;
-import frc4388.utility.controller.DeadbandedXboxController;
-import frc4388.utility.controller.XboxController;
-
-/**
- * This class is where the bulk of the robot should be declared. Since
- * Command-based is a "declarative" paradigm, very little robot logic should
- * actually be handled in the {@link Robot} periodic methods (other than the
- * scheduler calls). Instead, the structure of the robot (including subsystems,
- * commands, and button mappings) should be declared here.
- */
-public class RobotContainer {
- /* RobotMap */
- public final RobotMap m_robotMap = new RobotMap();
-
- /* Subsystems */
- public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
- m_robotMap.rightFront,
- m_robotMap.leftBack,
- m_robotMap.rightBack,
- m_robotMap.gyro);
-
- public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
-
- public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin);
-
- public final Limelight m_robotLimeLight = new Limelight();
-
- /* Controllers */
- private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
- private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
-
- /* Autos */
- public SendableChooser chooser = new SendableChooser<>();
-
- // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
-
- // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
- // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
-
- // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
- // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
-
- // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
-
- private PlaybackChooser playbackChooser;
-
- /* Commands */
- private Command emptyCommand = new InstantCommand();
- private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight);
-
- private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135));
-
- private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
-
- private boolean readyForPlacement = false;
- private Boolean isPole = null;
-
- private SequentialCommandGroup alignToPole =
- new SequentialCommandGroup(
- new RotateToAngle(m_robotSwerveDrive, 0.0),
- new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
- new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
- new RotateToAngle(m_robotSwerveDrive, 0.0),
- new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
- // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
- ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
-
- // private SequentialCommandGroup alignToShelf =
- // new SequentialCommandGroup(
- // new RotateToAngle(m_robotSwerveDrive, 0.0),
- // new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
- // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
- // new RotateToAngle(m_robotSwerveDrive, 0.0),
- // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
- // ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
-
- private SequentialCommandGroup alignToShelf =
- new SequentialCommandGroup(
- new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
- new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
- new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
- new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
- new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
- ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
-
- public SequentialCommandGroup place = null;
-
- private Consumer queuePlacement = (scg) -> {
- place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
- };
-
- // TODO: find actual values
- private SequentialCommandGroup placeCubeHigh =
- new SequentialCommandGroup(
- // new InstantCommand(() -> System.out.println("Placing cone high")),
- new PivotCommand(m_robotArm, 64 + 135),
- new InstantCommand(() -> m_robotArm.setRotVel(0)),
- new TeleCommand(m_robotArm, 95642),
- toggleClaw.asProxy(),
- armToHome.asProxy()
- );
-
- private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(
- new PivotCommand(m_robotArm, 70 + 135),
- new TeleCommand(m_robotArm, 32866),
- toggleClaw.asProxy(),
- armToHome.asProxy()
- );
-
- private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
- new PivotCommand(m_robotArm, 0),
- new TeleCommand(m_robotArm, 0),
- toggleClaw.asProxy(),
- armToHome.asProxy()
- );
-
- private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
- new PivotCommand(m_robotArm, 0),
- new TeleCommand(m_robotArm, 0),
- toggleClaw.asProxy(),
- armToHome.asProxy()
- );
-
- private SequentialCommandGroup placeLow = new SequentialCommandGroup(
- new PivotCommand(m_robotArm, 0),
- new TeleCommand(m_robotArm, 0),
- toggleClaw.asProxy(),
- armToHome.asProxy()
- );
-
-
- /**
- * The container for the robot. Contains subsystems, OI devices, and commands.
- */
- public RobotContainer() {
- configureButtonBindings();
-
- // * Default Commands
- m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
- m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
- getDeadbandedDriverController().getRight(),
- true);
- }, m_robotSwerveDrive)
- .withName("SwerveDrive DefaultCommand"));
-
- m_robotArm.setDefaultCommand(new RunCommand(() -> {
- m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
- m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
- }, m_robotArm)
- .withName("Arm DefaultCommand"));
-
- // * Auto Commands
- chooser.setDefaultOption("NoAuto", emptyCommand);
- chooser.addOption("alignToPole", alignToPole);
- chooser.addOption("alignToShelf", alignToShelf);
-
- chooser.addOption("placeConeHigh", placeConeHigh);
- chooser.addOption("placeConeMid", placeConeMid);
- chooser.addOption("placeCubeHigh", placeCubeHigh);
- chooser.addOption("placeCubeMid", placeCubeMid);
- chooser.addOption("placeLow", placeLow);
-
- // chooser.addOption("Blue1Path", blue1Path);
- // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
-
- // chooser.addOption("Red1Path", red1Path);
- // chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
-
- playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
- .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
- .buildDisplay();
- }
-
- // here be dragons - enter if you dare
- private static class TapState {
- public boolean gearTapped = true;
- public long gearTime = 0;
- }
-
- /**
- * Use this method to define your button->command mappings. Buttons can be
- * created by instantiating a {@link GenericHID} or one of its subclasses
- * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
- * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
- */
- private void configureButtonBindings() {
- // * Driver Buttons
- new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
- .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
-
- // because closure reasons - ask Daniel Thomas
- // final TapState tap = new TapState();
- new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
- .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
- .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
- // .onTrue(new InstantCommand(() -> {
- // tap.gearTapped = true;
- // tap.gearTime = System.currentTimeMillis();
- // }))
- // .whileTrue(new RunCommand(() -> {
- // if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) {
- // m_robotSwerveDrive.setToTurbo();
- // tap.gearTapped = false;
- // }
- // }))
- // .onFalse(new InstantCommand(() -> {
- // if (tap.gearTapped)
- // m_robotSwerveDrive.setToFast();
- // else
- // m_robotSwerveDrive.setToSlow();
- // }));
-
- new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
- .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
-
- new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
- .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final
-
- new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
- .onTrue(interruptCommand.asProxy()); // final
- // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
- // .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
- // () -> getDeadbandedDriverController().getLeftX(),
- // () -> getDeadbandedDriverController().getLeftY(),
- // () -> getDeadbandedDriverController().getRightX(),
- // () -> getDeadbandedDriverController().getRightY(),
- // "Blue1Path.txt"))
- // .onFalse(new InstantCommand());
-
- // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
- // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
- // .onFalse(new InstantCommand());
-
- // * Operator Buttons
-
- // align (pole)
- new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final
- .onTrue(alignToPole)
- .onFalse(interruptCommand.asProxy());
-
- // align (shelf)
- new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final
- .onTrue(alignToShelf)
- .onFalse(interruptCommand.asProxy());
-
- // toggle claw
- new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
- .onTrue(toggleClaw.asProxy());
-
- // kill soft limits
- new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
- .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
-
- // SmartDashboard.putBoolean("isn't SparkMAX", );
-
- new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
- .onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
- .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
-
- //Arm to Home
- new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
- .onTrue(armToHome.asProxy());
-
- //Interupt Button
- new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
- .onTrue(interruptCommand.asProxy());
-
-
- // place high
- new POVButton(getDeadbandedOperatorController(), 0)
- .onTrue(new ConditionalCommand(
- new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true),
- emptyCommand.asProxy(),
- () -> readyForPlacement == true)
- );
-
- // place mid
- new POVButton(getDeadbandedOperatorController(), 270)
- .onTrue(new ConditionalCommand(
- new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true),
- emptyCommand.asProxy(),
- () -> readyForPlacement == true)
- );
-
- // place low
- new POVButton(getDeadbandedOperatorController(), 180)
- .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
-
- // confirm
- new POVButton(getDeadbandedOperatorController(), 90)
- .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null));
-
- }
-
- /**
- * Use this to pass the autonomous command to the main {@link Robot} class.
- *
- * @return the command to run in autonomous
- */
- public Command getAutonomousCommand() {
- // return chooser.getSelected();
- return playbackChooser.getCommand();
- }
-
- public DeadbandedXboxController getDeadbandedDriverController() {
- return this.m_driverXbox;
- }
-
- public DeadbandedXboxController getDeadbandedOperatorController() {
- return this.m_operatorXbox;
- }
-
-}
+x/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+import java.util.function.Consumer;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.REVLibError;
+import com.revrobotics.CANSparkMax.FaultID;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.POVButton;
+import frc4388.robot.Constants.*;
+import frc4388.robot.subsystems.Arm;
+import frc4388.robot.subsystems.Claw;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.commands.BooleanCommand;
+import frc4388.robot.commands.Arm.PivotCommand;
+import frc4388.robot.commands.Arm.TeleCommand;
+import frc4388.robot.commands.Autos.AutoBalance;
+import frc4388.robot.commands.Autos.PlaybackChooser;
+import frc4388.robot.commands.Placement.AprilRotAlign;
+import frc4388.robot.commands.Placement.LimeAlign;
+import frc4388.robot.commands.Swerve.RotateToAngle;
+import frc4388.utility.controller.DeadbandedXboxController;
+import frc4388.utility.controller.XboxController;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+ /* RobotMap */
+ public final RobotMap m_robotMap = new RobotMap();
+
+ /* Subsystems */
+ public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront,
+ m_robotMap.rightFront,
+ m_robotMap.leftBack,
+ m_robotMap.rightBack,
+ m_robotMap.gyro);
+
+ public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
+
+ public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin);
+
+ public final Limelight m_robotLimeLight = new Limelight();
+
+ /* Controllers */
+ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
+ private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
+
+ /* Autos */
+ public SendableChooser chooser = new SendableChooser<>();
+
+ // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
+
+ // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
+ // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
+
+ // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
+ // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
+
+ // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
+
+ private PlaybackChooser playbackChooser;
+
+ /* Commands */
+ private Command emptyCommand = new InstantCommand();
+ private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight);
+
+ private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135));
+
+ private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
+
+ private boolean readyForPlacement = false;
+ private Boolean isPole = null;
+
+ private SequentialCommandGroup alignToPole =
+ new SequentialCommandGroup(
+ new RotateToAngle(m_robotSwerveDrive, 0.0),
+ new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
+ new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
+ new RotateToAngle(m_robotSwerveDrive, 0.0),
+ new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
+ // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
+ ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
+
+ // private SequentialCommandGroup alignToShelf =
+ // new SequentialCommandGroup(
+ // new RotateToAngle(m_robotSwerveDrive, 0.0),
+ // new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
+ // new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
+ // new RotateToAngle(m_robotSwerveDrive, 0.0),
+ // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
+ // ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
+
+ private SequentialCommandGroup alignToShelf =
+ new SequentialCommandGroup(
+ new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
+ new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
+ new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
+ new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
+ new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
+ ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
+
+ public SequentialCommandGroup place = null;
+
+ private Consumer queuePlacement = (scg) -> {
+ place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
+ };
+
+ // TODO: find actual values
+ private SequentialCommandGroup placeCubeHigh =
+ new SequentialCommandGroup(
+ // new InstantCommand(() -> System.out.println("Placing cone high")),
+ new PivotCommand(m_robotArm, 64 + 135),
+ new InstantCommand(() -> m_robotArm.setRotVel(0)),
+ new TeleCommand(m_robotArm, 95642),
+ toggleClaw.asProxy(),
+ armToHome.asProxy()
+ );
+
+ private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(
+ new PivotCommand(m_robotArm, 70 + 135),
+ new TeleCommand(m_robotArm, 32866),
+ toggleClaw.asProxy(),
+ armToHome.asProxy()
+ );
+
+ private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
+ new PivotCommand(m_robotArm, 0),
+ new TeleCommand(m_robotArm, 0),
+ toggleClaw.asProxy(),
+ armToHome.asProxy()
+ );
+
+ private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
+ new PivotCommand(m_robotArm, 0),
+ new TeleCommand(m_robotArm, 0),
+ toggleClaw.asProxy(),
+ armToHome.asProxy()
+ );
+
+ private SequentialCommandGroup placeLow = new SequentialCommandGroup(
+ new PivotCommand(m_robotArm, 0),
+ new TeleCommand(m_robotArm, 0),
+ toggleClaw.asProxy(),
+ armToHome.asProxy()
+ );
+
+
+ /**
+ * The container for the robot. Contains subsystems, OI devices, and commands.
+ */
+ public RobotContainer() {
+ configureButtonBindings();
+
+ // * Default Commands
+ m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+ m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
+ getDeadbandedDriverController().getRight(),
+ true);
+ }, m_robotSwerveDrive)
+ .withName("SwerveDrive DefaultCommand"));
+
+ m_robotArm.setDefaultCommand(new RunCommand(() -> {
+ m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
+ m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
+ }, m_robotArm)
+ .withName("Arm DefaultCommand"));
+
+ // * Auto Commands
+ chooser.setDefaultOption("NoAuto", emptyCommand);
+ chooser.addOption("alignToPole", alignToPole);
+ chooser.addOption("alignToShelf", alignToShelf);
+
+ chooser.addOption("placeConeHigh", placeConeHigh);
+ chooser.addOption("placeConeMid", placeConeMid);
+ chooser.addOption("placeCubeHigh", placeCubeHigh);
+ chooser.addOption("placeCubeMid", placeCubeMid);
+ chooser.addOption("placeLow", placeLow);
+
+ // chooser.addOption("Blue1Path", blue1Path);
+ // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
+
+ // chooser.addOption("Red1Path", red1Path);
+ // chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
+
+ playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
+ .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
+ .buildDisplay();
+ }
+
+ // here be dragons - enter if you dare
+ private static class TapState {
+ public boolean gearTapped = true;
+ public long gearTime = 0;
+ }
+
+ /**
+ * Use this method to define your button->command mappings. Buttons can be
+ * created by instantiating a {@link GenericHID} or one of its subclasses
+ * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+ * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+ */
+ private void configureButtonBindings() {
+ // * Driver Buttons
+ new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+ .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
+
+ // because closure reasons - ask Daniel Thomas
+ // final TapState tap = new TapState();
+ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
+ .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
+ .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
+ // .onTrue(new InstantCommand(() -> {
+ // tap.gearTapped = true;
+ // tap.gearTime = System.currentTimeMillis();
+ // }))
+ // .whileTrue(new RunCommand(() -> {
+ // if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) {
+ // m_robotSwerveDrive.setToTurbo();
+ // tap.gearTapped = false;
+ // }
+ // }))
+ // .onFalse(new InstantCommand(() -> {
+ // if (tap.gearTapped)
+ // m_robotSwerveDrive.setToFast();
+ // else
+ // m_robotSwerveDrive.setToSlow();
+ // }));
+
+ new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
+ .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
+
+ new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+ .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final
+
+ new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+ .onTrue(interruptCommand.asProxy()); // final
+ // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+ // .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
+ // () -> getDeadbandedDriverController().getLeftX(),
+ // () -> getDeadbandedDriverController().getLeftY(),
+ // () -> getDeadbandedDriverController().getRightX(),
+ // () -> getDeadbandedDriverController().getRightY(),
+ // "Blue1Path.txt"))
+ // .onFalse(new InstantCommand());
+
+ // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+ // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
+ // .onFalse(new InstantCommand());
+
+ // * Operator Buttons
+
+ // align (pole)
+ new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final
+ .onTrue(alignToPole)
+ .onFalse(interruptCommand.asProxy());
+
+ // align (shelf)
+ new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final
+ .onTrue(alignToShelf)
+ .onFalse(interruptCommand.asProxy());
+
+ // toggle claw
+ new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
+ .onTrue(toggleClaw.asProxy());
+
+ // kill soft limits
+ new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
+ .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
+
+ // SmartDashboard.putBoolean("isn't SparkMAX", );
+
+ new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
+ .onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
+ .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
+
+ //Arm to Home
+ new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
+ .onTrue(armToHome.asProxy());
+
+ //Interupt Button
+ new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
+ .onTrue(interruptCommand.asProxy());
+
+
+ // place high
+ new POVButton(getDeadbandedOperatorController(), 0)
+ .onTrue(new ConditionalCommand(
+ new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true),
+ emptyCommand.asProxy(),
+ () -> readyForPlacement == true)
+ );
+
+ // place mid
+ new POVButton(getDeadbandedOperatorController(), 270)
+ .onTrue(new ConditionalCommand(
+ new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true),
+ emptyCommand.asProxy(),
+ () -> readyForPlacement == true)
+ );
+
+ // place low
+ new POVButton(getDeadbandedOperatorController(), 180)
+ .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
+
+ // confirm
+ new POVButton(getDeadbandedOperatorController(), 90)
+ .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null));
+
+ }
+
+ /**
+ * Use this to pass the autonomous command to the main {@link Robot} class.
+ *
+ * @return the command to run in autonomous
+ */
+ public Command getAutonomousCommand() {
+ // return chooser.getSelected();
+ return playbackChooser.getCommand();
+ }
+
+ public DeadbandedXboxController getDeadbandedDriverController() {
+ return this.m_driverXbox;
+ }
+
+ public DeadbandedXboxController getDeadbandedOperatorController() {
+ return this.m_operatorXbox;
+ }
+
+}
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 113cf05..d8a49ce 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -1,279 +1,279 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot;
-
-import java.lang.reflect.Field;
-import java.lang.reflect.Member;
-import java.util.ArrayList;
-
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj.PWM;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc4388.robot.Constants.ArmConstants;
-import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.robot.subsystems.SwerveModule;
-import frc4388.utility.RobotGyro;
-
-/**
- * Defines and holds all I/O objects on the Roborio. This is useful for unit
- * testing and modularization.
- */
-public class RobotMap {
- private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
- public RobotGyro gyro = new RobotGyro(m_pigeon2);
-
-
- public SwerveModule leftFront;
- public SwerveModule rightFront;
- public SwerveModule leftBack;
- public SwerveModule rightBack;
-
- public RobotMap() {
- configureLEDMotorControllers();
- configureDriveMotors();
- configArmMotors();
-
- try {
- setup_motor_tests();
- } catch (IllegalArgumentException | IllegalAccessException e) {
- // TODO Auto-generated catch block
- e.printStackTrace();
- }
- }
-
- /* LED Subsystem */
- // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
-
- void configureLEDMotorControllers() {
-
- }
-
- // swerve drive subsystem
- public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
- public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
- public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
-
- public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
- public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
- public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
-
- public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
- public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
- public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
-
- public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
- public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
- public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
-
- void configureDriveMotors() {
- // config factory default
- leftFrontWheel.configFactoryDefault();
- leftFrontSteer.configFactoryDefault();
-
- rightFrontWheel.configFactoryDefault();
- rightFrontSteer.configFactoryDefault();
-
- leftBackWheel.configFactoryDefault();
- leftBackSteer.configFactoryDefault();
-
- rightBackWheel.configFactoryDefault();
- rightBackSteer.configFactoryDefault();
-
- // config open loop ramp
- leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- // config closed loop ramp
- leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- // config neutral deadband
- leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
- leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
-
- rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
- rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
-
- leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
- leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
-
- rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
- rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
-
- // set neutral mode
- leftFrontWheel.setNeutralMode(NeutralMode.Brake);
- rightFrontWheel.setNeutralMode(NeutralMode.Brake);
- leftBackWheel.setNeutralMode(NeutralMode.Brake);
- rightBackWheel.setNeutralMode(NeutralMode.Brake);
-
- leftFrontSteer.setNeutralMode(NeutralMode.Brake);
- rightFrontSteer.setNeutralMode(NeutralMode.Brake);
- leftBackSteer.setNeutralMode(NeutralMode.Brake);
- rightBackSteer.setNeutralMode(NeutralMode.Brake);
-
- // initialize SwerveModules
- this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
- this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
- this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
- this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
- }
-
- // arm stuff
- public WPI_TalonFX pivot = new WPI_TalonFX(15);
- public WPI_TalonFX tele = new WPI_TalonFX(16);
- public CANCoder pivotEncoder = new CANCoder(17);
-
- public void configArmMotors() {
- // config factory default
- pivot.configFactoryDefault();
- tele.configFactoryDefault();
-
- // config open loop ramp
- pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- // config closed loop ramp
- pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
- tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
-
- // config neutral mode to brake
- pivot.setNeutralMode(NeutralMode.Brake);
- tele.setNeutralMode(NeutralMode.Brake);
-
- // soft limits
- pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
- pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
- pivot.configForwardSoftLimitEnable(false);
- pivot.configReverseSoftLimitEnable(false);
-
- tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
- tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
- tele.configForwardSoftLimitEnable(false);
- tele.configReverseSoftLimitEnable(false);
- }
-
- // claw stuff (WHAT IS A SOAP ENGINEER)
- PWM leftClaw = new PWM(0);
- PWM rightClaw = new PWM(1);
- CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
-
- // ============================================
- // =============== AUTO TESTING ===============
- // ============================================
-
- static class MotorTest {
- boolean result;
- String name;
-
- enum MotorType {FALCON, NEO}
-
- // why no union :(
- MotorType motor_type;
- WPI_TalonFX as_falcon;
- CANSparkMax as_neo;
- }
-
- private static final long TEST_TIME = 500;
-
- public ArrayList motor_tests = new ArrayList<>();
- private int test_count = 0;
- private long test_time = 0;
- private double motor_position = 0;
-
- public void restart_motor_tests() {
- for (MotorTest test : motor_tests) {
- test.result = false;
- }
-
- test_time = 0;
- test_count = 0;
- motor_position = 0;
- }
-
- public boolean run_periodic_tests() {
- if (test_count >= motor_tests.size()) return true;
-
- MotorTest current_test = motor_tests.get(test_count);
-
- if (test_time == 0) {
- test_time = System.currentTimeMillis();
-
- switch (current_test.motor_type) {
- case FALCON:
- motor_position = current_test.as_falcon.getSelectedSensorPosition();
- current_test.as_falcon.set(.1);
- break;
-
- case NEO:
- // TODO: destroy robot
- break;
- }
- }
-
- if (System.currentTimeMillis() - test_time >= TEST_TIME) {
- switch (current_test.motor_type) {
- case FALCON:
- {
- double new_pos = current_test.as_falcon.getSelectedSensorPosition();
- if (Math.abs(new_pos - motor_position) > .1) {
- current_test.result = true;
- } else {
- System.out.printf("[DISCONNECTED] %s\n", current_test.name);
- }
-
- break;
- }
-
- case NEO: break;
- }
-
- test_time = 0;
- test_count++;
- }
-
- return false;
- }
-
- private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException {
- Class map_clazz = this.getClass();
-
- for (Field field : map_clazz.getFields()) {
- if (field.getType().equals(WPI_TalonFX.class)) {
- MotorTest test = new MotorTest();
- test.result = false;
- test.name = field.getName();
- test.motor_type = MotorTest.MotorType.FALCON;
- test.as_falcon = (WPI_TalonFX) field.get(this);
- }
- }
- }
-}
\ No newline at end of file
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot;
+
+import java.lang.reflect.Field;
+import java.lang.reflect.Member;
+import java.util.ArrayList;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.Constants.ArmConstants;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.subsystems.SwerveModule;
+import frc4388.utility.RobotGyro;
+
+/**
+ * Defines and holds all I/O objects on the Roborio. This is useful for unit
+ * testing and modularization.
+ */
+public class RobotMap {
+ private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
+ public RobotGyro gyro = new RobotGyro(m_pigeon2);
+
+
+ public SwerveModule leftFront;
+ public SwerveModule rightFront;
+ public SwerveModule leftBack;
+ public SwerveModule rightBack;
+
+ public RobotMap() {
+ configureLEDMotorControllers();
+ configureDriveMotors();
+ configArmMotors();
+
+ try {
+ setup_motor_tests();
+ } catch (IllegalArgumentException | IllegalAccessException e) {
+ // TODO Auto-generated catch block
+ e.printStackTrace();
+ }
+ }
+
+ /* LED Subsystem */
+ // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+
+ void configureLEDMotorControllers() {
+
+ }
+
+ // swerve drive subsystem
+ public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
+ public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
+ public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
+
+ public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
+ public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
+ public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
+
+ public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
+ public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
+ public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
+
+ public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
+ public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
+ public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
+
+ void configureDriveMotors() {
+ // config factory default
+ leftFrontWheel.configFactoryDefault();
+ leftFrontSteer.configFactoryDefault();
+
+ rightFrontWheel.configFactoryDefault();
+ rightFrontSteer.configFactoryDefault();
+
+ leftBackWheel.configFactoryDefault();
+ leftBackSteer.configFactoryDefault();
+
+ rightBackWheel.configFactoryDefault();
+ rightBackSteer.configFactoryDefault();
+
+ // config open loop ramp
+ leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ // config closed loop ramp
+ leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ // config neutral deadband
+ leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+ leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+ rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+ rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+ leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+ leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+ rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+ rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
+
+ // set neutral mode
+ leftFrontWheel.setNeutralMode(NeutralMode.Brake);
+ rightFrontWheel.setNeutralMode(NeutralMode.Brake);
+ leftBackWheel.setNeutralMode(NeutralMode.Brake);
+ rightBackWheel.setNeutralMode(NeutralMode.Brake);
+
+ leftFrontSteer.setNeutralMode(NeutralMode.Brake);
+ rightFrontSteer.setNeutralMode(NeutralMode.Brake);
+ leftBackSteer.setNeutralMode(NeutralMode.Brake);
+ rightBackSteer.setNeutralMode(NeutralMode.Brake);
+
+ // initialize SwerveModules
+ this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
+ this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
+ this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
+ this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
+ }
+
+ // arm stuff
+ public WPI_TalonFX pivot = new WPI_TalonFX(15);
+ public WPI_TalonFX tele = new WPI_TalonFX(16);
+ public CANCoder pivotEncoder = new CANCoder(17);
+
+ public void configArmMotors() {
+ // config factory default
+ pivot.configFactoryDefault();
+ tele.configFactoryDefault();
+
+ // config open loop ramp
+ pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ // config closed loop ramp
+ pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+ tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
+
+ // config neutral mode to brake
+ pivot.setNeutralMode(NeutralMode.Brake);
+ tele.setNeutralMode(NeutralMode.Brake);
+
+ // soft limits
+ pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
+ pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
+ pivot.configForwardSoftLimitEnable(false);
+ pivot.configReverseSoftLimitEnable(false);
+
+ tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+ tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+ tele.configForwardSoftLimitEnable(false);
+ tele.configReverseSoftLimitEnable(false);
+ }
+
+ // claw stuff (WHAT IS A SOAP ENGINEER)
+ PWM leftClaw = new PWM(0);
+ PWM rightClaw = new PWM(1);
+ CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
+
+ // ============================================
+ // =============== AUTO TESTING ===============
+ // ============================================
+
+ static class MotorTest {
+ boolean result;
+ String name;
+
+ enum MotorType {FALCON, NEO}
+
+ // why no union :(
+ MotorType motor_type;
+ WPI_TalonFX as_falcon;
+ CANSparkMax as_neo;
+ }
+
+ private static final long TEST_TIME = 500;
+
+ public ArrayList motor_tests = new ArrayList<>();
+ private int test_count = 0;
+ private long test_time = 0;
+ private double motor_position = 0;
+
+ public void restart_motor_tests() {
+ for (MotorTest test : motor_tests) {
+ test.result = false;
+ }
+
+ test_time = 0;
+ test_count = 0;
+ motor_position = 0;
+ }
+
+ public boolean run_periodic_tests() {
+ if (test_count >= motor_tests.size()) return true;
+
+ MotorTest current_test = motor_tests.get(test_count);
+
+ if (test_time == 0) {
+ test_time = System.currentTimeMillis();
+
+ switch (current_test.motor_type) {
+ case FALCON:
+ motor_position = current_test.as_falcon.getSelectedSensorPosition();
+ current_test.as_falcon.set(.1);
+ break;
+
+ case NEO:
+ // TODO: destroy robot
+ break;
+ }
+ }
+
+ if (System.currentTimeMillis() - test_time >= TEST_TIME) {
+ switch (current_test.motor_type) {
+ case FALCON:
+ {
+ double new_pos = current_test.as_falcon.getSelectedSensorPosition();
+ if (Math.abs(new_pos - motor_position) > .1) {
+ current_test.result = true;
+ } else {
+ System.out.printf("[DISCONNECTED] %s\n", current_test.name);
+ }
+
+ break;
+ }
+
+ case NEO: break;
+ }
+
+ test_time = 0;
+ test_count++;
+ }
+
+ return false;
+ }
+
+ private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException {
+ Class map_clazz = this.getClass();
+
+ for (Field field : map_clazz.getFields()) {
+ if (field.getType().equals(WPI_TalonFX.class)) {
+ MotorTest test = new MotorTest();
+ test.result = false;
+ test.name = field.getName();
+ test.motor_type = MotorTest.MotorType.FALCON;
+ test.as_falcon = (WPI_TalonFX) field.get(this);
+ }
+ }
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java b/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java
index 279e1b0..70fed4d 100644
--- a/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java
+++ b/src/main/java/frc4388/robot/commands/Arm/PivotCommand.java
@@ -1,33 +1,33 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Arm;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc4388.robot.commands.PelvicInflammatoryDisease;
-import frc4388.robot.subsystems.Arm;
-
-public class PivotCommand extends PelvicInflammatoryDisease {
- private final Arm arm;
- private final double target;
-
- /** Creates a new ArmCommand. */
- public PivotCommand(Arm arm, double target) {
- super(6, 1.5, 0, 0, 0.015);
- this.arm = arm;
- this.target = target;
- addRequirements(arm);
- }
-
- @Override
- public double getError() {
- return (target - arm.getArmRotation()) / 360;
- }
-
- @Override
- public void runWithOutput(double output) {
- SmartDashboard.putNumber("pivot output", output);
- arm.setRotVel(output);
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Arm;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Arm;
+
+public class PivotCommand extends PelvicInflammatoryDisease {
+ private final Arm arm;
+ private final double target;
+
+ /** Creates a new ArmCommand. */
+ public PivotCommand(Arm arm, double target) {
+ super(6, 1.5, 0, 0, 0.015);
+ this.arm = arm;
+ this.target = target;
+ addRequirements(arm);
+ }
+
+ @Override
+ public double getError() {
+ return (target - arm.getArmRotation()) / 360;
+ }
+
+ @Override
+ public void runWithOutput(double output) {
+ SmartDashboard.putNumber("pivot output", output);
+ arm.setRotVel(output);
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java
index a5a6e9f..1bb8978 100644
--- a/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java
+++ b/src/main/java/frc4388/robot/commands/Arm/TeleCommand.java
@@ -1,39 +1,39 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Arm;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.Constants.ArmConstants;
-import frc4388.robot.commands.PelvicInflammatoryDisease;
-import frc4388.robot.subsystems.Arm;
-
-public class TeleCommand extends CommandBase {
- private final Arm arm;
- private final double target;
- private boolean goIn;
-
- /** Creates a new ArmCommand. */
- public TeleCommand(Arm arm, double target) {
- this.arm = arm;
- this.target = target;
- addRequirements(arm);
- }
-
- @Override
- public void initialize() {
- this.goIn = target < arm.getArmLength();
- }
-
- @Override
- public void execute() {
- arm.setTeleVel(goIn ? 1 : -1);
- }
-
- @Override
- public boolean isFinished() {
- if (goIn) return arm.getArmLength() < target;
- else return arm.getArmLength() > target;
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+ package frc4388.robot.commands.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ArmConstants;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Arm;
+
+public class TeleCommand extends CommandBase {
+ private final Arm arm;
+ private final double target;
+ private boolean goIn;
+
+ /** Creates a new ArmCommand. */
+ public TeleCommand(Arm arm, double target) {
+ this.arm = arm;
+ this.target = target;
+ addRequirements(arm);
+ }
+
+ @Override
+ public void initialize() {
+ this.goIn = target < arm.getArmLength();
+ }
+
+ @Override
+ public void execute() {
+ arm.setTeleVel(goIn ? 1 : -1);
+ }
+
+ @Override
+ public boolean isFinished() {
+ if (goIn) return arm.getArmLength() < target;
+ else return arm.getArmLength() > target;
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java
index 4d18c99..cdb278f 100644
--- a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java
+++ b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java
@@ -1,42 +1,42 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Autos;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc4388.robot.commands.PelvicInflammatoryDisease;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.RobotGyro;
-
-public class AutoBalance extends PelvicInflammatoryDisease {
- RobotGyro gyro;
- SwerveDrive drive;
-
- /** Creates a new AutoBalance. */
- public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
- super(0.6, 0, 0, 0, 0);
-
- this.gyro = gyro;
- this.drive = drive;
-
- addRequirements(drive);
- }
-
- @Override
- 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, -.5, .5);
-
- if (Math.abs(getError()) < 3) out2 = 0;
- drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Autos;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.RobotGyro;
+
+public class AutoBalance extends PelvicInflammatoryDisease {
+ RobotGyro gyro;
+ SwerveDrive drive;
+
+ /** Creates a new AutoBalance. */
+ public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
+ super(0.6, 0, 0, 0, 0);
+
+ this.gyro = gyro;
+ this.drive = drive;
+
+ addRequirements(drive);
+ }
+
+ @Override
+ 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, -.5, .5);
+
+ if (Math.abs(getError()) < 3) out2 = 0;
+ drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt b/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt
index 70d9ee7..4f49b0e 100644
--- a/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt
+++ b/src/main/java/frc4388/robot/commands/Autos/Blue1Path.txt
@@ -1,150 +1,150 @@
-0.0,0.0,0.0,0.0,0
-0.0,0.0,0.0,0.0,2
-0.0,0.0,0.0,0.0,18
-0.0,0.0,0.0,0.0,31
-0.0,0.0,0.0,0.0,61
-0.0,0.0,0.0,0.0,72
-0.0,0.0,0.0,0.0,85
-0.0,0.0,0.0,0.0,102
-0.0,0.0,0.0,0.0,116
-0.0,0.0,0.0,0.0,163
-0.0,0.0,0.0,0.0,175
-0.0,0.0,0.0,0.0,188
-0.0,0.0,0.0,0.0,205
-0.0,0.0,0.0,0.0,217
-0.0,0.0,0.0,0.0,229
-0.0,0.0,0.0,0.0,241
-0.0,0.0,0.0,0.0,261
-0.0,0.0,0.0,0.0,281
-0.0,0.0,0.0,0.0,302
-0.0,0.0,0.0,0.0,322
-0.0,0.0,0.0,0.0,341
-0.0,0.0,0.0,0.0,361
-0.0,-0.1328125,0.0,0.0,397
-0.0,-0.1328125,0.0,0.0,412
-0.0,-0.1484375,0.0,0.0,425
-0.0,-0.1796875,0.0,0.0,441
-0.0,-0.1875,0.0,0.0,461
-0.0,-0.1953125,0.0,0.0,481
-0.0,-0.234375,0.0,0.0,502
-0.0,-0.2890625,0.0,0.0,521
-0.0,-0.3125,0.0,0.0,541
-0.0,-0.328125,0.0,0.0,561
-0.0,-0.3671875,0.0,0.0,582
-0.0,-0.390625,0.0,0.0,601
-0.0,-0.4609375,0.0,0.0,642
-0.0,-0.4765625,0.0,0.0,653
-0.0,-0.4765625,0.0,0.0,666
-0.0,-0.4765625,0.0,0.0,681
-0.0,-0.4765625,0.0,0.0,702
-0.0,-0.4765625,0.0,0.0,721
-0.0,-0.4765625,0.0,0.0,741
-0.0,-0.4765625,0.0,0.0,762
-0.0,-0.4765625,0.0,0.0,782
-0.0,-0.484375,0.0,0.0,803
-0.0,-0.484375,0.0,0.0,821
-0.0,-0.4921875,0.0,0.0,842
-0.0,-0.5078125,0.0,0.0,878
-0.0,-0.5234375,0.0,0.0,893
-0.0,-0.5234375,0.0,0.0,906
-0.0,-0.53125,0.0,0.0,922
-0.0,-0.53125,0.0,0.0,942
-0.0,-0.53125,0.0,0.0,962
-0.0,-0.53125,0.0,0.0,982
-0.0,-0.5390625,0.0,0.0,1001
-0.0,-0.5390625,0.0,0.0,1022
-0.0,-0.546875,0.0,0.0,1042
-0.0,-0.546875,0.0,0.0,1061
-0.0,-0.546875,0.0,0.0,1082
-0.0,-0.546875,0.0,0.0,1164
-0.0,-0.546875,0.0,0.0,1176
-0.0,-0.546875,0.0,0.0,1190
-0.0,-0.546875,0.0,0.0,1202
-0.0,-0.546875,0.0,0.0,1219
-0.0,-0.546875,0.0,0.0,1230
-0.0,-0.546875,0.0,0.0,1244
-0.0,-0.546875,0.0,0.0,1258
-0.0,-0.546875,0.0,0.0,1268
-0.0,-0.546875,0.0,0.0,1281
-0.0,-0.546875,0.0,0.0,1301
-0.0,-0.546875,0.0,0.0,1321
-0.0,-0.546875,0.0,0.0,1363
-0.0,-0.546875,0.0,0.0,1376
-0.0,-0.546875,0.0,0.0,1388
-0.0,-0.546875,0.0,0.0,1402
-0.0,-0.546875,0.0,0.0,1422
-0.0,-0.546875,0.0,0.0,1441
-0.0,-0.546875,0.0,0.0,1461
-0.0,-0.546875,0.0,0.0,1482
-0.0,-0.546875,0.0,0.0,1502
-0.0,-0.546875,0.0,0.0,1521
-0.0,-0.546875,0.0,0.0,1542
-0.0,-0.546875,0.0,0.0,1562
-0.0,-0.546875,0.0,0.0,1598
-0.0,-0.546875,0.0,0.0,1609
-0.0,-0.546875,0.0,0.0,1621
-0.0,-0.546875,0.0,0.0,1642
-0.0,-0.546875,0.0,0.0,1661
-0.0,-0.546875,0.0,0.0,1682
-0.0,-0.546875,0.0,0.0,1702
-0.0,-0.546875,0.0,0.0,1722
-0.0,-0.546875,0.0,0.0,1742
-0.0,-0.546875,0.0,0.0,1762
-0.0,-0.546875,0.0,0.0,1781
-0.0,-0.5390625,0.0,0.0,1801
-0.0,-0.5390625,0.0,0.0,1835
-0.0,-0.5390625,0.0,0.0,1847
-0.0,-0.5390625,0.0,0.0,1861
-0.0,-0.5390625,0.0,0.0,1882
-0.0,-0.5390625,0.0,0.0,1901
-0.0,-0.5390625,0.0,0.0,1921
-0.0,-0.5390625,0.0,0.0,1941
-0.0,-0.5390625,0.0,0.0,1962
-0.0,-0.5390625,0.0,0.0,1983
-0.0,-0.5390625,0.0,0.0,2001
-0.0,-0.5390625,0.0,0.0,2022
-0.0,-0.5390625,0.0,0.0,2042
-0.0,-0.5390625,0.0,0.0,2085
-0.0,-0.5390625,0.0,0.0,2097
-0.0,-0.5390625,0.0,0.0,2113
-0.0,-0.5390625,0.0,0.0,2124
-0.0,-0.5390625,0.0,0.0,2141
-0.0,-0.5390625,0.0,0.0,2161
-0.0,-0.5390625,0.0,0.0,2181
-0.0,-0.5390625,0.0,0.0,2201
-0.0,-0.5390625,0.0,0.0,2221
-0.0,-0.5390625,0.0,0.0,2241
-0.0,-0.5390625,0.0,0.0,2262
-0.0,-0.5390625,0.0,0.0,2283
-0.0,-0.2265625,0.0,0.0,2377
-0.0,-0.2265625,0.0,0.0,2390
-0.0,0.0,0.0,0.0,2402
-0.0,0.0,0.0,0.0,2417
-0.0,0.0,0.0,0.0,2428
-0.0,0.0,0.0,0.0,2440
-0.0,0.0,0.0,0.0,2451
-0.0,0.0,0.0,0.0,2463
-0.0,0.0,0.0,0.0,2475
-0.0,0.0,0.0,0.0,2493
-0.0,0.0,0.0,0.0,2505
-0.0,0.0,0.0,0.0,2521
-0.0,0.0,0.0,0.0,2558
-0.0,0.0,0.0,0.0,2576
-0.0,0.0,0.0,0.0,2592
-0.0,0.0,0.0,0.0,2603
-0.0,0.0,0.0,0.0,2621
-0.0,0.0,0.0,0.0,2641
-0.0,0.0,0.0,0.0,2661
-0.0,0.0,0.0,0.0,2681
-0.0,0.0,0.0,0.0,2702
-0.0,0.0,0.0,0.0,2721
-0.0,0.0,0.0,0.0,2742
-0.0,0.0,0.0,0.0,2762
-0.0,0.0,0.0,0.0,2805
-0.0,0.0,0.0,0.0,2814
-0.0,0.0,0.0,0.0,2825
-0.0,0.0,0.0,0.0,2841
-0.0,0.0,0.0,0.0,2861
-0.0,0.0,0.0,0.0,2882
-0.0,0.0,0.0,0.0,2901
+0.0,0.0,0.0,0.0,0
+0.0,0.0,0.0,0.0,2
+0.0,0.0,0.0,0.0,18
+0.0,0.0,0.0,0.0,31
+0.0,0.0,0.0,0.0,61
+0.0,0.0,0.0,0.0,72
+0.0,0.0,0.0,0.0,85
+0.0,0.0,0.0,0.0,102
+0.0,0.0,0.0,0.0,116
+0.0,0.0,0.0,0.0,163
+0.0,0.0,0.0,0.0,175
+0.0,0.0,0.0,0.0,188
+0.0,0.0,0.0,0.0,205
+0.0,0.0,0.0,0.0,217
+0.0,0.0,0.0,0.0,229
+0.0,0.0,0.0,0.0,241
+0.0,0.0,0.0,0.0,261
+0.0,0.0,0.0,0.0,281
+0.0,0.0,0.0,0.0,302
+0.0,0.0,0.0,0.0,322
+0.0,0.0,0.0,0.0,341
+0.0,0.0,0.0,0.0,361
+0.0,-0.1328125,0.0,0.0,397
+0.0,-0.1328125,0.0,0.0,412
+0.0,-0.1484375,0.0,0.0,425
+0.0,-0.1796875,0.0,0.0,441
+0.0,-0.1875,0.0,0.0,461
+0.0,-0.1953125,0.0,0.0,481
+0.0,-0.234375,0.0,0.0,502
+0.0,-0.2890625,0.0,0.0,521
+0.0,-0.3125,0.0,0.0,541
+0.0,-0.328125,0.0,0.0,561
+0.0,-0.3671875,0.0,0.0,582
+0.0,-0.390625,0.0,0.0,601
+0.0,-0.4609375,0.0,0.0,642
+0.0,-0.4765625,0.0,0.0,653
+0.0,-0.4765625,0.0,0.0,666
+0.0,-0.4765625,0.0,0.0,681
+0.0,-0.4765625,0.0,0.0,702
+0.0,-0.4765625,0.0,0.0,721
+0.0,-0.4765625,0.0,0.0,741
+0.0,-0.4765625,0.0,0.0,762
+0.0,-0.4765625,0.0,0.0,782
+0.0,-0.484375,0.0,0.0,803
+0.0,-0.484375,0.0,0.0,821
+0.0,-0.4921875,0.0,0.0,842
+0.0,-0.5078125,0.0,0.0,878
+0.0,-0.5234375,0.0,0.0,893
+0.0,-0.5234375,0.0,0.0,906
+0.0,-0.53125,0.0,0.0,922
+0.0,-0.53125,0.0,0.0,942
+0.0,-0.53125,0.0,0.0,962
+0.0,-0.53125,0.0,0.0,982
+0.0,-0.5390625,0.0,0.0,1001
+0.0,-0.5390625,0.0,0.0,1022
+0.0,-0.546875,0.0,0.0,1042
+0.0,-0.546875,0.0,0.0,1061
+0.0,-0.546875,0.0,0.0,1082
+0.0,-0.546875,0.0,0.0,1164
+0.0,-0.546875,0.0,0.0,1176
+0.0,-0.546875,0.0,0.0,1190
+0.0,-0.546875,0.0,0.0,1202
+0.0,-0.546875,0.0,0.0,1219
+0.0,-0.546875,0.0,0.0,1230
+0.0,-0.546875,0.0,0.0,1244
+0.0,-0.546875,0.0,0.0,1258
+0.0,-0.546875,0.0,0.0,1268
+0.0,-0.546875,0.0,0.0,1281
+0.0,-0.546875,0.0,0.0,1301
+0.0,-0.546875,0.0,0.0,1321
+0.0,-0.546875,0.0,0.0,1363
+0.0,-0.546875,0.0,0.0,1376
+0.0,-0.546875,0.0,0.0,1388
+0.0,-0.546875,0.0,0.0,1402
+0.0,-0.546875,0.0,0.0,1422
+0.0,-0.546875,0.0,0.0,1441
+0.0,-0.546875,0.0,0.0,1461
+0.0,-0.546875,0.0,0.0,1482
+0.0,-0.546875,0.0,0.0,1502
+0.0,-0.546875,0.0,0.0,1521
+0.0,-0.546875,0.0,0.0,1542
+0.0,-0.546875,0.0,0.0,1562
+0.0,-0.546875,0.0,0.0,1598
+0.0,-0.546875,0.0,0.0,1609
+0.0,-0.546875,0.0,0.0,1621
+0.0,-0.546875,0.0,0.0,1642
+0.0,-0.546875,0.0,0.0,1661
+0.0,-0.546875,0.0,0.0,1682
+0.0,-0.546875,0.0,0.0,1702
+0.0,-0.546875,0.0,0.0,1722
+0.0,-0.546875,0.0,0.0,1742
+0.0,-0.546875,0.0,0.0,1762
+0.0,-0.546875,0.0,0.0,1781
+0.0,-0.5390625,0.0,0.0,1801
+0.0,-0.5390625,0.0,0.0,1835
+0.0,-0.5390625,0.0,0.0,1847
+0.0,-0.5390625,0.0,0.0,1861
+0.0,-0.5390625,0.0,0.0,1882
+0.0,-0.5390625,0.0,0.0,1901
+0.0,-0.5390625,0.0,0.0,1921
+0.0,-0.5390625,0.0,0.0,1941
+0.0,-0.5390625,0.0,0.0,1962
+0.0,-0.5390625,0.0,0.0,1983
+0.0,-0.5390625,0.0,0.0,2001
+0.0,-0.5390625,0.0,0.0,2022
+0.0,-0.5390625,0.0,0.0,2042
+0.0,-0.5390625,0.0,0.0,2085
+0.0,-0.5390625,0.0,0.0,2097
+0.0,-0.5390625,0.0,0.0,2113
+0.0,-0.5390625,0.0,0.0,2124
+0.0,-0.5390625,0.0,0.0,2141
+0.0,-0.5390625,0.0,0.0,2161
+0.0,-0.5390625,0.0,0.0,2181
+0.0,-0.5390625,0.0,0.0,2201
+0.0,-0.5390625,0.0,0.0,2221
+0.0,-0.5390625,0.0,0.0,2241
+0.0,-0.5390625,0.0,0.0,2262
+0.0,-0.5390625,0.0,0.0,2283
+0.0,-0.2265625,0.0,0.0,2377
+0.0,-0.2265625,0.0,0.0,2390
+0.0,0.0,0.0,0.0,2402
+0.0,0.0,0.0,0.0,2417
+0.0,0.0,0.0,0.0,2428
+0.0,0.0,0.0,0.0,2440
+0.0,0.0,0.0,0.0,2451
+0.0,0.0,0.0,0.0,2463
+0.0,0.0,0.0,0.0,2475
+0.0,0.0,0.0,0.0,2493
+0.0,0.0,0.0,0.0,2505
+0.0,0.0,0.0,0.0,2521
+0.0,0.0,0.0,0.0,2558
+0.0,0.0,0.0,0.0,2576
+0.0,0.0,0.0,0.0,2592
+0.0,0.0,0.0,0.0,2603
+0.0,0.0,0.0,0.0,2621
+0.0,0.0,0.0,0.0,2641
+0.0,0.0,0.0,0.0,2661
+0.0,0.0,0.0,0.0,2681
+0.0,0.0,0.0,0.0,2702
+0.0,0.0,0.0,0.0,2721
+0.0,0.0,0.0,0.0,2742
+0.0,0.0,0.0,0.0,2762
+0.0,0.0,0.0,0.0,2805
+0.0,0.0,0.0,0.0,2814
+0.0,0.0,0.0,0.0,2825
+0.0,0.0,0.0,0.0,2841
+0.0,0.0,0.0,0.0,2861
+0.0,0.0,0.0,0.0,2882
+0.0,0.0,0.0,0.0,2901
0.0,0.0,0.0,0.0,2922
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
index 2438a38..45a25fd 100644
--- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
+++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
@@ -1,91 +1,91 @@
-package frc4388.robot.commands.Autos;
-
-import java.io.File;
-import java.util.ArrayList;
-import java.util.HashMap;
-
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc4388.robot.commands.Swerve.JoystickPlayback;
-import frc4388.robot.subsystems.SwerveDrive;
-
-public class PlaybackChooser {
- private final ArrayList> m_choosers = new ArrayList<>();
- private SendableChooser m_playback = null;
- private final ArrayList m_widgets = new ArrayList<>();
- private final HashMap m_commandPool = new HashMap<>();
-
- private final File m_dir = new File("/home/lvuser/autos/");
- private int m_cmdNum = 0;
- private final SwerveDrive m_swerve;
-
- // commands
- private Command m_noAuto = new InstantCommand();
-
- public PlaybackChooser(SwerveDrive swerve, Object... pool) {
- m_swerve = swerve;
- }
-
- public PlaybackChooser addOption(String name, Command option) {
- m_commandPool.put(name, option);
- return this;
- }
-
- public PlaybackChooser buildDisplay() {
- for (int i = 0; i < 10; i++) {
- appendCommand();
- }
- m_playback = m_choosers.get(0);
- nextChooser();
-
- Shuffleboard.getTab("Auto Chooser")
- .add("Add Sequence", new InstantCommand(() -> nextChooser()))
- .withPosition(4, 0);
- return this;
- }
-
- // This will be bound to a button for the time being
- public void appendCommand() {
- var chooser = new SendableChooser();
- chooser.setDefaultOption("No Auto", m_noAuto);
-
- m_choosers.add(chooser);
- ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
- .add("Command: " + m_choosers.size(), chooser)
- .withSize(4, 1)
- .withPosition(0, m_choosers.size() - 1)
- .withWidget(BuiltInWidgets.kSplitButtonChooser);
-
- m_widgets.add(widget);
- }
-
- public void nextChooser() {
- SendableChooser chooser = m_choosers.get(m_cmdNum++);
-
- for (String auto : m_dir.list()) {
- chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
- }
- for (var cmd_name : m_commandPool.keySet()) {
- chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
- }
- }
-
- public Command getCommand() {
- Command command = m_playback.getSelected();
- command = command == null ? m_noAuto : command.asProxy();
-
- Command[] commands = new Command[m_cmdNum - 1];
- for (int i = 0; i < m_cmdNum - 1; i++) {
- Command command2 = m_choosers.get(i + 1).getSelected();
- command2 = command2 == null ? m_noAuto : command2.asProxy();
-
- commands[i] = command2.asProxy();
- }
-
- return command.andThen(commands);
- }
-}
+package frc4388.robot.commands.Autos;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc4388.robot.commands.Swerve.JoystickPlayback;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class PlaybackChooser {
+ private final ArrayList> m_choosers = new ArrayList<>();
+ private SendableChooser m_playback = null;
+ private final ArrayList m_widgets = new ArrayList<>();
+ private final HashMap m_commandPool = new HashMap<>();
+
+ private final File m_dir = new File("/home/lvuser/autos/");
+ private int m_cmdNum = 0;
+ private final SwerveDrive m_swerve;
+
+ // commands
+ private Command m_noAuto = new InstantCommand();
+
+ public PlaybackChooser(SwerveDrive swerve, Object... pool) {
+ m_swerve = swerve;
+ }
+
+ public PlaybackChooser addOption(String name, Command option) {
+ m_commandPool.put(name, option);
+ return this;
+ }
+
+ public PlaybackChooser buildDisplay() {
+ for (int i = 0; i < 10; i++) {
+ appendCommand();
+ }
+ m_playback = m_choosers.get(0);
+ nextChooser();
+
+ Shuffleboard.getTab("Auto Chooser")
+ .add("Add Sequence", new InstantCommand(() -> nextChooser()))
+ .withPosition(4, 0);
+ return this;
+ }
+
+ // This will be bound to a button for the time being
+ public void appendCommand() {
+ var chooser = new SendableChooser();
+ chooser.setDefaultOption("No Auto", m_noAuto);
+
+ m_choosers.add(chooser);
+ ComplexWidget widget = Shuffleboard.getTab("Auto Chooser")
+ .add("Command: " + m_choosers.size(), chooser)
+ .withSize(4, 1)
+ .withPosition(0, m_choosers.size() - 1)
+ .withWidget(BuiltInWidgets.kSplitButtonChooser);
+
+ m_widgets.add(widget);
+ }
+
+ public void nextChooser() {
+ SendableChooser chooser = m_choosers.get(m_cmdNum++);
+
+ for (String auto : m_dir.list()) {
+ chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
+ }
+ for (var cmd_name : m_commandPool.keySet()) {
+ chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
+ }
+ }
+
+ public Command getCommand() {
+ Command command = m_playback.getSelected();
+ command = command == null ? m_noAuto : command.asProxy();
+
+ Command[] commands = new Command[m_cmdNum - 1];
+ for (int i = 0; i < m_cmdNum - 1; i++) {
+ Command command2 = m_choosers.get(i + 1).getSelected();
+ command2 = command2 == null ? m_noAuto : command2.asProxy();
+
+ commands[i] = command2.asProxy();
+ }
+
+ return command.andThen(commands);
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt
index c99ee2c..2e68168 100644
--- a/src/main/java/frc4388/robot/commands/Autos/Taxi.txt
+++ b/src/main/java/frc4388/robot/commands/Autos/Taxi.txt
@@ -1,225 +1,225 @@
-0.0,0.0,0.0,0.0,0
-0.0,0.0,0.0,0.0,0
-0.0,0.0,0.0,0.0,12
-0.0,0.0,0.0,0.0,26
-0.0,0.0,0.0,0.0,37
-0.0,0.0,0.0,0.0,50
-0.0,0.0,0.0,0.0,62
-0.0,0.0,0.0,0.0,73
-0.0,0.0,0.0,0.0,88
-0.0,0.0,0.0,0.0,103
-0.0,0.0,0.0,0.0,116
-0.0,0.0,0.0,0.0,160
-0.0,0.0,0.0,0.0,173
-0.0,0.0,0.0,0.0,185
-0.0,0.0,0.0,0.0,200
-0.0,0.0,0.0,0.0,211
-0.0,0.0,0.0,0.0,223
-0.0,0.0,0.0,0.0,235
-0.0,0.0,0.0,0.0,247
-0.0,0.0,0.0,0.0,263
-0.0,0.0,0.0,0.0,283
-0.0,0.0,0.0,0.0,303
-0.0,-0.109375,0.0,0.0,323
-0.0,-0.1484375,0.0,0.0,343
-0.0,-0.2109375,0.0,0.0,363
-0.0,-0.3671875,0.0,0.0,398
-0.0,-0.4140625,0.0,0.0,411
-0.0,-0.4765625,0.0,0.0,425
-0.0,-0.5078125,0.0,0.0,443
-0.0,-0.5078125,0.0,0.0,463
-0.0,-0.53125,0.0,0.0,483
-0.0,-0.5546875,0.0,0.0,504
-0.0,-0.5625,0.0,0.0,523
-0.0,-0.5625,0.0,0.0,544
-0.0,-0.5703125,0.0,0.0,563
-0.0,-0.5859375,0.0,0.0,584
-0.0,-0.5859375,0.0,0.0,603
-0.0,-0.5859375,0.0,0.0,640
-0.0,-0.59375,0.0,0.0,657
-0.0,-0.6015625,0.0,0.0,672
-0.0,-0.6015625,0.0,0.0,685
-0.0,-0.6015625,0.0,0.0,703
-0.0,-0.6015625,0.0,0.0,723
-0.0,-0.6015625,0.0,0.0,743
-0.0,-0.6015625,0.0,0.0,763
-0.0,-0.6015625,0.0,0.0,783
-0.0,-0.6015625,0.0,0.0,803
-0.0,-0.6015625,0.0,0.0,823
-0.0,-0.6015625,0.0,0.0,844
-0.0,-0.6015625,0.0,0.0,878
-0.0,-0.6015625,0.0,0.0,893
-0.0,-0.6015625,0.0,0.0,907
-0.0,-0.6015625,0.0,0.0,924
-0.0,-0.609375,0.0,0.0,943
-0.0,-0.609375,0.0,0.0,963
-0.0,-0.609375,0.0,0.0,983
-0.0,-0.609375,0.0,0.0,1004
-0.0,-0.609375,0.0,0.0,1023
-0.0,-0.609375,0.0,0.0,1043
-0.0,-0.609375,0.0,0.0,1064
-0.0,-0.609375,0.0,0.0,1083
-0.0,-0.609375,0.0,0.0,1156
-0.0,-0.609375,0.0,0.0,1172
-0.0,-0.609375,0.0,0.0,1185
-0.0,-0.609375,0.0,0.0,1200
-0.0,-0.609375,0.0,0.0,1215
-0.0,-0.609375,0.0,0.0,1225
-0.0,-0.609375,0.0,0.0,1236
-0.0,-0.609375,0.0,0.0,1249
-0.0,-0.609375,0.0,0.0,1263
-0.0,-0.609375,0.0,0.0,1283
-0.0,-0.609375,0.0,0.0,1303
-0.0,-0.609375,0.0,0.0,1323
-0.0,-0.609375,0.0,0.0,1363
-0.0,-0.6015625,0.0,0.0,1376
-0.0,-0.6015625,0.0,0.0,1394
-0.0,-0.6015625,0.0,0.0,1405
-0.0,-0.6015625,0.0,0.0,1423
-0.0,-0.6015625,0.0,0.0,1443
-0.0,-0.6015625,0.0,0.0,1463
-0.0,-0.6015625,0.0,0.0,1483
-0.0,-0.6015625,0.0,0.0,1503
-0.0,-0.6015625,0.0,0.0,1523
-0.0,-0.6015625,0.0,0.0,1543
-0.0,-0.6015625,0.0,0.0,1563
-0.0,-0.6015625,0.0,0.0,1597
-0.0,-0.6015625,0.0,0.0,1608
-0.0,-0.6015625,0.0,0.0,1624
-0.0,-0.6015625,0.0,0.0,1643
-0.0,-0.6015625,0.0,0.0,1664
-0.0,-0.5859375,0.0,0.0,1683
-0.0,-0.5859375,0.0,0.0,1703
-0.0,-0.5625,0.0,0.0,1723
-0.0,-0.5625,0.0,0.0,1743
-0.0,-0.5625,0.0,0.0,1763
-0.0,-0.5625,0.0,0.0,1783
-0.0,-0.5625,0.0,0.0,1803
-0.0,-0.5625,0.0,0.0,1843
-0.0,-0.5625,0.0,0.0,1855
-0.0,-0.5625,0.0,0.0,1868
-0.0,-0.5625,0.0,0.0,1883
-0.0,-0.5625,0.0,0.0,1903
-0.0,-0.5625,0.0,0.0,1923
-0.0,-0.5625,0.0,0.0,1943
-0.0,-0.5625,0.0,0.0,1963
-0.0,-0.5625,0.0,0.0,1983
-0.0,-0.5625,0.0,0.0,2003
-0.0,-0.5625,0.0,0.0,2024
-0.0,-0.5625,0.0,0.0,2043
-0.0,-0.5625,0.0,0.0,2081
-0.0,-0.5625,0.0,0.0,2093
-0.0,-0.5625,0.0,0.0,2105
-0.0,-0.5625,0.0,0.0,2123
-0.0,-0.5625,0.0,0.0,2143
-0.0,-0.5625,0.0,0.0,2163
-0.0,-0.5625,0.0,0.0,2183
-0.0,-0.5625,0.0,0.0,2203
-0.0,-0.5625,0.0,0.0,2223
-0.0,-0.5625,0.0,0.0,2243
-0.0,-0.5625,0.0,0.0,2263
-0.0,-0.5625,0.0,0.0,2283
-0.0,-0.5625,0.0,0.0,2366
-0.0,-0.5625,0.0,0.0,2377
-0.0,-0.5625,0.0,0.0,2394
-0.0,-0.5703125,0.0,0.0,2405
-0.0,-0.5703125,0.0,0.0,2418
-0.0,-0.5703125,0.0,0.0,2431
-0.0,-0.5703125,0.0,0.0,2444
-0.0,-0.5703125,0.0,0.0,2458
-0.0,-0.5703125,0.0,0.0,2470
-0.0,-0.5703125,0.0,0.0,2485
-0.0,-0.5703125,0.0,0.0,2503
-0.0,-0.5703125,0.0,0.0,2523
-0.0,-0.5703125,0.0,0.0,2563
-0.0,-0.5703125,0.0,0.0,2577
-0.0,-0.5703125,0.0,0.0,2591
-0.0,-0.5703125,0.0,0.0,2608
-0.0,-0.5703125,0.0,0.0,2624
-0.0,-0.5703125,0.0,0.0,2643
-0.0,-0.5703125,0.0,0.0,2677
-0.0,-0.5703125,0.0,0.0,2698
-0.0,-0.5703125,0.0,0.0,2711
-0.0,-0.5703125,0.0,0.0,2725
-0.0,-0.5703125,0.0,0.0,2743
-0.0,-0.5703125,0.0,0.0,2764
-0.0,-0.5703125,0.0,0.0,2810
-0.0,-0.5703125,0.0,0.0,2820
-0.0,-0.5703125,0.0,0.0,2833
-0.0,-0.5703125,0.0,0.0,2845
-0.0,-0.5703125,0.0,0.0,2863
-0.0,-0.5703125,0.0,0.0,2883
-0.0,-0.5703125,0.0,0.0,2904
-0.0,-0.5703125,0.0,0.0,2924
-0.0,-0.5703125,0.0,0.0,2943
-0.0,-0.5703125,0.0,0.0,2963
-0.0,-0.5703125,0.0,0.0,2983
-0.0,-0.5703125,0.0,0.0,3003
-0.0,-0.5703125,0.0,0.0,3033
-0.0,-0.5703125,0.0,0.0,3050
-0.0,-0.5703125,0.0,0.0,3065
-0.0,-0.5703125,0.0,0.0,3083
-0.0,-0.5703125,0.0,0.0,3103
-0.0,-0.5703125,0.0,0.0,3123
-0.0,-0.5703125,0.0,0.0,3144
-0.0,-0.5703125,0.0,0.0,3164
-0.0,-0.5703125,0.0,0.0,3184
-0.0,-0.5703125,0.0,0.0,3203
-0.0,-0.5703125,0.0,0.0,3223
-0.0,-0.5703125,0.0,0.0,3243
-0.0,-0.5703125,0.0,0.0,3272
-0.0,-0.5703125,0.0,0.0,3289
-0.0,-0.5703125,0.0,0.0,3303
-0.0,-0.5703125,0.0,0.0,3323
-0.0,-0.5703125,0.0,0.0,3343
-0.0,-0.5703125,0.0,0.0,3363
-0.0,-0.5703125,0.0,0.0,3383
-0.0,-0.5703125,0.0,0.0,3403
-0.0,-0.5703125,0.0,0.0,3423
-0.0,-0.5703125,0.0,0.0,3443
-0.0,-0.5703125,0.0,0.0,3463
-0.0,-0.5703125,0.0,0.0,3483
-0.0,-0.5703125,0.0,0.0,3566
-0.0,-0.5703125,0.0,0.0,3578
-0.0,-0.5703125,0.0,0.0,3596
-0.0,-0.5703125,0.0,0.0,3610
-0.0,-0.5703125,0.0,0.0,3623
-0.0,-0.5703125,0.0,0.0,3640
-0.0,-0.5703125,0.0,0.0,3651
-0.0,-0.5703125,0.0,0.0,3663
-0.0,-0.5703125,0.0,0.0,3678
-0.0,-0.5703125,0.0,0.0,3691
-0.0,-0.5703125,0.0,0.0,3706
-0.0,-0.5703125,0.0,0.0,3723
-0.0,-0.5703125,0.0,0.0,3766
-0.0,-0.5703125,0.0,0.0,3778
-0.0,-0.5703125,0.0,0.0,3792
-0.0,-0.5703125,0.0,0.0,3807
-0.0,-0.5703125,0.0,0.0,3823
-0.0,-0.5703125,0.0,0.0,3843
-0.0,-0.53125,0.0,0.0,3863
-0.0,-0.53125,0.0,0.0,3884
-0.0,-0.421875,0.0,0.0,3904
-0.0,0.0,0.0,0.0,3924
-0.0,0.0,0.0,0.0,3944
-0.0,0.0,0.0,0.0,3963
-0.0,0.0,0.0,0.0,3999
-0.0,0.0,0.0,0.0,4010
-0.0,0.0,0.0,0.0,4025
-0.0,0.0,0.0,0.0,4043
-0.0,0.0,0.0,0.0,4063
-0.0,0.0,0.0,0.0,4083
-0.0,0.0,0.0,0.0,4103
-0.0,0.0,0.0,0.0,4123
-0.0,0.0,0.0,0.0,4143
-0.0,0.0,0.0,0.0,4163
-0.0,0.0,0.0,0.0,4183
-0.0,0.0,0.0,0.0,4203
-0.0,0.0,0.0,0.0,4236
-0.0,0.0,0.0,0.0,4247
-0.0,0.0,0.0,0.0,4264
-0.0,0.0,0.0,0.0,4284
-0.0,0.0,0.0,0.0,4304
-0.0,0.0,0.0,0.0,4324
-0.0,0.0,0.0,0.0,4343
+0.0,0.0,0.0,0.0,0
+0.0,0.0,0.0,0.0,0
+0.0,0.0,0.0,0.0,12
+0.0,0.0,0.0,0.0,26
+0.0,0.0,0.0,0.0,37
+0.0,0.0,0.0,0.0,50
+0.0,0.0,0.0,0.0,62
+0.0,0.0,0.0,0.0,73
+0.0,0.0,0.0,0.0,88
+0.0,0.0,0.0,0.0,103
+0.0,0.0,0.0,0.0,116
+0.0,0.0,0.0,0.0,160
+0.0,0.0,0.0,0.0,173
+0.0,0.0,0.0,0.0,185
+0.0,0.0,0.0,0.0,200
+0.0,0.0,0.0,0.0,211
+0.0,0.0,0.0,0.0,223
+0.0,0.0,0.0,0.0,235
+0.0,0.0,0.0,0.0,247
+0.0,0.0,0.0,0.0,263
+0.0,0.0,0.0,0.0,283
+0.0,0.0,0.0,0.0,303
+0.0,-0.109375,0.0,0.0,323
+0.0,-0.1484375,0.0,0.0,343
+0.0,-0.2109375,0.0,0.0,363
+0.0,-0.3671875,0.0,0.0,398
+0.0,-0.4140625,0.0,0.0,411
+0.0,-0.4765625,0.0,0.0,425
+0.0,-0.5078125,0.0,0.0,443
+0.0,-0.5078125,0.0,0.0,463
+0.0,-0.53125,0.0,0.0,483
+0.0,-0.5546875,0.0,0.0,504
+0.0,-0.5625,0.0,0.0,523
+0.0,-0.5625,0.0,0.0,544
+0.0,-0.5703125,0.0,0.0,563
+0.0,-0.5859375,0.0,0.0,584
+0.0,-0.5859375,0.0,0.0,603
+0.0,-0.5859375,0.0,0.0,640
+0.0,-0.59375,0.0,0.0,657
+0.0,-0.6015625,0.0,0.0,672
+0.0,-0.6015625,0.0,0.0,685
+0.0,-0.6015625,0.0,0.0,703
+0.0,-0.6015625,0.0,0.0,723
+0.0,-0.6015625,0.0,0.0,743
+0.0,-0.6015625,0.0,0.0,763
+0.0,-0.6015625,0.0,0.0,783
+0.0,-0.6015625,0.0,0.0,803
+0.0,-0.6015625,0.0,0.0,823
+0.0,-0.6015625,0.0,0.0,844
+0.0,-0.6015625,0.0,0.0,878
+0.0,-0.6015625,0.0,0.0,893
+0.0,-0.6015625,0.0,0.0,907
+0.0,-0.6015625,0.0,0.0,924
+0.0,-0.609375,0.0,0.0,943
+0.0,-0.609375,0.0,0.0,963
+0.0,-0.609375,0.0,0.0,983
+0.0,-0.609375,0.0,0.0,1004
+0.0,-0.609375,0.0,0.0,1023
+0.0,-0.609375,0.0,0.0,1043
+0.0,-0.609375,0.0,0.0,1064
+0.0,-0.609375,0.0,0.0,1083
+0.0,-0.609375,0.0,0.0,1156
+0.0,-0.609375,0.0,0.0,1172
+0.0,-0.609375,0.0,0.0,1185
+0.0,-0.609375,0.0,0.0,1200
+0.0,-0.609375,0.0,0.0,1215
+0.0,-0.609375,0.0,0.0,1225
+0.0,-0.609375,0.0,0.0,1236
+0.0,-0.609375,0.0,0.0,1249
+0.0,-0.609375,0.0,0.0,1263
+0.0,-0.609375,0.0,0.0,1283
+0.0,-0.609375,0.0,0.0,1303
+0.0,-0.609375,0.0,0.0,1323
+0.0,-0.609375,0.0,0.0,1363
+0.0,-0.6015625,0.0,0.0,1376
+0.0,-0.6015625,0.0,0.0,1394
+0.0,-0.6015625,0.0,0.0,1405
+0.0,-0.6015625,0.0,0.0,1423
+0.0,-0.6015625,0.0,0.0,1443
+0.0,-0.6015625,0.0,0.0,1463
+0.0,-0.6015625,0.0,0.0,1483
+0.0,-0.6015625,0.0,0.0,1503
+0.0,-0.6015625,0.0,0.0,1523
+0.0,-0.6015625,0.0,0.0,1543
+0.0,-0.6015625,0.0,0.0,1563
+0.0,-0.6015625,0.0,0.0,1597
+0.0,-0.6015625,0.0,0.0,1608
+0.0,-0.6015625,0.0,0.0,1624
+0.0,-0.6015625,0.0,0.0,1643
+0.0,-0.6015625,0.0,0.0,1664
+0.0,-0.5859375,0.0,0.0,1683
+0.0,-0.5859375,0.0,0.0,1703
+0.0,-0.5625,0.0,0.0,1723
+0.0,-0.5625,0.0,0.0,1743
+0.0,-0.5625,0.0,0.0,1763
+0.0,-0.5625,0.0,0.0,1783
+0.0,-0.5625,0.0,0.0,1803
+0.0,-0.5625,0.0,0.0,1843
+0.0,-0.5625,0.0,0.0,1855
+0.0,-0.5625,0.0,0.0,1868
+0.0,-0.5625,0.0,0.0,1883
+0.0,-0.5625,0.0,0.0,1903
+0.0,-0.5625,0.0,0.0,1923
+0.0,-0.5625,0.0,0.0,1943
+0.0,-0.5625,0.0,0.0,1963
+0.0,-0.5625,0.0,0.0,1983
+0.0,-0.5625,0.0,0.0,2003
+0.0,-0.5625,0.0,0.0,2024
+0.0,-0.5625,0.0,0.0,2043
+0.0,-0.5625,0.0,0.0,2081
+0.0,-0.5625,0.0,0.0,2093
+0.0,-0.5625,0.0,0.0,2105
+0.0,-0.5625,0.0,0.0,2123
+0.0,-0.5625,0.0,0.0,2143
+0.0,-0.5625,0.0,0.0,2163
+0.0,-0.5625,0.0,0.0,2183
+0.0,-0.5625,0.0,0.0,2203
+0.0,-0.5625,0.0,0.0,2223
+0.0,-0.5625,0.0,0.0,2243
+0.0,-0.5625,0.0,0.0,2263
+0.0,-0.5625,0.0,0.0,2283
+0.0,-0.5625,0.0,0.0,2366
+0.0,-0.5625,0.0,0.0,2377
+0.0,-0.5625,0.0,0.0,2394
+0.0,-0.5703125,0.0,0.0,2405
+0.0,-0.5703125,0.0,0.0,2418
+0.0,-0.5703125,0.0,0.0,2431
+0.0,-0.5703125,0.0,0.0,2444
+0.0,-0.5703125,0.0,0.0,2458
+0.0,-0.5703125,0.0,0.0,2470
+0.0,-0.5703125,0.0,0.0,2485
+0.0,-0.5703125,0.0,0.0,2503
+0.0,-0.5703125,0.0,0.0,2523
+0.0,-0.5703125,0.0,0.0,2563
+0.0,-0.5703125,0.0,0.0,2577
+0.0,-0.5703125,0.0,0.0,2591
+0.0,-0.5703125,0.0,0.0,2608
+0.0,-0.5703125,0.0,0.0,2624
+0.0,-0.5703125,0.0,0.0,2643
+0.0,-0.5703125,0.0,0.0,2677
+0.0,-0.5703125,0.0,0.0,2698
+0.0,-0.5703125,0.0,0.0,2711
+0.0,-0.5703125,0.0,0.0,2725
+0.0,-0.5703125,0.0,0.0,2743
+0.0,-0.5703125,0.0,0.0,2764
+0.0,-0.5703125,0.0,0.0,2810
+0.0,-0.5703125,0.0,0.0,2820
+0.0,-0.5703125,0.0,0.0,2833
+0.0,-0.5703125,0.0,0.0,2845
+0.0,-0.5703125,0.0,0.0,2863
+0.0,-0.5703125,0.0,0.0,2883
+0.0,-0.5703125,0.0,0.0,2904
+0.0,-0.5703125,0.0,0.0,2924
+0.0,-0.5703125,0.0,0.0,2943
+0.0,-0.5703125,0.0,0.0,2963
+0.0,-0.5703125,0.0,0.0,2983
+0.0,-0.5703125,0.0,0.0,3003
+0.0,-0.5703125,0.0,0.0,3033
+0.0,-0.5703125,0.0,0.0,3050
+0.0,-0.5703125,0.0,0.0,3065
+0.0,-0.5703125,0.0,0.0,3083
+0.0,-0.5703125,0.0,0.0,3103
+0.0,-0.5703125,0.0,0.0,3123
+0.0,-0.5703125,0.0,0.0,3144
+0.0,-0.5703125,0.0,0.0,3164
+0.0,-0.5703125,0.0,0.0,3184
+0.0,-0.5703125,0.0,0.0,3203
+0.0,-0.5703125,0.0,0.0,3223
+0.0,-0.5703125,0.0,0.0,3243
+0.0,-0.5703125,0.0,0.0,3272
+0.0,-0.5703125,0.0,0.0,3289
+0.0,-0.5703125,0.0,0.0,3303
+0.0,-0.5703125,0.0,0.0,3323
+0.0,-0.5703125,0.0,0.0,3343
+0.0,-0.5703125,0.0,0.0,3363
+0.0,-0.5703125,0.0,0.0,3383
+0.0,-0.5703125,0.0,0.0,3403
+0.0,-0.5703125,0.0,0.0,3423
+0.0,-0.5703125,0.0,0.0,3443
+0.0,-0.5703125,0.0,0.0,3463
+0.0,-0.5703125,0.0,0.0,3483
+0.0,-0.5703125,0.0,0.0,3566
+0.0,-0.5703125,0.0,0.0,3578
+0.0,-0.5703125,0.0,0.0,3596
+0.0,-0.5703125,0.0,0.0,3610
+0.0,-0.5703125,0.0,0.0,3623
+0.0,-0.5703125,0.0,0.0,3640
+0.0,-0.5703125,0.0,0.0,3651
+0.0,-0.5703125,0.0,0.0,3663
+0.0,-0.5703125,0.0,0.0,3678
+0.0,-0.5703125,0.0,0.0,3691
+0.0,-0.5703125,0.0,0.0,3706
+0.0,-0.5703125,0.0,0.0,3723
+0.0,-0.5703125,0.0,0.0,3766
+0.0,-0.5703125,0.0,0.0,3778
+0.0,-0.5703125,0.0,0.0,3792
+0.0,-0.5703125,0.0,0.0,3807
+0.0,-0.5703125,0.0,0.0,3823
+0.0,-0.5703125,0.0,0.0,3843
+0.0,-0.53125,0.0,0.0,3863
+0.0,-0.53125,0.0,0.0,3884
+0.0,-0.421875,0.0,0.0,3904
+0.0,0.0,0.0,0.0,3924
+0.0,0.0,0.0,0.0,3944
+0.0,0.0,0.0,0.0,3963
+0.0,0.0,0.0,0.0,3999
+0.0,0.0,0.0,0.0,4010
+0.0,0.0,0.0,0.0,4025
+0.0,0.0,0.0,0.0,4043
+0.0,0.0,0.0,0.0,4063
+0.0,0.0,0.0,0.0,4083
+0.0,0.0,0.0,0.0,4103
+0.0,0.0,0.0,0.0,4123
+0.0,0.0,0.0,0.0,4143
+0.0,0.0,0.0,0.0,4163
+0.0,0.0,0.0,0.0,4183
+0.0,0.0,0.0,0.0,4203
+0.0,0.0,0.0,0.0,4236
+0.0,0.0,0.0,0.0,4247
+0.0,0.0,0.0,0.0,4264
+0.0,0.0,0.0,0.0,4284
+0.0,0.0,0.0,0.0,4304
+0.0,0.0,0.0,0.0,4324
+0.0,0.0,0.0,0.0,4343
0.0,0.0,0.0,0.0,4363
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/BooleanCommand.java b/src/main/java/frc4388/robot/commands/BooleanCommand.java
index 91e49b3..23c32ff 100644
--- a/src/main/java/frc4388/robot/commands/BooleanCommand.java
+++ b/src/main/java/frc4388/robot/commands/BooleanCommand.java
@@ -1,67 +1,67 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands;
-
-import java.util.function.Supplier;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-public class BooleanCommand extends CommandBase {
-
- private Supplier onTrue;
- private Supplier onFalse;
-
- private Supplier condition;
-
- private Supplier selected;
-
-
- /** Creates a new BooleanCommand. */
- public BooleanCommand(Supplier onTrue, Supplier onFalse, Supplier condition) {
- this.onTrue = onTrue;
- this.onFalse = onFalse;
- this.condition = condition;
- }
-
- // Called when the command is initially scheduled.
- @Override
- public void initialize() {
- if (condition.get()) {
- selected = onTrue;
- } else {
- selected = onFalse;
- }
- if (selected.get() != null) {
- selected.get().initialize();
- }
- }
-
- // Called every time the scheduler runs while the command is scheduled.
- @Override
- public void execute() {
- if (selected.get() != null) {
- selected.get().execute();
- }
- }
-
- // Called once the command ends or is interrupted.
- @Override
- public void end(boolean interrupted) {
- if (selected.get() != null) {
- selected.get().end(interrupted);
- }
- }
-
- // Returns true when the command should end.
- @Override
- public boolean isFinished() {
- if (selected.get() != null) {
- return selected.get().isFinished();
- } else {
- return true;
- }
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class BooleanCommand extends CommandBase {
+
+ private Supplier onTrue;
+ private Supplier onFalse;
+
+ private Supplier condition;
+
+ private Supplier selected;
+
+
+ /** Creates a new BooleanCommand. */
+ public BooleanCommand(Supplier onTrue, Supplier onFalse, Supplier condition) {
+ this.onTrue = onTrue;
+ this.onFalse = onFalse;
+ this.condition = condition;
+ }
+
+ // Called when the command is initially scheduled.
+ @Override
+ public void initialize() {
+ if (condition.get()) {
+ selected = onTrue;
+ } else {
+ selected = onFalse;
+ }
+ if (selected.get() != null) {
+ selected.get().initialize();
+ }
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ @Override
+ public void execute() {
+ if (selected.get() != null) {
+ selected.get().execute();
+ }
+ }
+
+ // Called once the command ends or is interrupted.
+ @Override
+ public void end(boolean interrupted) {
+ if (selected.get() != null) {
+ selected.get().end(interrupted);
+ }
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ if (selected.get() != null) {
+ return selected.get().isFinished();
+ } else {
+ return true;
+ }
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
index a3c9e6c..0b949fb 100644
--- a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
+++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
@@ -1,60 +1,60 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.utility.Gains;
-
-public abstract class PelvicInflammatoryDisease extends CommandBase {
- protected Gains gains;
- private double output = 0;
- private double tolerance = 0;
-
- /** Creates a new PelvicInflammatoryDisease. */
- public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) {
- gains = new Gains(kp, ki, kd, kf, 0);
- this.tolerance = tolerance;
- }
-
- public PelvicInflammatoryDisease(Gains gains, double tolerance) {
- this.gains = gains;
- this.tolerance = tolerance;
- }
-
- /** produces the error from the setpoint */
- public abstract double getError();
-
- /** figure it out bitch */
- public abstract void runWithOutput(double output);
-
- // Called when the command is initially scheduled.
- @Override
- public final void initialize() {
- output = 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;
-
- output = error * gains.kP;
- output += cumError * gains.kI;
- output += delta * gains.kD;
- output += gains.kF;
-
- runWithOutput(output);
- }
-
- // Returns true when the command should end.
- @Override
- public final boolean isFinished() {
- return Math.abs(getError()) < tolerance;
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.utility.Gains;
+
+public abstract class PelvicInflammatoryDisease extends CommandBase {
+ protected Gains gains;
+ private double output = 0;
+ private double tolerance = 0;
+
+ /** Creates a new PelvicInflammatoryDisease. */
+ public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) {
+ gains = new Gains(kp, ki, kd, kf, 0);
+ this.tolerance = tolerance;
+ }
+
+ public PelvicInflammatoryDisease(Gains gains, double tolerance) {
+ this.gains = gains;
+ this.tolerance = tolerance;
+ }
+
+ /** produces the error from the setpoint */
+ public abstract double getError();
+
+ /** figure it out bitch */
+ public abstract void runWithOutput(double output);
+
+ // Called when the command is initially scheduled.
+ @Override
+ public final void initialize() {
+ output = 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;
+
+ output = error * gains.kP;
+ output += cumError * gains.kI;
+ output += delta * gains.kD;
+ output += gains.kF;
+
+ runWithOutput(output);
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public final boolean isFinished() {
+ return Math.abs(getError()) < tolerance;
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java
index 212d979..666aea3 100644
--- a/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java
+++ b/src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java
@@ -1,42 +1,42 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Placement;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import frc4388.robot.commands.PelvicInflammatoryDisease;
-import frc4388.robot.subsystems.Limelight;
-import frc4388.robot.subsystems.SwerveDrive;
-
-public class AprilRotAlign extends PelvicInflammatoryDisease {
-
- SwerveDrive drive;
- Limelight lime;
-
- /** Creates a new AprilRotAlign. */
- public AprilRotAlign(SwerveDrive drive, Limelight lime) {
- super(0.1, 0.2, 0.0, 0.0, 0.0);
-
- this.drive = drive;
- this.lime = lime;
-
- addRequirements(drive, lime);
- }
-
- @Override
- public double getError() {
- double err = 0.0;
-
- try {
- err = lime.getAprilSkew();
- } catch (NullPointerException ex) {}
-
- return err;
- }
-
- @Override
- public void runWithOutput(double output) {
- drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true);
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Placement;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class AprilRotAlign extends PelvicInflammatoryDisease {
+
+ SwerveDrive drive;
+ Limelight lime;
+
+ /** Creates a new AprilRotAlign. */
+ public AprilRotAlign(SwerveDrive drive, Limelight lime) {
+ super(0.1, 0.2, 0.0, 0.0, 0.0);
+
+ this.drive = drive;
+ this.lime = lime;
+
+ addRequirements(drive, lime);
+ }
+
+ @Override
+ public double getError() {
+ double err = 0.0;
+
+ try {
+ err = lime.getAprilSkew();
+ } catch (NullPointerException ex) {}
+
+ return err;
+ }
+
+ @Override
+ public void runWithOutput(double output) {
+ drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true);
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java
index e79cffc..5a4fa73 100644
--- a/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java
+++ b/src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java
@@ -1,44 +1,44 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Placement;
-
-import java.util.function.DoubleSupplier;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import frc4388.robot.commands.PelvicInflammatoryDisease;
-import frc4388.robot.subsystems.Limelight;
-import frc4388.robot.subsystems.SwerveDrive;
-
-public class DriveToLimeDistance extends PelvicInflammatoryDisease {
-
- SwerveDrive drive;
- Limelight lime;
-
- double targetDistance;
- DoubleSupplier ds;
-
- /** Creates a new DriveToLimeDistance. */
- public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) {
- super(0.5, 0.0, 0.0, 0.0, 1);
-
- this.drive = drive;
- this.lime = lime;
- this.targetDistance = targetDistance;
- this.ds = ds;
-
- addRequirements(drive, lime);
- }
-
- @Override
- public double getError() {
- return targetDistance - ds.getAsDouble();
- }
-
- @Override
- public void runWithOutput(double output) {
- System.out.println(output / Math.abs(getError()));
- drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true);
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Placement;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class DriveToLimeDistance extends PelvicInflammatoryDisease {
+
+ SwerveDrive drive;
+ Limelight lime;
+
+ double targetDistance;
+ DoubleSupplier ds;
+
+ /** Creates a new DriveToLimeDistance. */
+ public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) {
+ super(0.5, 0.0, 0.0, 0.0, 1);
+
+ this.drive = drive;
+ this.lime = lime;
+ this.targetDistance = targetDistance;
+ this.ds = ds;
+
+ addRequirements(drive, lime);
+ }
+
+ @Override
+ public double getError() {
+ return targetDistance - ds.getAsDouble();
+ }
+
+ @Override
+ public void runWithOutput(double output) {
+ System.out.println(output / Math.abs(getError()));
+ drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true);
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java
index 5b2e87f..2c0900c 100644
--- a/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java
+++ b/src/main/java/frc4388/robot/commands/Placement/LimeAlign.java
@@ -1,48 +1,48 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Placement;
-
-import java.util.function.DoubleSupplier;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import frc4388.robot.Constants.VisionConstants;
-import frc4388.robot.commands.PelvicInflammatoryDisease;
-import frc4388.robot.subsystems.Limelight;
-import frc4388.robot.subsystems.SwerveDrive;
-
-public class LimeAlign extends PelvicInflammatoryDisease {
-
- SwerveDrive drive;
- Limelight lime;
-
- DoubleSupplier ds;
-
- public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) {
- super(0.4, 0.4, 0.0, 0.0, tolerance);
-
- this.drive = drive;
- this.lime = lime;
- this.ds = ds;
-
- addRequirements(drive, lime);
- }
-
- @Override
- public double getError() {
- double err = 0.0;
-
- try {
- System.out.println(ds.getAsDouble());
- err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
- } catch (NullPointerException ex) {}
-
- return err;
- }
-
- @Override
- public void runWithOutput(double output) {
- drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Placement;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class LimeAlign extends PelvicInflammatoryDisease {
+
+ SwerveDrive drive;
+ Limelight lime;
+
+ DoubleSupplier ds;
+
+ public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) {
+ super(0.4, 0.4, 0.0, 0.0, tolerance);
+
+ this.drive = drive;
+ this.lime = lime;
+ this.ds = ds;
+
+ addRequirements(drive, lime);
+ }
+
+ @Override
+ public double getError() {
+ double err = 0.0;
+
+ try {
+ System.out.println(ds.getAsDouble());
+ err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
+ } catch (NullPointerException ex) {}
+
+ return err;
+ }
+
+ @Override
+ public void runWithOutput(double output) {
+ drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
index 56e8d43..a0f3af1 100644
--- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
+++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
@@ -1,141 +1,141 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Swerve;
-
-import java.io.File;
-import java.io.FileNotFoundException;
-import java.util.ArrayList;
-import java.util.Scanner;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-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 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;
-
- 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;
- }
-
- // 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);
-
- 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;
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Swerve;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.util.ArrayList;
+import java.util.Scanner;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+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 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;
+
+ 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;
+ }
+
+ // 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);
+
+ 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;
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
index 84608cc..d52b53e 100644
--- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
@@ -1,97 +1,99 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Swerve;
-
-import java.io.File;
-import java.io.IOException;
-import java.io.PrintWriter;
-import java.util.ArrayList;
-import java.util.function.Supplier;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.utility.UtilityStructs.TimedOutput;
-
-public class JoystickRecorder extends CommandBase {
- public final SwerveDrive swerve;
-
- public final Supplier leftX;
- public final Supplier leftY;
- public final Supplier rightX;
- public final Supplier rightY;
- private String filename;
- public final ArrayList outputs = new ArrayList<>();
- private long startTime = -1;
-
-
- /** Creates a new JoystickRecorder. */
- public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY,
- Supplier rightX, Supplier 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.driveWithInput(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;
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Swerve;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.UtilityStructs.TimedOutput;
+import frc4388.utility.controller.XboxController;
+
+public class JoystickRecorder extends CommandBase {
+ public final SwerveDrive swerve;
+ public final XboxController driveXbox;
+ public final XboxController operatorXbox;
+
+ private String filename;
+ public final ArrayList outputs = new ArrayList<>();
+ private long startTime = -1;
+
+ /** Creates a new JoystickRecorder. */
+ public JoystickRecorder(SwerveDrive swerve, XboxController driveXbox,
+ XboxController operatorXbox, String filename) {
+ this.swerve = swerve;
+ this.driveXbox = driveXbox;
+ this.operatorXbox = operatorXbox;
+ 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.driverLeftX = driveXbox.getLeftXAxis();
+ inputs.driverLeftY = driveXbox.getLeftYAxis();
+ inputs.driverRightX = driveXbox.getRightXAxis();
+ inputs.driverRightY = driveXbox.getRightYAxis();
+
+ inputs.operatorLeftX = driveXbox.getLeftXAxis();
+ inputs.operatorLeftY = driveXbox.getLeftYAxis();
+ inputs.operatorRightX = driveXbox.getRightXAxis();
+ inputs.operatorRightY = driveXbox.getRightYAxis();
+
+ inputs.timedOffset = System.currentTimeMillis() - startTime;
+
+ outputs.add(inputs);
+
+ swerve.driveWithInput(new Translation2d(inputs.driverLeftX, inputs.driverLeftX),
+ new Translation2d(inputs.driverRightX, inputs.driverRightY),
+ 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.driverLeftX + "," + input.driverLeftX + "," +
+ input.driverRightX + "," + input.driverRightY + "," +
+ input.operatorLeftX + "," + input.operatorLeftX + "," +
+ input.operatorRightX + "," + input.operatorRightY + "," +
+ input.timedOffset);
+ }
+
+ writer.close();
+ } catch (IOException e) {
+ e.printStackTrace();
+ }
+ }
+
+ // Returns true when the command should end.
+ @Override
+ public boolean isFinished() {
+ return false;
+ }
+}
diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
index 8cf630f..b47084a 100644
--- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
+++ b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
@@ -1,35 +1,35 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Swerve;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import frc4388.robot.commands.PelvicInflammatoryDisease;
-import frc4388.robot.subsystems.SwerveDrive;
-
-public class RotateToAngle extends PelvicInflammatoryDisease {
-
- SwerveDrive drive;
- double targetAngle;
-
- /** 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;
-
- addRequirements(drive);
- }
-
- @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);
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Swerve;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class RotateToAngle extends PelvicInflammatoryDisease {
+
+ SwerveDrive drive;
+ double targetAngle;
+
+ /** 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;
+
+ addRequirements(drive);
+ }
+
+ @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);
+ }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Apriltags.java b/src/main/java/frc4388/robot/subsystems/Apriltags.java
index c6062e8..35bc6c7 100644
--- a/src/main/java/frc4388/robot/subsystems/Apriltags.java
+++ b/src/main/java/frc4388/robot/subsystems/Apriltags.java
@@ -1,36 +1,36 @@
-package frc4388.robot.subsystems;
-
-//import edu.wpi.first.apriltag.AprilTag;
-//import edu.wpi.first.math.geometry.Pose3d;
-//import edu.wpi.first.math.geometry.Rotation3d;
-//import edu.wpi.first.networktables.NetworkTable;
-//import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-public class Apriltags {
- public static class Tag {
- public boolean visible = true;
- public double x, y, z = 0;
- public double ry, rp, rr = 0;
- }
-
- public Tag getTagPosRot() {
- final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-
- final Tag tag = new Tag();
- tag.visible = isAprilTag();
- tag.x = tagTable.getEntry("TagPosX").getDouble(0);
- tag.y = tagTable.getEntry("TagPosY").getDouble(0);
- tag.z = tagTable.getEntry("TagPosZ").getDouble(0);
- tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
- tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
- tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
-
- return tag;
- }
-
- public boolean isAprilTag() {
- final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
- return tagTable.getEntry("IsTag").getBoolean(false);
- }
-}
+package frc4388.robot.subsystems;
+
+//import edu.wpi.first.apriltag.AprilTag;
+//import edu.wpi.first.math.geometry.Pose3d;
+//import edu.wpi.first.math.geometry.Rotation3d;
+//import edu.wpi.first.networktables.NetworkTable;
+//import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Apriltags {
+ public static class Tag {
+ public boolean visible = true;
+ public double x, y, z = 0;
+ public double ry, rp, rr = 0;
+ }
+
+ public Tag getTagPosRot() {
+ final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+ final Tag tag = new Tag();
+ tag.visible = isAprilTag();
+ tag.x = tagTable.getEntry("TagPosX").getDouble(0);
+ tag.y = tagTable.getEntry("TagPosY").getDouble(0);
+ tag.z = tagTable.getEntry("TagPosZ").getDouble(0);
+ tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
+ tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
+ tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
+
+ return tag;
+ }
+
+ public boolean isAprilTag() {
+ final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+ return tagTable.getEntry("IsTag").getBoolean(false);
+ }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index 94081df..e99e59c 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/src/main/java/frc4388/robot/subsystems/Arm.java
@@ -1,159 +1,159 @@
-package frc4388.robot.subsystems;
-
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
-import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.CANCoderConfiguration;
-import frc4388.robot.Constants.ArmConstants;
-import frc4388.utility.DeferredBlock;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class Arm extends SubsystemBase {
- private WPI_TalonFX m_tele;
- public WPI_TalonFX m_pivot;
- private CANCoder m_pivotEncoder;
-
- // Moves arm to distance [dist] then returns new ang
- public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) {
- m_tele = tele;
- m_pivot = pivot;
- m_pivotEncoder = encoder;
-
- m_tele.configFactoryDefault();
- m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
- m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
- m_pivot.configFactoryDefault();
-
- // * Example of deferred code
- new DeferredBlock(() -> resetTeleSoftLimit());
-
- CANCoderConfiguration config = new CANCoderConfiguration();
- config.magnetOffsetDegrees = ArmConstants.OFFSET;
- m_pivotEncoder.configAllSettings(config);
- }
-
- public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
- this(pivot, tele, encoder, false);
- }
-
- public void setRotVel(double vel) {
- if (vel > 1) vel = 1;
-
- var degrees = Math.abs(getArmRotation()) - 135;
- SmartDashboard.putNumber("arm degrees", degrees);
- SmartDashboard.putNumber("arm rot vel", vel);
-
- if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
- m_pivot.set(ControlMode.PercentOutput, 0);
- } else if (degrees > 90 && vel > 0) {
- m_pivot.set(ControlMode.PercentOutput, .15 * vel);
- } else if (degrees < 25 && vel < 0) {
- m_pivot.set(ControlMode.PercentOutput, .15 * vel);
- } else {
- m_pivot.set(ControlMode.PercentOutput, .3 * vel);
- }
- }
-
- public void setTeleVel(double vel) {
- m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
- }
-
- public void armSetRotation(double rot) {
- if (rot > 1 || rot < 0) return;
- // Move arm code
- m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) +
- ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
- }
-
- public void armSetLength(double len) {
- if (len > 1 || len < 0) return;
- // Move arm code
- m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) +
- ArmConstants.TELE_FORWARD_SOFT_LIMIT);
-
- if (m_tele.isRevLimitSwitchClosed() == 1) {
- m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
- } else if (m_tele.isFwdLimitSwitchClosed() == 1) {
- m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
- }
- }
-
- public double getArmLength() {
- return m_tele.getSelectedSensorPosition() - tele_soft;
- }
-
- public double getArmRotation() {
- return m_pivotEncoder.getAbsolutePosition();
- }
-
- public void runPivotTele(double pivot, double tele) {
- double abs_pivot = Math.toRadians(getArmRotation() - 135);
- double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
- (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
-
- if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
- setRotVel(pivot);
- setTeleVel(tele);
- }
- }
-
- /**
- * Checks that an input is within bounds
- * @param _len length from 0 to 1
- * @param _theta radians from the zero (straight up)
- * @return
- */
- public static boolean checkLimits(double _len, double _theta) {
- var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN;
- var x = len * Math.cos(_theta);
- var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
-
- var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
- return y < minHeight;
- }
-
- boolean tele_softLimit = false;
- double tele_soft = 0;
- public void resetTeleSoftLimit() {
- if (!tele_softLimit) {
- tele_soft = m_tele.getSelectedSensorPosition();
- m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
- m_tele.configReverseSoftLimitThreshold(tele_soft);
- m_tele.configForwardSoftLimitEnable(true);
- m_tele.configReverseSoftLimitEnable(true);
- } else {
- m_tele.configForwardSoftLimitEnable(false);
- m_tele.configReverseSoftLimitEnable(false);
- }
-
- tele_softLimit = !tele_softLimit;
- }
-
- boolean resetable = true;
- boolean tele_reset = true;
-
- @Override
- public void periodic() {
-
- if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
- var tele_soft = m_tele.getSelectedSensorPosition();
- m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
- m_tele.configReverseSoftLimitThreshold(1000 - tele_soft);
- m_tele.configForwardSoftLimitEnable(true);
- m_tele.configReverseSoftLimitEnable(true);
- tele_reset = false;
- } else if (m_tele.isFwdLimitSwitchClosed() == 0) {
- tele_reset = true;
- }
-
- // double x = Math.cos(Math.toRadians(degrees));
- SmartDashboard.putNumber("arm length", getArmLength());
- }
-
- public void killSoftLimits() {
- resetTeleSoftLimit();
- }
-}
\ No newline at end of file
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
+import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.CANCoderConfiguration;
+import frc4388.robot.Constants.ArmConstants;
+import frc4388.utility.DeferredBlock;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Arm extends SubsystemBase {
+ private WPI_TalonFX m_tele;
+ public WPI_TalonFX m_pivot;
+ private CANCoder m_pivotEncoder;
+
+ // Moves arm to distance [dist] then returns new ang
+ public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) {
+ m_tele = tele;
+ m_pivot = pivot;
+ m_pivotEncoder = encoder;
+
+ m_tele.configFactoryDefault();
+ m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+ m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+ m_pivot.configFactoryDefault();
+
+ // * Example of deferred code
+ new DeferredBlock(() -> resetTeleSoftLimit());
+
+ CANCoderConfiguration config = new CANCoderConfiguration();
+ config.magnetOffsetDegrees = ArmConstants.OFFSET;
+ m_pivotEncoder.configAllSettings(config);
+ }
+
+ public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
+ this(pivot, tele, encoder, false);
+ }
+
+ public void setRotVel(double vel) {
+ if (vel > 1) vel = 1;
+
+ var degrees = Math.abs(getArmRotation()) - 135;
+ SmartDashboard.putNumber("arm degrees", degrees);
+ SmartDashboard.putNumber("arm rot vel", vel);
+
+ if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
+ m_pivot.set(ControlMode.PercentOutput, 0);
+ } else if (degrees > 90 && vel > 0) {
+ m_pivot.set(ControlMode.PercentOutput, .15 * vel);
+ } else if (degrees < 25 && vel < 0) {
+ m_pivot.set(ControlMode.PercentOutput, .15 * vel);
+ } else {
+ m_pivot.set(ControlMode.PercentOutput, .3 * vel);
+ }
+ }
+
+ public void setTeleVel(double vel) {
+ m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
+ }
+
+ public void armSetRotation(double rot) {
+ if (rot > 1 || rot < 0) return;
+ // Move arm code
+ m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) +
+ ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
+ }
+
+ public void armSetLength(double len) {
+ if (len > 1 || len < 0) return;
+ // Move arm code
+ m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) +
+ ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+
+ if (m_tele.isRevLimitSwitchClosed() == 1) {
+ m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+ } else if (m_tele.isFwdLimitSwitchClosed() == 1) {
+ m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
+ }
+ }
+
+ public double getArmLength() {
+ return m_tele.getSelectedSensorPosition() - tele_soft;
+ }
+
+ public double getArmRotation() {
+ return m_pivotEncoder.getAbsolutePosition();
+ }
+
+ public void runPivotTele(double pivot, double tele) {
+ double abs_pivot = Math.toRadians(getArmRotation() - 135);
+ double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
+ (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
+
+ if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
+ setRotVel(pivot);
+ setTeleVel(tele);
+ }
+ }
+
+ /**
+ * Checks that an input is within bounds
+ * @param _len length from 0 to 1
+ * @param _theta radians from the zero (straight up)
+ * @return
+ */
+ public static boolean checkLimits(double _len, double _theta) {
+ var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN;
+ var x = len * Math.cos(_theta);
+ var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
+
+ var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
+ return y < minHeight;
+ }
+
+ boolean tele_softLimit = false;
+ double tele_soft = 0;
+ public void resetTeleSoftLimit() {
+ if (!tele_softLimit) {
+ tele_soft = m_tele.getSelectedSensorPosition();
+ m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
+ m_tele.configReverseSoftLimitThreshold(tele_soft);
+ m_tele.configForwardSoftLimitEnable(true);
+ m_tele.configReverseSoftLimitEnable(true);
+ } else {
+ m_tele.configForwardSoftLimitEnable(false);
+ m_tele.configReverseSoftLimitEnable(false);
+ }
+
+ tele_softLimit = !tele_softLimit;
+ }
+
+ boolean resetable = true;
+ boolean tele_reset = true;
+
+ @Override
+ public void periodic() {
+
+ if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
+ var tele_soft = m_tele.getSelectedSensorPosition();
+ m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
+ m_tele.configReverseSoftLimitThreshold(1000 - tele_soft);
+ m_tele.configForwardSoftLimitEnable(true);
+ m_tele.configReverseSoftLimitEnable(true);
+ tele_reset = false;
+ } else if (m_tele.isFwdLimitSwitchClosed() == 0) {
+ tele_reset = true;
+ }
+
+ // double x = Math.cos(Math.toRadians(degrees));
+ SmartDashboard.putNumber("arm length", getArmLength());
+ }
+
+ public void killSoftLimits() {
+ resetTeleSoftLimit();
+ }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java
index 985f70a..f80e444 100644
--- a/src/main/java/frc4388/robot/subsystems/Claw.java
+++ b/src/main/java/frc4388/robot/subsystems/Claw.java
@@ -1,61 +1,61 @@
-package frc4388.robot.subsystems;
-
-import java.util.Timer;
-import java.util.TimerTask;
-
-import com.revrobotics.CANSparkMax;
-
-import edu.wpi.first.wpilibj.PWM;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class Claw extends SubsystemBase {
-
- private final PWM m_leftMotor;
- private final PWM m_rightMotor;
- private final CANSparkMax m_spinnyspin;
-
- private boolean m_open = false;
-
- // Opens claw
- public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
- m_leftMotor = leftMotor;
- m_rightMotor = rightMotor;
- m_spinnyspin = spinnyspin;
-
- setClaw(true);
- }
-
- public void setClaw(boolean open) {
- // Open claw
- m_open = open;
- m_leftMotor.setRaw(m_open ? 1000 : 2000);
- m_rightMotor.setRaw(m_open ? 2000 : 1000);
-
- if (!m_open) {
- m_spinnyspin.set(-0.2);
-
- new Timer().schedule(new TimerTask() {
- @Override
- public void run() {
- nospinnyspin();
- }
- }, 750);
- }
- }
-
- public void toggle() {
- setClaw(!m_open);
- }
-
- public boolean isClawOpen() {
- return m_open;
- }
-
- public void yesspinnyspin() {
- m_spinnyspin.set(0.2);
- }
-
- public void nospinnyspin() {
- m_spinnyspin.set(0);
- }
-}
+package frc4388.robot.subsystems;
+
+import java.util.Timer;
+import java.util.TimerTask;
+
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Claw extends SubsystemBase {
+
+ private final PWM m_leftMotor;
+ private final PWM m_rightMotor;
+ private final CANSparkMax m_spinnyspin;
+
+ private boolean m_open = false;
+
+ // Opens claw
+ public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
+ m_leftMotor = leftMotor;
+ m_rightMotor = rightMotor;
+ m_spinnyspin = spinnyspin;
+
+ setClaw(true);
+ }
+
+ public void setClaw(boolean open) {
+ // Open claw
+ m_open = open;
+ m_leftMotor.setRaw(m_open ? 1000 : 2000);
+ m_rightMotor.setRaw(m_open ? 2000 : 1000);
+
+ if (!m_open) {
+ m_spinnyspin.set(-0.2);
+
+ new Timer().schedule(new TimerTask() {
+ @Override
+ public void run() {
+ nospinnyspin();
+ }
+ }, 750);
+ }
+ }
+
+ public void toggle() {
+ setClaw(!m_open);
+ }
+
+ public boolean isClawOpen() {
+ return m_open;
+ }
+
+ public void yesspinnyspin() {
+ m_spinnyspin.set(0.2);
+ }
+
+ public void nospinnyspin() {
+ m_spinnyspin.set(0);
+ }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java
index 0d4af80..8d828c8 100644
--- a/src/main/java/frc4388/robot/subsystems/LED.java
+++ b/src/main/java/frc4388/robot/subsystems/LED.java
@@ -1,63 +1,63 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import frc4388.robot.Constants.LEDConstants;
-import frc4388.utility.LEDPatterns;
-
-/**
- * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
- * Driver
- */
-public class LED extends SubsystemBase {
-
- private LEDPatterns m_currentPattern;
- private Spark m_LEDController;
-
- /**
- * Add your docs here.
- */
- public LED(Spark LEDController){
- m_LEDController = LEDController;
- setPattern(LEDConstants.DEFAULT_PATTERN);
- updateLED();
- System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
- }
-
- @Override
- public void periodic(){
- SmartDashboard.putNumber("LED", m_currentPattern.getValue());
- }
-
- /**
- * Add your docs here.
- */
- public void updateLED(){
- m_LEDController.set(m_currentPattern.getValue());
- }
-
- /**
- * Add your docs here.
- */
- public void setPattern(LEDPatterns pattern){
- m_currentPattern = pattern;
- m_LEDController.set(m_currentPattern.getValue());
- }
-
- /**
- * Add your docs here.
- * @return
- */
- public LEDPatterns getPattern() {
- return m_currentPattern;
- }
-}
\ No newline at end of file
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.LEDPatterns;
+
+/**
+ * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
+ * Driver
+ */
+public class LED extends SubsystemBase {
+
+ private LEDPatterns m_currentPattern;
+ private Spark m_LEDController;
+
+ /**
+ * Add your docs here.
+ */
+ public LED(Spark LEDController){
+ m_LEDController = LEDController;
+ setPattern(LEDConstants.DEFAULT_PATTERN);
+ updateLED();
+ System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
+ }
+
+ @Override
+ public void periodic(){
+ SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+ }
+
+ /**
+ * Add your docs here.
+ */
+ public void updateLED(){
+ m_LEDController.set(m_currentPattern.getValue());
+ }
+
+ /**
+ * Add your docs here.
+ */
+ public void setPattern(LEDPatterns pattern){
+ m_currentPattern = pattern;
+ m_LEDController.set(m_currentPattern.getValue());
+ }
+
+ /**
+ * Add your docs here.
+ * @return
+ */
+ public LEDPatterns getPattern() {
+ return m_currentPattern;
+ }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java
index 9d1289b..399e6f0 100644
--- a/src/main/java/frc4388/robot/subsystems/Limelight.java
+++ b/src/main/java/frc4388/robot/subsystems/Limelight.java
@@ -1,165 +1,165 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.subsystems;
-
-import java.io.IOException;
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Optional;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.common.hardware.VisionLEDMode;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-import org.photonvision.targeting.TargetCorner;
-
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.VisionConstants;
-
-public class Limelight extends SubsystemBase {
-
- private PhotonCamera cam;
- private PhotonPoseEstimator photonPoseEstimator;
-
- private boolean lightOn;
-
- /** Creates a new Limelight. */
- public Limelight() {
- cam = new PhotonCamera(VisionConstants.NAME);
- cam.setDriverMode(false);
- }
-
- public void setLEDs(boolean on) {
- lightOn = on;
- cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
- }
-
- public void toggleLEDs() {
- lightOn = !lightOn;
- cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
- }
-
- public void setDriverMode(boolean driverMode) {
- cam.setDriverMode(driverMode);
- }
-
- public void setToLimePipeline() {
- cam.setPipelineIndex(1);
- setLEDs(true);
- }
-
- public void setToAprilPipeline() {
- cam.setPipelineIndex(0);
- setLEDs(false);
- }
-
- public PhotonTrackedTarget getAprilPoint() {
- if (!cam.isConnected()) return null;
-
- PhotonPipelineResult result = cam.getLatestResult();
-
- if (!result.hasTargets()) return null;
-
- return result.getBestTarget();
- }
-
- private List getAprilCorners() {
- if (!cam.isConnected()) return null;
-
- PhotonPipelineResult result = cam.getLatestResult();
-
- if (!result.hasTargets()) return null;
-
- return result.getBestTarget().getDetectedCorners();
- }
-
- public double getAprilSkew() {
- List corners = getAprilCorners();
- ArrayList bottomSide = getAprilBottomSide(corners);
-
- if (bottomSide == null) return 0;
-
- TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
- TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
-
- return bottomLeft.y - bottomRight.y;
- }
-
- private ArrayList getAprilBottomSide(List box) {
- if (box == null) return null;
-
- ArrayList bottomSide = new ArrayList<>();
-
- TargetCorner l1 = new TargetCorner(-1, -1);
- TargetCorner l2 = new TargetCorner(-1, -1);
-
- for (TargetCorner c : box) {
- if (c.y > l1.y) l1 = c;
- }
-
- for (TargetCorner c : box) {
- if (c.y == l1.y) continue;
- if (c.y > l2.y) l2 = c;
- }
-
- bottomSide.add(l1);
- bottomSide.add(l2);
-
- return bottomSide;
- }
-
- public double getDistanceToApril() {
- PhotonTrackedTarget aprilPoint = getAprilPoint();
- if (aprilPoint == null) return -1;
-
- double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
- double theta = 35.0 + aprilPoint.getPitch();
-
- double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
- return distanceToApril;
- }
-
- public PhotonTrackedTarget getLowestTape() {
- if (!cam.isConnected()) return null;
-
- PhotonPipelineResult result = cam.getLatestResult();
-
- if (!result.hasTargets()) return null;
-
- ArrayList points = (ArrayList) result.getTargets();
-
- PhotonTrackedTarget lowest = points.get(0);
- for (PhotonTrackedTarget point : points) {
- if (point.getPitch() < lowest.getPitch()) {
- lowest = point;
- }
- }
-
- return lowest;
- }
-
- public double getDistanceToTape() {
- PhotonTrackedTarget tapePoint = getLowestTape();
- if (tapePoint == null) return -1;
-
- double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
- double theta = 35.0 + tapePoint.getPitch();
-
- double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
- return distanceToTape;
- }
-
- @Override
- public void periodic() {}
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.common.hardware.VisionLEDMode;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.VisionConstants;
+
+public class Limelight extends SubsystemBase {
+
+ private PhotonCamera cam;
+ private PhotonPoseEstimator photonPoseEstimator;
+
+ private boolean lightOn;
+
+ /** Creates a new Limelight. */
+ public Limelight() {
+ cam = new PhotonCamera(VisionConstants.NAME);
+ cam.setDriverMode(false);
+ }
+
+ public void setLEDs(boolean on) {
+ lightOn = on;
+ cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+ }
+
+ public void toggleLEDs() {
+ lightOn = !lightOn;
+ cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+ }
+
+ public void setDriverMode(boolean driverMode) {
+ cam.setDriverMode(driverMode);
+ }
+
+ public void setToLimePipeline() {
+ cam.setPipelineIndex(1);
+ setLEDs(true);
+ }
+
+ public void setToAprilPipeline() {
+ cam.setPipelineIndex(0);
+ setLEDs(false);
+ }
+
+ public PhotonTrackedTarget getAprilPoint() {
+ if (!cam.isConnected()) return null;
+
+ PhotonPipelineResult result = cam.getLatestResult();
+
+ if (!result.hasTargets()) return null;
+
+ return result.getBestTarget();
+ }
+
+ private List getAprilCorners() {
+ if (!cam.isConnected()) return null;
+
+ PhotonPipelineResult result = cam.getLatestResult();
+
+ if (!result.hasTargets()) return null;
+
+ return result.getBestTarget().getDetectedCorners();
+ }
+
+ public double getAprilSkew() {
+ List corners = getAprilCorners();
+ ArrayList bottomSide = getAprilBottomSide(corners);
+
+ if (bottomSide == null) return 0;
+
+ TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
+ TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
+
+ return bottomLeft.y - bottomRight.y;
+ }
+
+ private ArrayList getAprilBottomSide(List box) {
+ if (box == null) return null;
+
+ ArrayList bottomSide = new ArrayList<>();
+
+ TargetCorner l1 = new TargetCorner(-1, -1);
+ TargetCorner l2 = new TargetCorner(-1, -1);
+
+ for (TargetCorner c : box) {
+ if (c.y > l1.y) l1 = c;
+ }
+
+ for (TargetCorner c : box) {
+ if (c.y == l1.y) continue;
+ if (c.y > l2.y) l2 = c;
+ }
+
+ bottomSide.add(l1);
+ bottomSide.add(l2);
+
+ return bottomSide;
+ }
+
+ public double getDistanceToApril() {
+ PhotonTrackedTarget aprilPoint = getAprilPoint();
+ if (aprilPoint == null) return -1;
+
+ double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
+ double theta = 35.0 + aprilPoint.getPitch();
+
+ double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
+ return distanceToApril;
+ }
+
+ public PhotonTrackedTarget getLowestTape() {
+ if (!cam.isConnected()) return null;
+
+ PhotonPipelineResult result = cam.getLatestResult();
+
+ if (!result.hasTargets()) return null;
+
+ ArrayList points = (ArrayList) result.getTargets();
+
+ PhotonTrackedTarget lowest = points.get(0);
+ for (PhotonTrackedTarget point : points) {
+ if (point.getPitch() < lowest.getPitch()) {
+ lowest = point;
+ }
+ }
+
+ return lowest;
+ }
+
+ public double getDistanceToTape() {
+ PhotonTrackedTarget tapePoint = getLowestTape();
+ if (tapePoint == null) return -1;
+
+ double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
+ double theta = 35.0 + tapePoint.getPitch();
+
+ double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
+ return distanceToTape;
+ }
+
+ @Override
+ public void periodic() {}
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Location.java b/src/main/java/frc4388/robot/subsystems/Location.java
index 0bf5322..0501adf 100644
--- a/src/main/java/frc4388/robot/subsystems/Location.java
+++ b/src/main/java/frc4388/robot/subsystems/Location.java
@@ -1,27 +1,27 @@
-package frc4388.robot.subsystems;
-
-import frc4388.robot.subsystems.Apriltags.Tag;
-
-public class Location {
- final Apriltags apriltag = new Apriltags();
-
- private boolean isLimelight = false;
- private boolean isApriltag = false;
-
- //Determines which source to get pos and rot from and also resets
- private void reoderPrio(){
- isLimelight = false; //If limelight gets position and if within a certain range of poles
- isApriltag = apriltag.isAprilTag();
- }
-
- public Tag getPosRot() {
- reoderPrio();
- if(isApriltag){
- return apriltag.getTagPosRot();
- } else if (isLimelight) {
- return null;
- }
-
- return null;
- }
-}
+package frc4388.robot.subsystems;
+
+import frc4388.robot.subsystems.Apriltags.Tag;
+
+public class Location {
+ final Apriltags apriltag = new Apriltags();
+
+ private boolean isLimelight = false;
+ private boolean isApriltag = false;
+
+ //Determines which source to get pos and rot from and also resets
+ private void reoderPrio(){
+ isLimelight = false; //If limelight gets position and if within a certain range of poles
+ isApriltag = apriltag.isAprilTag();
+ }
+
+ public Tag getPosRot() {
+ reoderPrio();
+ if(isApriltag){
+ return apriltag.getTagPosRot();
+ } else if (isLimelight) {
+ return null;
+ }
+
+ return null;
+ }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index 055230b..974a817 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -1,195 +1,195 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.utility.RobotGyro;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-public class SwerveDrive extends SubsystemBase {
-
- private SwerveModule leftFront;
- private SwerveModule rightFront;
- private SwerveModule leftBack;
- private SwerveModule rightBack;
-
- 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 SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
-
- private RobotGyro gyro;
-
- public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
-
- public double rotTarget = 0.0;
- 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;
-
- 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;
-
- 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(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
- } else {
- // Create robot-relative speeds.
- chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
- }
- setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
- }
-
- /**
- * 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 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 stopModules() {
- for (SwerveModule module : this.modules) {
- module.stop();
- }
- }
-
- public SwerveDriveKinematics getKinematics() {
- return this.kinematics;
- }
-
- @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;
- }
- }
-
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.RobotGyro;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class SwerveDrive extends SubsystemBase {
+
+ private SwerveModule leftFront;
+ private SwerveModule rightFront;
+ private SwerveModule leftBack;
+ private SwerveModule rightBack;
+
+ 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 SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
+
+ private RobotGyro gyro;
+
+ public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+
+ public double rotTarget = 0.0;
+ 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;
+
+ 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;
+
+ 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(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+ } else {
+ // Create robot-relative speeds.
+ chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+ }
+ setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
+ }
+
+ /**
+ * 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 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 stopModules() {
+ for (SwerveModule module : this.modules) {
+ module.stop();
+ }
+ }
+
+ public SwerveDriveKinematics getKinematics() {
+ return this.kinematics;
+ }
+
+ @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;
+ }
+ }
+
+}
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index 1ddab51..107b895 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -1,161 +1,161 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.subsystems;
-
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.Constants.SwerveDriveConstants;
-import frc4388.utility.Gains;
-
-public class SwerveModule extends SubsystemBase {
- private WPI_TalonFX driveMotor;
- private WPI_TalonFX angleMotor;
- private CANCoder encoder;
-
- public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
-
- /** Creates a new SwerveModule. */
- public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
- this.driveMotor = driveMotor;
- this.angleMotor = angleMotor;
- this.encoder = encoder;
-
- TalonFXConfiguration angleConfig = new TalonFXConfiguration();
- angleConfig.slot0.kP = swerveGains.kP;
- angleConfig.slot0.kI = swerveGains.kI;
- angleConfig.slot0.kD = swerveGains.kD;
-
- // use the CANcoder as the remote sensor for the primary TalonFX PID
- angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
- angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
- angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
- angleMotor.configAllSettings(angleConfig);
-
- encoder.configMagnetOffset(offset);
-
- driveMotor.setSelectedSensorPosition(0);
- driveMotor.config_kP(0, 0.2);
- }
-
- /**
- * Get the drive motor of the SwerveModule
- * @return the drive motor of the SwerveModule
- */
- public WPI_TalonFX getDriveMotor() {
- return this.driveMotor;
- }
-
- /**
- * Get the angle motor of the SwerveModule
- * @return the angle motor of the SwerveModule
- */
- public WPI_TalonFX getAngleMotor() {
- return this.angleMotor;
- }
-
- /**
- * Get the CANcoder of the SwerveModule
- * @return the CANcoder of the SwerveModule
- */
- public CANCoder getEncoder() {
- return this.encoder;
- }
-
- /**
- * Get the angle of a SwerveModule as a Rotation2d
- * @return the angle of a SwerveModule as a Rotation2d
- */
- public Rotation2d getAngle() {
- // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
- return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
- }
-
- public double getAngularVel() {
- return this.angleMotor.getSelectedSensorVelocity();
- }
-
- public double getDrivePos() {
- return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
- }
-
- public double getDriveVel() {
- return this.driveMotor.getSelectedSensorVelocity(0);
- }
-
- public void stop() {
- driveMotor.set(0);
- angleMotor.set(0);
- }
-
- public void rotateToAngle(double angle) {
- angleMotor.set(TalonFXControlMode.Position, angle);
- }
-
- /**
- * Get state of swerve module
- * @return speed in m/s and angle in degrees
- */
- public SwerveModuleState getState() {
- return new SwerveModuleState(
- Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
- getAngle()
- );
- }
-
- /**
- * Returns the current position of the SwerveModule
- * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
- */
- public SwerveModulePosition getPosition() {
- return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
- }
-
- /**
- * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
- * @param desiredState a SwerveModuleState representing the desired new state of the module
- */
- public void setDesiredState(SwerveModuleState desiredState) {
- Rotation2d currentRotation = this.getAngle();
-
- SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
-
- // calculate the difference between our current rotational position and our new rotational position
- Rotation2d rotationDelta = state.angle.minus(currentRotation);
-
- // calculate the new absolute position of the SwerveModule based on the difference in rotation
- double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
-
- // convert the CANCoder from its position reading to ticks
- double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
-
- angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
-
- double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-
- driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
- }
-
- public void reset(double position) {
- encoder.setPositionToAbsolute();
- }
-
- public double getCurrent() {
- return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
- }
-
- public double getVoltage() {
- return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.Gains;
+
+public class SwerveModule extends SubsystemBase {
+ private WPI_TalonFX driveMotor;
+ private WPI_TalonFX angleMotor;
+ private CANCoder encoder;
+
+ public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
+
+ /** Creates a new SwerveModule. */
+ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
+ this.driveMotor = driveMotor;
+ this.angleMotor = angleMotor;
+ this.encoder = encoder;
+
+ TalonFXConfiguration angleConfig = new TalonFXConfiguration();
+ angleConfig.slot0.kP = swerveGains.kP;
+ angleConfig.slot0.kI = swerveGains.kI;
+ angleConfig.slot0.kD = swerveGains.kD;
+
+ // use the CANcoder as the remote sensor for the primary TalonFX PID
+ angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
+ angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
+ angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
+ angleMotor.configAllSettings(angleConfig);
+
+ encoder.configMagnetOffset(offset);
+
+ driveMotor.setSelectedSensorPosition(0);
+ driveMotor.config_kP(0, 0.2);
+ }
+
+ /**
+ * Get the drive motor of the SwerveModule
+ * @return the drive motor of the SwerveModule
+ */
+ public WPI_TalonFX getDriveMotor() {
+ return this.driveMotor;
+ }
+
+ /**
+ * Get the angle motor of the SwerveModule
+ * @return the angle motor of the SwerveModule
+ */
+ public WPI_TalonFX getAngleMotor() {
+ return this.angleMotor;
+ }
+
+ /**
+ * Get the CANcoder of the SwerveModule
+ * @return the CANcoder of the SwerveModule
+ */
+ public CANCoder getEncoder() {
+ return this.encoder;
+ }
+
+ /**
+ * Get the angle of a SwerveModule as a Rotation2d
+ * @return the angle of a SwerveModule as a Rotation2d
+ */
+ public Rotation2d getAngle() {
+ // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
+ return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
+ }
+
+ public double getAngularVel() {
+ return this.angleMotor.getSelectedSensorVelocity();
+ }
+
+ public double getDrivePos() {
+ return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
+ }
+
+ public double getDriveVel() {
+ return this.driveMotor.getSelectedSensorVelocity(0);
+ }
+
+ public void stop() {
+ driveMotor.set(0);
+ angleMotor.set(0);
+ }
+
+ public void rotateToAngle(double angle) {
+ angleMotor.set(TalonFXControlMode.Position, angle);
+ }
+
+ /**
+ * Get state of swerve module
+ * @return speed in m/s and angle in degrees
+ */
+ public SwerveModuleState getState() {
+ return new SwerveModuleState(
+ Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
+ getAngle()
+ );
+ }
+
+ /**
+ * Returns the current position of the SwerveModule
+ * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
+ */
+ public SwerveModulePosition getPosition() {
+ return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
+ }
+
+ /**
+ * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
+ * @param desiredState a SwerveModuleState representing the desired new state of the module
+ */
+ public void setDesiredState(SwerveModuleState desiredState) {
+ Rotation2d currentRotation = this.getAngle();
+
+ SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
+
+ // calculate the difference between our current rotational position and our new rotational position
+ Rotation2d rotationDelta = state.angle.minus(currentRotation);
+
+ // calculate the new absolute position of the SwerveModule based on the difference in rotation
+ double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
+
+ // convert the CANCoder from its position reading to ticks
+ double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
+
+ angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
+
+ double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+
+ driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
+ }
+
+ public void reset(double position) {
+ encoder.setPositionToAbsolute();
+ }
+
+ public double getCurrent() {
+ return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
+ }
+
+ public double getVoltage() {
+ return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
+ }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java
index 371f621..61d74dd 100644
--- a/src/main/java/frc4388/robot/subsystems/Vision.java
+++ b/src/main/java/frc4388/robot/subsystems/Vision.java
@@ -1,38 +1,38 @@
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-public class Vision {
- private final NetworkTableEntry m_isTags;
- private final NetworkTableEntry m_xPoses;
- private final NetworkTableEntry m_yPoses;
- private final NetworkTableEntry m_zPoses;
-
- public Vision() {
- final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-
- m_isTags = tagTable.getEntry("IsTag");
- m_xPoses = tagTable.getEntry("TagPosX");
- m_yPoses = tagTable.getEntry("TagPosY");
- m_zPoses = tagTable.getEntry("TagPosZ");
- }
-
- public AprilTag[] getAprilTags() {
- if (!m_isTags.getBoolean(false)) return new AprilTag[0];
-
- double xarr[] = m_xPoses.getDoubleArray(new double[] {});
- double yarr[] = m_yPoses.getDoubleArray(new double[] {});
- double zarr[] = m_zPoses.getDoubleArray(new double[] {});
-
- AprilTag tags[] = new AprilTag[xarr.length];
- for (int i = 0; i < tags.length; i++) {
- tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
- }
-
- return tags;
- }
-}
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Vision {
+ private final NetworkTableEntry m_isTags;
+ private final NetworkTableEntry m_xPoses;
+ private final NetworkTableEntry m_yPoses;
+ private final NetworkTableEntry m_zPoses;
+
+ public Vision() {
+ final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+ m_isTags = tagTable.getEntry("IsTag");
+ m_xPoses = tagTable.getEntry("TagPosX");
+ m_yPoses = tagTable.getEntry("TagPosY");
+ m_zPoses = tagTable.getEntry("TagPosZ");
+ }
+
+ public AprilTag[] getAprilTags() {
+ if (!m_isTags.getBoolean(false)) return new AprilTag[0];
+
+ double xarr[] = m_xPoses.getDoubleArray(new double[] {});
+ double yarr[] = m_yPoses.getDoubleArray(new double[] {});
+ double zarr[] = m_zPoses.getDoubleArray(new double[] {});
+
+ AprilTag tags[] = new AprilTag[xarr.length];
+ for (int i = 0; i < tags.length; i++) {
+ tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
+ }
+
+ return tags;
+ }
+}
diff --git a/src/main/java/frc4388/utility/AbhiIsADumbass.java b/src/main/java/frc4388/utility/AbhiIsADumbass.java
index 14a9515..59d06df 100644
--- a/src/main/java/frc4388/utility/AbhiIsADumbass.java
+++ b/src/main/java/frc4388/utility/AbhiIsADumbass.java
@@ -1,43 +1,43 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.utility;
-
-/** Exception that occurs if the limelight can't see enough points
- * @author Abhi Sachi
- * @author Aarav Shah
- */
-public class AbhiIsADumbass extends Exception {
- /**
- * Creates new AbhiIsADumbass with error text 'null'
- */
- public AbhiIsADumbass() {
- super("Unable to see sufficient vision points (Abhi is a dumbass)");
- }
-
- /** Creates new AbhiIsADumbass with error text message
- *
- * @param message Error text message
- */
- public AbhiIsADumbass(String message) {
- super(message);
- }
-
- /** Creates new AbhiIsADumbass with error text message and detailed stack trace
- *
- * @param message Error text message
- * @param cause Root cause of error
- */
- public AbhiIsADumbass(String message, Throwable cause) {
- super(message, cause);
- }
-
- /** Creates new AbhiIsADumbass with error text 'null' and detailed stack trace
- *
- * @param cause Root cause of error
- */
- public AbhiIsADumbass(Throwable cause) {
- super("Unable to see sufficient vision points (Abhi is a dumbass)", cause);
- }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+/** Exception that occurs if the limelight can't see enough points
+ * @author Abhi Sachi
+ * @author Aarav Shah
+ */
+public class AbhiIsADumbass extends Exception {
+ /**
+ * Creates new AbhiIsADumbass with error text 'null'
+ */
+ public AbhiIsADumbass() {
+ super("Unable to see sufficient vision points (Abhi is a dumbass)");
+ }
+
+ /** Creates new AbhiIsADumbass with error text message
+ *
+ * @param message Error text message
+ */
+ public AbhiIsADumbass(String message) {
+ super(message);
+ }
+
+ /** Creates new AbhiIsADumbass with error text message and detailed stack trace
+ *
+ * @param message Error text message
+ * @param cause Root cause of error
+ */
+ public AbhiIsADumbass(String message, Throwable cause) {
+ super(message, cause);
+ }
+
+ /** Creates new AbhiIsADumbass with error text 'null' and detailed stack trace
+ *
+ * @param cause Root cause of error
+ */
+ public AbhiIsADumbass(Throwable cause) {
+ super("Unable to see sufficient vision points (Abhi is a dumbass)", cause);
+ }
+}
diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java
index c2ad269..8d269e3 100644
--- a/src/main/java/frc4388/utility/AprilTag.java
+++ b/src/main/java/frc4388/utility/AprilTag.java
@@ -1,13 +1,13 @@
-package frc4388.utility;
-
-// This is a seperate class in case I want to encode rotation or other
-// information about the tag
-public class AprilTag {
- public final double x, y, z;
-
- public AprilTag(double _x, double _y, double _z) {
- x = _x;
- y = _y;
- z = _z;
- }
-}
+package frc4388.utility;
+
+// This is a seperate class in case I want to encode rotation or other
+// information about the tag
+public class AprilTag {
+ public final double x, y, z;
+
+ public AprilTag(double _x, double _y, double _z) {
+ x = _x;
+ y = _y;
+ z = _z;
+ }
+}
diff --git a/src/main/java/frc4388/utility/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java
index 20d3c57..1a8e397 100644
--- a/src/main/java/frc4388/utility/DeferredBlock.java
+++ b/src/main/java/frc4388/utility/DeferredBlock.java
@@ -1,23 +1,23 @@
-package frc4388.utility;
-
-import java.util.ArrayList;
-
-public class DeferredBlock {
- private static ArrayList m_blocks = new ArrayList<>();
- private static boolean m_hasRun = false;
-
- public DeferredBlock(Runnable block) {
- m_blocks.add(block);
- }
-
- public static void execute() {
- if (m_hasRun) return;
-
- for (Runnable block : m_blocks) {
- block.run();
- }
-
- m_blocks.clear(); // for garbage collection
- m_hasRun = true;
- }
-}
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+public class DeferredBlock {
+ private static ArrayList m_blocks = new ArrayList<>();
+ private static boolean m_hasRun = false;
+
+ public DeferredBlock(Runnable block) {
+ m_blocks.add(block);
+ }
+
+ public static void execute() {
+ if (m_hasRun) return;
+
+ for (Runnable block : m_blocks) {
+ block.run();
+ }
+
+ m_blocks.clear(); // for garbage collection
+ m_hasRun = true;
+ }
+}
diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java
index 7a3a026..a1ef977 100644
--- a/src/main/java/frc4388/utility/Gains.java
+++ b/src/main/java/frc4388/utility/Gains.java
@@ -1,83 +1,83 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.utility;
-
-/** Add your docs here. */
-public class Gains {
- public double kP;
- public double kI;
- public double kD;
- public double kF;
- public int kIZone;
- public double kPeakOutput;
- public double kMaxOutput;
- public double kMinOutput;
-
- /**
- * Creates Gains object for PIDs
- * @param kP The P value.
- * @param kI The I value.
- * @param kD The D value.
- * @param kF The F value.
- * @param kIZone The zone of the I value.
- * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
- */
- public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
- this.kP = kP;
- this.kI = kI;
- this.kD = kD;
- this.kF = kF;
- this.kIZone = kIZone;
- this.kPeakOutput = kPeakOutput;
- this.kMaxOutput = kPeakOutput;
- this.kMinOutput = -kPeakOutput;
- }
-
- /**
- * Creates Gains object for PIDs
- * @param kP The P value.
- * @param kI The I value.
- * @param kD The D value.
- * @param kF The F value.
- * @param kIZone The zone of the I value.
- */
- public Gains(double kP, double kI, double kD, double kF, int kIZone) {
- this.kP = kP;
- this.kI = kI;
- this.kD = kD;
- this.kF = kF;
- this.kIZone = kIZone;
- this.kPeakOutput = 1.0;
- this.kMaxOutput = 1.0;
- this.kMinOutput = -1.0;
- }
-
- public Gains(double kP, double kI, double kD) {
- this.kP = kP;
- this.kI = kI;
- this.kD = kD;
- }
-
- /**
- * Creates Gains object for PIDs
- * @param kP The P value.
- * @param kI The I value.
- * @param kD The D value.
- * @param kF The F value.
- * @param kIZone The zone of the I value.
- * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
- * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
- */
- public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
- this.kP = kP;
- this.kI = kI;
- this.kD = kD;
- this.kF = kF;
- this.kIZone = kIZone;
- this.kMaxOutput = kMaxOutput;
- this.kMinOutput = kMinOutput;
- this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
- }
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+/** Add your docs here. */
+public class Gains {
+ public double kP;
+ public double kI;
+ public double kD;
+ public double kF;
+ public int kIZone;
+ public double kPeakOutput;
+ public double kMaxOutput;
+ public double kMinOutput;
+
+ /**
+ * Creates Gains object for PIDs
+ * @param kP The P value.
+ * @param kI The I value.
+ * @param kD The D value.
+ * @param kF The F value.
+ * @param kIZone The zone of the I value.
+ * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
+ */
+ public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
+ this.kP = kP;
+ this.kI = kI;
+ this.kD = kD;
+ this.kF = kF;
+ this.kIZone = kIZone;
+ this.kPeakOutput = kPeakOutput;
+ this.kMaxOutput = kPeakOutput;
+ this.kMinOutput = -kPeakOutput;
+ }
+
+ /**
+ * Creates Gains object for PIDs
+ * @param kP The P value.
+ * @param kI The I value.
+ * @param kD The D value.
+ * @param kF The F value.
+ * @param kIZone The zone of the I value.
+ */
+ public Gains(double kP, double kI, double kD, double kF, int kIZone) {
+ this.kP = kP;
+ this.kI = kI;
+ this.kD = kD;
+ this.kF = kF;
+ this.kIZone = kIZone;
+ this.kPeakOutput = 1.0;
+ this.kMaxOutput = 1.0;
+ this.kMinOutput = -1.0;
+ }
+
+ public Gains(double kP, double kI, double kD) {
+ this.kP = kP;
+ this.kI = kI;
+ this.kD = kD;
+ }
+
+ /**
+ * Creates Gains object for PIDs
+ * @param kP The P value.
+ * @param kI The I value.
+ * @param kD The D value.
+ * @param kF The F value.
+ * @param kIZone The zone of the I value.
+ * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
+ * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
+ */
+ public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
+ this.kP = kP;
+ this.kI = kI;
+ this.kD = kD;
+ this.kF = kF;
+ this.kIZone = kIZone;
+ this.kMaxOutput = kMaxOutput;
+ this.kMinOutput = kMinOutput;
+ this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
+ }
}
\ No newline at end of file
diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/LEDPatterns.java
index 6df032c..807de98 100644
--- a/src/main/java/frc4388/utility/LEDPatterns.java
+++ b/src/main/java/frc4388/utility/LEDPatterns.java
@@ -1,45 +1,45 @@
-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),
- 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),
- RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
- RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
- RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
- 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),
- 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),
- 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),
- 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_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;
- }
-}
\ No newline at end of file
+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),
+ 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),
+ RAINBOW_TWINKLES(-0.55f), PARTY_TWINKLES(-0.53f), OCEAN_TWINKLES(-0.51f), LAVA_TWINKLES(-0.49f), FOREST_TWINKLES(-0.47f),
+ RAINBOW_WAVES(-0.45f), PARTY_WAVES(-0.43f), OCEAN_WAVES(-0.41f), LAVA_WAVES(-0.39f), FOREST_WAVES(-0.37f),
+ RED_SCANNER(-0.35f), GRAY_SCANNER(-0.33f), RED_CHASE(-0.31f), BLUE_CHASE(-0.29f), GRAY_CHASE(-0.27f), RED_HEARTBEAT(-0.25f),
+ 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),
+ 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),
+ 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),
+ 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_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;
+ }
+}
diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index a037914..4da3e13 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/src/main/java/frc4388/utility/RobotGyro.java
@@ -1,200 +1,200 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.utility;
-
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
-import com.kauailabs.navx.frc.AHRS;
-
-// import edu.wpi.first.wpilibj.GyroBase;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.math.MathUtil;
-
-/**
- * Gyro class that allows for interchangeable use between a pigeon and a navX
- */
-public class RobotGyro implements Gyro {
- private RobotTime m_robotTime = RobotTime.getInstance();
-
- private WPI_Pigeon2 m_pigeon = null;
- private AHRS m_navX = null;
- public boolean m_isGyroAPigeon; //true if pigeon, false if navX
-
- private double m_lastPigeonAngle;
- private double m_deltaPigeonAngle;
-
- private double pitchZero = 0;
- private double rollZero = 0;
-
- /**
- * Creates a Gyro based on a pigeon
- * @param gyro the gyroscope to use for Gyro
- */
- public RobotGyro(WPI_Pigeon2 gyro) {
- m_pigeon = gyro;
- m_isGyroAPigeon = true;
- }
-
- /**
- * Creates a Gyro based on a navX
- * @param gyro the gyroscope to use for Gyro
- */
- public RobotGyro(AHRS gyro){
- m_navX = gyro;
- m_isGyroAPigeon = false;
- }
-
- /**
- * Resets yaw, pitch, and roll.
- */
- public void resetZeroValues() {
- if (!m_isGyroAPigeon) return;
-
- pitchZero = m_pigeon.getPitch();
- rollZero = m_pigeon.getRoll();
- }
-
- /**
- * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
- * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
- */
- public void updatePigeonDeltas() {
- double currentPigeonAngle = getAngle();
- m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
- m_lastPigeonAngle = currentPigeonAngle;
- }
-
- /**
- * NavX:
- *
Calibrate the gyro by running for a number of samples and computing the center value. Then use
- * the center value as the Accumulator center value for subsequent measurements. It's important to
- * make sure that the robot is not moving while the centering calculations are in progress, this
- * is typically done when the robot is first turned on while it's sitting at rest before the
- * competition starts.
- *
- *
Pigeon:
- *
Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
- * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
- * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
- * make sure that the robot is not moving while the tempurature calculations are in progress, this
- * is typically done when the robot is first turned on while it's sitting at rest before the
- * competition starts.
- */
- @Override
- public void calibrate() {
- if (m_isGyroAPigeon) {
- m_pigeon.calibrate();
- } else {
- m_navX.calibrate();
- }
- }
-
- @Override
- public void reset() {
- resetZeroValues();
-
- if (m_isGyroAPigeon) {
- m_pigeon.setYaw(0);
- } else {
- m_navX.reset();
- }
- }
-
- /**
- * 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];
- m_pigeon.getYawPitchRoll(ypr);
-
- return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
- }
-
- @Override
- public double getAngle() {
- if (m_isGyroAPigeon) {
- return getPigeonAngles()[0];
- } else {
- return m_navX.getAngle();
- }
- }
-
- public double getYaw() {
- return this.getAngle();
- }
-
- /**
- * Gets an absolute heading of the robot
- * @return heading from -180 to 180 degrees
- */
- public double getHeading() {
- return getHeading(getAngle());
- }
-
- /**
- * Gets an absolute heading of the robot
- * @return heading from -180 to 180 degrees
- */
- public double getHeading(double angle) {
- return Math.IEEEremainder(angle, 360);
- }
-
- /**
- * Returns the current pitch value (in degrees, from -90 to 90)
- * reported by the sensor. Pitch is a measure of rotation around
- * the Y Axis.
- * @return The current pitch value in degrees (-90 to 90).
- */
- public double getPitch() {
- if (m_isGyroAPigeon) {
- return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
- } else {
- return MathUtil.clamp(m_navX.getPitch(), -90, 90);
- }
- }
-
- /**
- * Returns the current roll value (in degrees, from -90 to 90)
- * reported by the sensor. Roll is a measure of rotation around
- * the X Axis.
- * @return The current roll value in degrees (-90 to 90).
- */
- public double getRoll() {
- if (m_isGyroAPigeon) {
- return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
- } else {
- return MathUtil.clamp(m_navX.getRoll(), -90, 90);
- }
- }
-
- @Override
- public double getRate() {
- if (m_isGyroAPigeon) {
- return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
- } else {
- return m_navX.getRate();
- }
- }
-
- public WPI_Pigeon2 getPigeon(){
- return m_pigeon;
- }
-
- public AHRS getNavX(){
- return m_navX;
- }
-
- @Override
- public void close() throws Exception {
-
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.utility;
+
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.kauailabs.navx.frc.AHRS;
+
+// import edu.wpi.first.wpilibj.GyroBase;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.math.MathUtil;
+
+/**
+ * Gyro class that allows for interchangeable use between a pigeon and a navX
+ */
+public class RobotGyro implements Gyro {
+ private RobotTime m_robotTime = RobotTime.getInstance();
+
+ private WPI_Pigeon2 m_pigeon = null;
+ private AHRS m_navX = null;
+ public boolean m_isGyroAPigeon; //true if pigeon, false if navX
+
+ private double m_lastPigeonAngle;
+ private double m_deltaPigeonAngle;
+
+ private double pitchZero = 0;
+ private double rollZero = 0;
+
+ /**
+ * Creates a Gyro based on a pigeon
+ * @param gyro the gyroscope to use for Gyro
+ */
+ public RobotGyro(WPI_Pigeon2 gyro) {
+ m_pigeon = gyro;
+ m_isGyroAPigeon = true;
+ }
+
+ /**
+ * Creates a Gyro based on a navX
+ * @param gyro the gyroscope to use for Gyro
+ */
+ public RobotGyro(AHRS gyro){
+ m_navX = gyro;
+ m_isGyroAPigeon = false;
+ }
+
+ /**
+ * Resets yaw, pitch, and roll.
+ */
+ public void resetZeroValues() {
+ if (!m_isGyroAPigeon) return;
+
+ pitchZero = m_pigeon.getPitch();
+ rollZero = m_pigeon.getRoll();
+ }
+
+ /**
+ * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
+ * that the getRate() method for a navX will likely be much more accurate than for a pigeon.
+ */
+ public void updatePigeonDeltas() {
+ double currentPigeonAngle = getAngle();
+ m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
+ m_lastPigeonAngle = currentPigeonAngle;
+ }
+
+ /**
+ *
NavX:
+ *
Calibrate the gyro by running for a number of samples and computing the center value. Then use
+ * the center value as the Accumulator center value for subsequent measurements. It's important to
+ * make sure that the robot is not moving while the centering calculations are in progress, this
+ * is typically done when the robot is first turned on while it's sitting at rest before the
+ * competition starts.
+ *
+ *
Pigeon:
+ *
Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
+ * into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
+ * has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
+ * make sure that the robot is not moving while the tempurature calculations are in progress, this
+ * is typically done when the robot is first turned on while it's sitting at rest before the
+ * competition starts.
+ */
+ @Override
+ public void calibrate() {
+ if (m_isGyroAPigeon) {
+ m_pigeon.calibrate();
+ } else {
+ m_navX.calibrate();
+ }
+ }
+
+ @Override
+ public void reset() {
+ resetZeroValues();
+
+ if (m_isGyroAPigeon) {
+ m_pigeon.setYaw(0);
+ } else {
+ m_navX.reset();
+ }
+ }
+
+ /**
+ * 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];
+ m_pigeon.getYawPitchRoll(ypr);
+
+ return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
+ }
+
+ @Override
+ public double getAngle() {
+ if (m_isGyroAPigeon) {
+ return getPigeonAngles()[0];
+ } else {
+ return m_navX.getAngle();
+ }
+ }
+
+ public double getYaw() {
+ return this.getAngle();
+ }
+
+ /**
+ * Gets an absolute heading of the robot
+ * @return heading from -180 to 180 degrees
+ */
+ public double getHeading() {
+ return getHeading(getAngle());
+ }
+
+ /**
+ * Gets an absolute heading of the robot
+ * @return heading from -180 to 180 degrees
+ */
+ public double getHeading(double angle) {
+ return Math.IEEEremainder(angle, 360);
+ }
+
+ /**
+ * Returns the current pitch value (in degrees, from -90 to 90)
+ * reported by the sensor. Pitch is a measure of rotation around
+ * the Y Axis.
+ * @return The current pitch value in degrees (-90 to 90).
+ */
+ public double getPitch() {
+ if (m_isGyroAPigeon) {
+ return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
+ } else {
+ return MathUtil.clamp(m_navX.getPitch(), -90, 90);
+ }
+ }
+
+ /**
+ * Returns the current roll value (in degrees, from -90 to 90)
+ * reported by the sensor. Roll is a measure of rotation around
+ * the X Axis.
+ * @return The current roll value in degrees (-90 to 90).
+ */
+ public double getRoll() {
+ if (m_isGyroAPigeon) {
+ return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
+ } else {
+ return MathUtil.clamp(m_navX.getRoll(), -90, 90);
+ }
+ }
+
+ @Override
+ public double getRate() {
+ if (m_isGyroAPigeon) {
+ return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
+ } else {
+ return m_navX.getRate();
+ }
+ }
+
+ public WPI_Pigeon2 getPigeon(){
+ return m_pigeon;
+ }
+
+ public AHRS getNavX(){
+ return m_navX;
+ }
+
+ @Override
+ public void close() throws Exception {
+
+ }
+}
diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java
index 694f850..c632bbb 100644
--- a/src/main/java/frc4388/utility/RobotTime.java
+++ b/src/main/java/frc4388/utility/RobotTime.java
@@ -1,79 +1,79 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.utility;
-
-/**
- *
Keeps track of Robot times like time passed, delta time, etc
- *
All times are in milliseconds
- */
-public class RobotTime {
- private long m_currTime = System.currentTimeMillis();
- public long m_deltaTime = 0;
-
- private long m_startRobotTime = m_currTime;
- public long m_robotTime = 0;
- public long m_lastRobotTime = 0;
-
- private long m_startMatchTime = 0;
- public long m_matchTime = 0;
- public long m_lastMatchTime = 0;
-
- public long m_frameNumber = 0;
-
- /**
- * Private constructor prevents other classes from instantiating
- */
- private RobotTime(){}
-
- private static RobotTime instance = null;
-
- /**
- * Gets the instance of Robot Time. If there is no instance running one will be created.
- * @return instance of Robot Time
- */
- public static RobotTime getInstance() {
- if (instance == null) {
- instance = new RobotTime();
- }
- return instance;
- }
-
- /**
- * Call this once per periodic loop.
- */
- public void updateTimes() {
- m_lastRobotTime = m_robotTime;
- m_lastMatchTime = m_matchTime;
-
- m_currTime = System.currentTimeMillis();
- m_robotTime = m_currTime - m_startRobotTime;
- m_deltaTime = m_robotTime - m_lastRobotTime;
- m_frameNumber++;
-
- if (m_startMatchTime != 0) {
- m_matchTime = m_currTime - m_startMatchTime;
- }
- }
-
- /**
- * Call this in both the auto and periodic inits
- */
- public void startMatchTime() {
- if (m_startMatchTime == 0) {
- m_startMatchTime = m_currTime;
- }
- }
-
- /**
- * Call this in disabled init
- */
- public void endMatchTime() {
- m_startMatchTime = 0;
- m_matchTime = 0;
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.utility;
+
+/**
+ *
Keeps track of Robot times like time passed, delta time, etc
+ *
All times are in milliseconds
+ */
+public class RobotTime {
+ private long m_currTime = System.currentTimeMillis();
+ public long m_deltaTime = 0;
+
+ private long m_startRobotTime = m_currTime;
+ public long m_robotTime = 0;
+ public long m_lastRobotTime = 0;
+
+ private long m_startMatchTime = 0;
+ public long m_matchTime = 0;
+ public long m_lastMatchTime = 0;
+
+ public long m_frameNumber = 0;
+
+ /**
+ * Private constructor prevents other classes from instantiating
+ */
+ private RobotTime(){}
+
+ private static RobotTime instance = null;
+
+ /**
+ * Gets the instance of Robot Time. If there is no instance running one will be created.
+ * @return instance of Robot Time
+ */
+ public static RobotTime getInstance() {
+ if (instance == null) {
+ instance = new RobotTime();
+ }
+ return instance;
+ }
+
+ /**
+ * Call this once per periodic loop.
+ */
+ public void updateTimes() {
+ m_lastRobotTime = m_robotTime;
+ m_lastMatchTime = m_matchTime;
+
+ m_currTime = System.currentTimeMillis();
+ m_robotTime = m_currTime - m_startRobotTime;
+ m_deltaTime = m_robotTime - m_lastRobotTime;
+ m_frameNumber++;
+
+ if (m_startMatchTime != 0) {
+ m_matchTime = m_currTime - m_startMatchTime;
+ }
+ }
+
+ /**
+ * Call this in both the auto and periodic inits
+ */
+ public void startMatchTime() {
+ if (m_startMatchTime == 0) {
+ m_startMatchTime = m_currTime;
+ }
+ }
+
+ /**
+ * Call this in disabled init
+ */
+ public void endMatchTime() {
+ m_startMatchTime = 0;
+ m_matchTime = 0;
+ }
+}
diff --git a/src/main/java/frc4388/utility/RobotUnits.java b/src/main/java/frc4388/utility/RobotUnits.java
index 9e07312..ab2eac5 100644
--- a/src/main/java/frc4388/utility/RobotUnits.java
+++ b/src/main/java/frc4388/utility/RobotUnits.java
@@ -1,27 +1,27 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.utility;
-
-/** Aarav's good units class (better than WPILib)
- * @author Aarav Shah */
-
-public class RobotUnits {
- // constants
-
- // 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;}
-
- // falcon conversions
- public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
-
- public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
-
- // distance conversions
- public static double metersToFeet(final double meters) {return meters * 3.28084;}
-
- public static double feetToMeters(final double feet) {return feet / 3.28084;}
-}
\ No newline at end of file
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility;
+
+/** Aarav's good units class (better than WPILib)
+ * @author Aarav Shah */
+
+public class RobotUnits {
+ // constants
+
+ // 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;}
+
+ // falcon conversions
+ public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
+
+ public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
+
+ // distance conversions
+ public static double metersToFeet(final double meters) {return meters * 3.28084;}
+
+ public static double feetToMeters(final double feet) {return feet / 3.28084;}
+}
diff --git a/src/main/java/frc4388/utility/UtilityStructs.java b/src/main/java/frc4388/utility/UtilityStructs.java
index e8b10cc..40d54c3 100644
--- a/src/main/java/frc4388/utility/UtilityStructs.java
+++ b/src/main/java/frc4388/utility/UtilityStructs.java
@@ -1,12 +1,17 @@
-package frc4388.utility;
-
-public class UtilityStructs {
- public static class TimedOutput {
- public double leftX = 0.0;
- public double leftY = 0.0;
- public double rightX = 0.0;
- public double rightY = 0.0;
-
- public long timedOffset = 0;
- }
-}
+package frc4388.utility;
+
+public class UtilityStructs {
+ public static class TimedOutput {
+ public double driverLeftX = 0.0;
+ public double driverLeftY = 0.0;
+ public double driverRightX = 0.0;
+ public double driverRightY = 0.0;
+
+ public double operatorLeftX = 0.0;
+ public double operatorLeftY = 0.0;
+ public double operatorRightX = 0.0;
+ public double operatorRightY = 0.0;
+
+ public long timedOffset = 0;
+ }
+}
diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
index 4577a2e..a0a5017 100644
--- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
@@ -1,27 +1,27 @@
-package frc4388.utility.controller;
-
-import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.XboxController;
-
-public class DeadbandedXboxController extends XboxController {
- 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(); }
-
- 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();
-
- if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
-
- return translation2d;
- }
-}
\ No newline at end of file
+package frc4388.utility.controller;
+
+import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class DeadbandedXboxController extends XboxController {
+ 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(); }
+
+ 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();
+
+ if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
+
+ return translation2d;
+ }
+}
diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java
index 13aa763..0a10c70 100644
--- a/src/main/java/frc4388/utility/controller/IHandController.java
+++ b/src/main/java/frc4388/utility/controller/IHandController.java
@@ -1,21 +1,21 @@
-package frc4388.utility.controller;
-
-/**
- * Add your docs here.
- */
-public interface IHandController {
-
- public double getLeftXAxis();
-
- public double getLeftYAxis();
-
- public double getRightXAxis();
-
- public double getRightYAxis();
-
- public double getLeftTriggerAxis();
-
- public double getRightTriggerAxis();
-
- public int getDpadAngle();
-}
+package frc4388.utility.controller;
+
+/**
+ * Add your docs here.
+ */
+public interface IHandController {
+
+ public double getLeftXAxis();
+
+ public double getLeftYAxis();
+
+ public double getRightXAxis();
+
+ public double getRightYAxis();
+
+ public double getLeftTriggerAxis();
+
+ public double getRightTriggerAxis();
+
+ public int getDpadAngle();
+}
diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java
index 8b8f0f8..5af6017 100644
--- a/src/main/java/frc4388/utility/controller/XboxController.java
+++ b/src/main/java/frc4388/utility/controller/XboxController.java
@@ -1,218 +1,218 @@
-package frc4388.utility.controller;
-
-import edu.wpi.first.wpilibj.Joystick;
-
-/**
- * This is a wrapper for the WPILib Joystick class that represents an XBox
- * controller.
- * @author frc1675
- */
-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 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;
-
- 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 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 DEADZONE = 0.1;
-
- private Joystick m_stick;
-
- /**
- * Add your docs here.
- */
- public XboxController(int portNumber){
- m_stick = new Joystick(portNumber);
- }
-
- /**
- * Add your docs here.
- */
- 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;
- }
- }
-
- public boolean getAButton(){
- return m_stick.getRawButton(A_BUTTON);
- }
-
- public boolean getXButton(){
- return m_stick.getRawButton(X_BUTTON);
- }
-
- public boolean getBButton(){
- return m_stick.getRawButton(B_BUTTON);
- }
-
- public boolean getYButton(){
- return m_stick.getRawButton(Y_BUTTON);
- }
-
- public boolean getBackButton(){
- return m_stick.getRawButton(BACK_BUTTON);
- }
-
- public boolean getStartButton(){
- return m_stick.getRawButton(START_BUTTON);
- }
-
- public boolean getLeftBumperButton(){
- return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
- }
-
- public boolean getRightBumperButton(){
- return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
- }
-
- public boolean getLeftJoystickButton(){
- return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
- }
-
- public boolean getRightJoystickButton(){
- return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
- }
-
- public double getLeftXAxis(){
- return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
- }
-
- public double getLeftYAxis(){
- return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
- }
-
- public double getRightXAxis(){
- return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
- }
-
- public double getRightYAxis(){
- return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
- }
-
- public double getLeftTriggerAxis(){
- return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
- }
-
- public double getRightTriggerAxis(){
- return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
- }
-
- /**
- * Get the angle input from the dpad.
- * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
- */
- public int getDpadAngle() {
- return m_stick.getPOV(0);
- }
-
- public boolean getDPadLeft(){
- return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
- }
-
- public boolean getDPadRight(){
- return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
- }
-
- public boolean getDPadTop(){
- return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
- }
-
- public boolean getDPadBottom(){
- return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
- }
-
- public boolean getLeftTrigger(){
- return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
- }
-
- public boolean getRightTrigger(){
- return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
- }
-
- public boolean getRightAxisUpTrigger(){
- return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
- }
-
- public boolean getRightAxisDownTrigger(){
- return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
- }
-
- public boolean getRightAxisLeftTrigger(){
- return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
- }
-
- public boolean getRightAxisRightTrigger(){
- return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
- }
-
- public boolean getLeftAxisUpTrigger(){
- return (getLeftYAxis() < LEFT_AXIS_UP_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);
- }
-}
\ No newline at end of file
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+/**
+ * This is a wrapper for the WPILib Joystick class that represents an XBox
+ * controller.
+ * @author frc1675
+ */
+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 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;
+
+ 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 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 DEADZONE = 0.1;
+
+ private Joystick m_stick;
+
+ /**
+ * Add your docs here.
+ */
+ public XboxController(int portNumber){
+ m_stick = new Joystick(portNumber);
+ }
+
+ /**
+ * Add your docs here.
+ */
+ 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;
+ }
+ }
+
+ public boolean getAButton(){
+ return m_stick.getRawButton(A_BUTTON);
+ }
+
+ public boolean getXButton(){
+ return m_stick.getRawButton(X_BUTTON);
+ }
+
+ public boolean getBButton(){
+ return m_stick.getRawButton(B_BUTTON);
+ }
+
+ public boolean getYButton(){
+ return m_stick.getRawButton(Y_BUTTON);
+ }
+
+ public boolean getBackButton(){
+ return m_stick.getRawButton(BACK_BUTTON);
+ }
+
+ public boolean getStartButton(){
+ return m_stick.getRawButton(START_BUTTON);
+ }
+
+ public boolean getLeftBumperButton(){
+ return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
+ }
+
+ public boolean getRightBumperButton(){
+ return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
+ }
+
+ public boolean getLeftJoystickButton(){
+ return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
+ }
+
+ public boolean getRightJoystickButton(){
+ return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
+ }
+
+ public double getLeftXAxis(){
+ return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
+ }
+
+ public double getLeftYAxis(){
+ return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
+ }
+
+ public double getRightXAxis(){
+ return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
+ }
+
+ public double getRightYAxis(){
+ return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
+ }
+
+ public double getLeftTriggerAxis(){
+ return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
+ }
+
+ public double getRightTriggerAxis(){
+ return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
+ }
+
+ /**
+ * Get the angle input from the dpad.
+ * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
+ */
+ public int getDpadAngle() {
+ return m_stick.getPOV(0);
+ }
+
+ public boolean getDPadLeft(){
+ return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
+ }
+
+ public boolean getDPadRight(){
+ return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
+ }
+
+ public boolean getDPadTop(){
+ return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
+ }
+
+ public boolean getDPadBottom(){
+ return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
+ }
+
+ public boolean getLeftTrigger(){
+ return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
+ }
+
+ public boolean getRightTrigger(){
+ return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
+ }
+
+ public boolean getRightAxisUpTrigger(){
+ return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
+ }
+
+ public boolean getRightAxisDownTrigger(){
+ return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
+ }
+
+ public boolean getRightAxisLeftTrigger(){
+ return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
+ }
+
+ public boolean getRightAxisRightTrigger(){
+ return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
+ }
+
+ public boolean getLeftAxisUpTrigger(){
+ return (getLeftYAxis() < LEFT_AXIS_UP_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);
+ }
+}
diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java
index 8c2fe88..e13408b 100644
--- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java
@@ -1,68 +1,68 @@
-package frc4388.utility.controller;
-
-import edu.wpi.first.wpilibj2.command.button.Button;
-
-/**
- * Mapping for the Xbox controller triggers to allow triggers to be defined as
- * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
- * exceeds a tolerance defined in {@link XboxController}.
- */
-public class XboxTriggerButton extends Button {
- 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;
-
- /**
- * 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();
- }
- 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;
- }
-}
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj2.command.button.Button;
+
+/**
+ * Mapping for the Xbox controller triggers to allow triggers to be defined as
+ * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
+ * exceeds a tolerance defined in {@link XboxController}.
+ */
+public class XboxTriggerButton extends Button {
+ 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;
+
+ /**
+ * 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();
+ }
+ 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;
+ }
+}
diff --git a/src/test/java/frc4388/mocks/MockPigeonIMU.java b/src/test/java/frc4388/mocks/MockPigeonIMU.java
index ecddde7..ac7583e 100644
--- a/src/test/java/frc4388/mocks/MockPigeonIMU.java
+++ b/src/test/java/frc4388/mocks/MockPigeonIMU.java
@@ -1,54 +1,54 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.mocks;
-
-import com.ctre.phoenix.ErrorCode;
-import com.ctre.phoenix.sensors.PigeonIMU;
-
-/**
- * Add your docs here.
- */
-public class MockPigeonIMU extends PigeonIMU {
- public int m_deviceNumber;
- public double currentYaw;
- public double currentPitch;
- public double currentRoll;
-
- public MockPigeonIMU(int deviceNumber) {
- super(deviceNumber);
- m_deviceNumber = deviceNumber;
- }
-
- @Override
- public ErrorCode setYaw(double angleDeg) {
- currentYaw = angleDeg;
- return ErrorCode.OK;
- }
-
- /**
- * @param currentPitch the Pitch to set
- */
- public void setCurrentPitch(double currentPitch) {
- this.currentPitch = currentPitch;
- }
-
- /**
- * @param currentRoll the Roll to set
- */
- public void setCurrentRoll(double currentRoll) {
- this.currentRoll = currentRoll;
- }
-
- @Override
- public ErrorCode getYawPitchRoll(double[] ypr_deg) {
- ypr_deg[0] = currentYaw;
- ypr_deg[1] = currentPitch;
- ypr_deg[2] = currentRoll;
- return ErrorCode.OK;
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.mocks;
+
+import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix.sensors.PigeonIMU;
+
+/**
+ * Add your docs here.
+ */
+public class MockPigeonIMU extends PigeonIMU {
+ public int m_deviceNumber;
+ public double currentYaw;
+ public double currentPitch;
+ public double currentRoll;
+
+ public MockPigeonIMU(int deviceNumber) {
+ super(deviceNumber);
+ m_deviceNumber = deviceNumber;
+ }
+
+ @Override
+ public ErrorCode setYaw(double angleDeg) {
+ currentYaw = angleDeg;
+ return ErrorCode.OK;
+ }
+
+ /**
+ * @param currentPitch the Pitch to set
+ */
+ public void setCurrentPitch(double currentPitch) {
+ this.currentPitch = currentPitch;
+ }
+
+ /**
+ * @param currentRoll the Roll to set
+ */
+ public void setCurrentRoll(double currentRoll) {
+ this.currentRoll = currentRoll;
+ }
+
+ @Override
+ public ErrorCode getYawPitchRoll(double[] ypr_deg) {
+ ypr_deg[0] = currentYaw;
+ ypr_deg[1] = currentPitch;
+ ypr_deg[2] = currentRoll;
+ return ErrorCode.OK;
+ }
+}
diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old
index 9b2dc8e..4d97824 100644
--- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old
+++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.old
@@ -1,61 +1,61 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.subsystems;
-
-import org.junit.jupiter.api.Test;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-// import static org.junit.Assert.assertEquals;
-// import static org.mockito.Mockito.mock;
-// import org.junit.Test;
-
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import frc4388.robot.Constants.LEDConstants;
-import frc4388.utility.LEDPatterns;
-
-/**
- * Add your docs here.
- */
-public class LEDSubsystemTest {
- @Test
- public void testConstructor() {
- // Arrange
- Spark ledController = new Spark(0);
-
- // Act
- LED led = new LED(ledController);
-
- // Assert
- assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
- }
-
- @Test
- public void testPatterns() {
- // Arrange
- Spark ledController = new Spark(0);
- LED led = new LED(ledController);
-
- // Act
- led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
-
- // Assert
- assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
-
- // Act
- led.setPattern(LEDPatterns.BLUE_BREATH);
-
- // Assert
- assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
-
- // Act
- led.setPattern(LEDPatterns.SOLID_BLACK);
-
- // Assert
- assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.subsystems;
+
+import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+// import static org.junit.Assert.assertEquals;
+// import static org.mockito.Mockito.mock;
+// import org.junit.Test;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.LEDPatterns;
+
+/**
+ * Add your docs here.
+ */
+public class LEDSubsystemTest {
+ @Test
+ public void testConstructor() {
+ // Arrange
+ Spark ledController = new Spark(0);
+
+ // Act
+ LED led = new LED(ledController);
+
+ // Assert
+ assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
+ }
+
+ @Test
+ public void testPatterns() {
+ // Arrange
+ Spark ledController = new Spark(0);
+ LED led = new LED(ledController);
+
+ // Act
+ led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
+
+ // Assert
+ assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
+
+ // Act
+ led.setPattern(LEDPatterns.BLUE_BREATH);
+
+ // Assert
+ assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
+
+ // Act
+ led.setPattern(LEDPatterns.SOLID_BLACK);
+
+ // Assert
+ assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
+ }
+}
diff --git a/src/test/java/frc4388/utility/Delay.java b/src/test/java/frc4388/utility/Delay.java
new file mode 100644
index 0000000..492ab06
--- /dev/null
+++ b/src/test/java/frc4388/utility/Delay.java
@@ -0,0 +1,8 @@
+// package frc4388.utility;
+
+// public class Delay {
+// void delaymillis(long millis) {
+// delay(millis)
+
+// }
+// }
diff --git a/src/test/java/frc4388/utility/RobotGyroUtilityTest.old b/src/test/java/frc4388/utility/RobotGyroUtilityTest.old
index 1dca123..80978b2 100644
--- a/src/test/java/frc4388/utility/RobotGyroUtilityTest.old
+++ b/src/test/java/frc4388/utility/RobotGyroUtilityTest.old
@@ -1,185 +1,185 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.utility;
-
-import org.junit.jupiter.api.Test;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-// import static org.junit.Assert.*;
-// import static org.mockito.Mockito.*;
-
-import com.kauailabs.navx.frc.AHRS;
-
-import frc4388.mocks.MockPigeonIMU;
-import frc4388.robot.Constants.DriveConstants;
-
-/**
- * Add your docs here.
- */
-public class RobotGyroUtilityTest {
- // TODO UNTESTED: most functions for NavX
-
- private RobotGyro gyroPigeon;
- private RobotGyro gyroNavX;
-
- @Test
- public void testConstructor() {
- // Arrange
- MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
- AHRS navX = new AHRS();
- gyroPigeon = new RobotGyro(pigeon);
- gyroNavX = new RobotGyro(navX);
-
- // Assert
- assertEquals(true, gyroPigeon.m_isGyroAPigeon);
- assertEquals(pigeon, gyroPigeon.getPigeon());
- assertEquals(null, gyroPigeon.getNavX());
- assertEquals(false, gyroNavX.m_isGyroAPigeon);
- assertEquals(navX, gyroNavX.getNavX());
- assertEquals(null, gyroNavX.getPigeon());
- }
-
- @Test
- public void testHeadingPigeon() {
- // Arrange
- MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
- gyroPigeon = new RobotGyro(pigeon);
-
- // Act & Assert
- assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
- assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
- assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
- assertEquals(30, gyroPigeon.getHeading(30), 0.0001);
- assertEquals(0, gyroPigeon.getHeading(0), 0.0001);
- assertEquals(180, gyroPigeon.getHeading(180), 0.0001);
- assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001);
- assertEquals(97, gyroPigeon.getHeading(1537), 0.0001);
- assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001);
- }
-
- @Test
- public void testYawPitchRollPigeon() {
- // Arrange
- MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
- gyroPigeon = new RobotGyro(pigeon);
-
- // Assert
- assertEquals(0, gyroPigeon.getAngle(), 0.0001);
-
- // Act
- pigeon.setYaw(40);
-
- // Assert
- assertEquals(40, gyroPigeon.getAngle(), 0.0001);
-
- // Act
- gyroPigeon.reset();
-
- // Assert
- assertEquals(0, gyroPigeon.getAngle(), 0.0001);
-
- // Act
- pigeon.setYaw(-1457);
- pigeon.setCurrentPitch(100);
- pigeon.setCurrentRoll(100);
-
- // Assert
- assertEquals(-1457, gyroPigeon.getAngle(), 0.0001);
- assertEquals(90, gyroPigeon.getPitch(), 0.0001);
- assertEquals(90, gyroPigeon.getRoll(), 0.0001);
-
- // Act
- pigeon.setCurrentPitch(45);
- pigeon.setCurrentRoll(45);
-
- // Assert
- assertEquals(45, gyroPigeon.getPitch(), 0.0001);
- assertEquals(45, gyroPigeon.getRoll(), 0.0001);
-
- // Act
- pigeon.setCurrentPitch(0);
- pigeon.setCurrentRoll(0);
-
- // Assert
- assertEquals(0, gyroPigeon.getPitch(), 0.0001);
- assertEquals(0, gyroPigeon.getRoll(), 0.0001);
-
- // Act
- pigeon.setCurrentPitch(-60);
- pigeon.setCurrentRoll(-60);
-
- // Assert
- assertEquals(-60, gyroPigeon.getPitch(), 0.0001);
- assertEquals(-60, gyroPigeon.getRoll(), 0.0001);
-
- // Act
- pigeon.setCurrentPitch(-90);
- pigeon.setCurrentRoll(-90);
-
- // Assert
- assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
- assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
-
- // Act
- pigeon.setCurrentPitch(-100);
- pigeon.setCurrentRoll(-100);
-
- // Assert
- assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
- assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
- }
-
- @Test
- public void testRatesPigeon() {
- // Arrange
- MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
- gyroPigeon = new RobotGyro(pigeon);
- RobotTime robotTime = RobotTime.getInstance();
- gyroPigeon.updatePigeonDeltas();
-
- // Act
- robotTime.m_deltaTime = 5;
- pigeon.setYaw(0);
- gyroPigeon.updatePigeonDeltas();
-
- // Assert
- assertEquals(0, gyroPigeon.getRate(), 1);
-
- // Act
- robotTime.m_deltaTime = 5;
- pigeon.setYaw(90);
- gyroPigeon.updatePigeonDeltas();
-
- // Assert
- assertEquals(18000, gyroPigeon.getRate(), 0.001);
-
- // Act
- robotTime.m_deltaTime = 5;
- pigeon.setYaw(90);
- gyroPigeon.updatePigeonDeltas();
-
- // Assert
- assertEquals(0, gyroPigeon.getRate(), 0.001);
-
- // Act
- robotTime.m_deltaTime = 3;
- pigeon.setYaw(-30);
- gyroPigeon.updatePigeonDeltas();
-
- // Assert
- assertEquals(-40000, gyroPigeon.getRate(), 0.001);
-
- // Act
- robotTime.m_deltaTime = 6;
- pigeon.setYaw(690);
- gyroPigeon.updatePigeonDeltas();
-
- // Assert
- assertEquals(120000, gyroPigeon.getRate(), 0.001);
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.utility;
+
+import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+// import static org.junit.Assert.*;
+// import static org.mockito.Mockito.*;
+
+import com.kauailabs.navx.frc.AHRS;
+
+import frc4388.mocks.MockPigeonIMU;
+import frc4388.robot.Constants.DriveConstants;
+
+/**
+ * Add your docs here.
+ */
+public class RobotGyroUtilityTest {
+ // TODO UNTESTED: most functions for NavX
+
+ private RobotGyro gyroPigeon;
+ private RobotGyro gyroNavX;
+
+ @Test
+ public void testConstructor() {
+ // Arrange
+ MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+ AHRS navX = new AHRS();
+ gyroPigeon = new RobotGyro(pigeon);
+ gyroNavX = new RobotGyro(navX);
+
+ // Assert
+ assertEquals(true, gyroPigeon.m_isGyroAPigeon);
+ assertEquals(pigeon, gyroPigeon.getPigeon());
+ assertEquals(null, gyroPigeon.getNavX());
+ assertEquals(false, gyroNavX.m_isGyroAPigeon);
+ assertEquals(navX, gyroNavX.getNavX());
+ assertEquals(null, gyroNavX.getPigeon());
+ }
+
+ @Test
+ public void testHeadingPigeon() {
+ // Arrange
+ MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+ gyroPigeon = new RobotGyro(pigeon);
+
+ // Act & Assert
+ assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
+ assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
+ assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
+ assertEquals(30, gyroPigeon.getHeading(30), 0.0001);
+ assertEquals(0, gyroPigeon.getHeading(0), 0.0001);
+ assertEquals(180, gyroPigeon.getHeading(180), 0.0001);
+ assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001);
+ assertEquals(97, gyroPigeon.getHeading(1537), 0.0001);
+ assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001);
+ }
+
+ @Test
+ public void testYawPitchRollPigeon() {
+ // Arrange
+ MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+ gyroPigeon = new RobotGyro(pigeon);
+
+ // Assert
+ assertEquals(0, gyroPigeon.getAngle(), 0.0001);
+
+ // Act
+ pigeon.setYaw(40);
+
+ // Assert
+ assertEquals(40, gyroPigeon.getAngle(), 0.0001);
+
+ // Act
+ gyroPigeon.reset();
+
+ // Assert
+ assertEquals(0, gyroPigeon.getAngle(), 0.0001);
+
+ // Act
+ pigeon.setYaw(-1457);
+ pigeon.setCurrentPitch(100);
+ pigeon.setCurrentRoll(100);
+
+ // Assert
+ assertEquals(-1457, gyroPigeon.getAngle(), 0.0001);
+ assertEquals(90, gyroPigeon.getPitch(), 0.0001);
+ assertEquals(90, gyroPigeon.getRoll(), 0.0001);
+
+ // Act
+ pigeon.setCurrentPitch(45);
+ pigeon.setCurrentRoll(45);
+
+ // Assert
+ assertEquals(45, gyroPigeon.getPitch(), 0.0001);
+ assertEquals(45, gyroPigeon.getRoll(), 0.0001);
+
+ // Act
+ pigeon.setCurrentPitch(0);
+ pigeon.setCurrentRoll(0);
+
+ // Assert
+ assertEquals(0, gyroPigeon.getPitch(), 0.0001);
+ assertEquals(0, gyroPigeon.getRoll(), 0.0001);
+
+ // Act
+ pigeon.setCurrentPitch(-60);
+ pigeon.setCurrentRoll(-60);
+
+ // Assert
+ assertEquals(-60, gyroPigeon.getPitch(), 0.0001);
+ assertEquals(-60, gyroPigeon.getRoll(), 0.0001);
+
+ // Act
+ pigeon.setCurrentPitch(-90);
+ pigeon.setCurrentRoll(-90);
+
+ // Assert
+ assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
+ assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
+
+ // Act
+ pigeon.setCurrentPitch(-100);
+ pigeon.setCurrentRoll(-100);
+
+ // Assert
+ assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
+ assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
+ }
+
+ @Test
+ public void testRatesPigeon() {
+ // Arrange
+ MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+ gyroPigeon = new RobotGyro(pigeon);
+ RobotTime robotTime = RobotTime.getInstance();
+ gyroPigeon.updatePigeonDeltas();
+
+ // Act
+ robotTime.m_deltaTime = 5;
+ pigeon.setYaw(0);
+ gyroPigeon.updatePigeonDeltas();
+
+ // Assert
+ assertEquals(0, gyroPigeon.getRate(), 1);
+
+ // Act
+ robotTime.m_deltaTime = 5;
+ pigeon.setYaw(90);
+ gyroPigeon.updatePigeonDeltas();
+
+ // Assert
+ assertEquals(18000, gyroPigeon.getRate(), 0.001);
+
+ // Act
+ robotTime.m_deltaTime = 5;
+ pigeon.setYaw(90);
+ gyroPigeon.updatePigeonDeltas();
+
+ // Assert
+ assertEquals(0, gyroPigeon.getRate(), 0.001);
+
+ // Act
+ robotTime.m_deltaTime = 3;
+ pigeon.setYaw(-30);
+ gyroPigeon.updatePigeonDeltas();
+
+ // Assert
+ assertEquals(-40000, gyroPigeon.getRate(), 0.001);
+
+ // Act
+ robotTime.m_deltaTime = 6;
+ pigeon.setYaw(690);
+ gyroPigeon.updatePigeonDeltas();
+
+ // Assert
+ assertEquals(120000, gyroPigeon.getRate(), 0.001);
+ }
+}
diff --git a/src/test/java/frc4388/utility/RobotTimeUtilityTest.old b/src/test/java/frc4388/utility/RobotTimeUtilityTest.old
index c0d4551..ba5fb7a 100644
--- a/src/test/java/frc4388/utility/RobotTimeUtilityTest.old
+++ b/src/test/java/frc4388/utility/RobotTimeUtilityTest.old
@@ -1,105 +1,105 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
-/* Open Source Software - may be modified and shared by FRC teams. The code */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project. */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.utility;
-
-import org.junit.jupiter.api.Test;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-// import static org.junit.Assert.*;
-
-/**
- * Add your docs here.
- */
-public class RobotTimeUtilityTest {
- RobotTime robotTime = RobotTime.getInstance();
-
- @Test
- public void testUpdateTimes() {
- // Arrange
- long lastTime;
- robotTime.m_deltaTime = 0;
- robotTime.m_robotTime = 0;
- robotTime.m_lastRobotTime = 0;
- robotTime.m_frameNumber = 0;
- robotTime.endMatchTime();
- robotTime.m_lastMatchTime = 0;
-
- // Assert
- assertEquals(0, robotTime.m_deltaTime);
- assertEquals(0, robotTime.m_robotTime);
- assertEquals(0, robotTime.m_lastRobotTime);
- assertEquals(0, robotTime.m_frameNumber);
- lastTime = robotTime.m_robotTime;
-
- // Act
- wait(1);
- robotTime.updateTimes();
-
- // Assert
- assertEquals(true, robotTime.m_deltaTime > 0);
- assertEquals(true, robotTime.m_robotTime > 0);
- assertEquals(lastTime, robotTime.m_lastRobotTime);
- assertEquals(1, robotTime.m_frameNumber);
- lastTime = robotTime.m_robotTime;
-
- // Act
- wait(1);
- robotTime.updateTimes();
-
- // Assert
- assertEquals(true, robotTime.m_deltaTime > 0);
- assertEquals(true, robotTime.m_robotTime > 0);
- assertEquals(lastTime, robotTime.m_lastRobotTime);
- assertEquals(2, robotTime.m_frameNumber);
- }
-
- @Test
- public void testMatchTime() {
- // Arrange
- long lastTime;
-
- // Assert
- assertEquals(0, robotTime.m_matchTime);
- assertEquals(0, robotTime.m_lastMatchTime);
- lastTime = robotTime.m_matchTime;
-
- // Act
- robotTime.startMatchTime();
- wait(1);
- robotTime.updateTimes();
-
- // Assert
- assertEquals(true, robotTime.m_matchTime > 0);
- assertEquals(lastTime, robotTime.m_lastMatchTime);
- lastTime = robotTime.m_matchTime;
-
- // Act
- wait(1);
- robotTime.updateTimes();
- robotTime.endMatchTime();
-
- // Assert
- assertEquals(0, robotTime.m_matchTime);
- assertEquals(lastTime, robotTime.m_lastMatchTime);
- lastTime = robotTime.m_matchTime;
-
- // Act
- wait(1);
- robotTime.updateTimes();
-
- // Assert
- assertEquals(0, robotTime.m_matchTime);
- assertEquals(lastTime, robotTime.m_lastMatchTime);
- }
-
- private void wait(int millis) {
- try {
- Thread.sleep(millis);
- } catch (Exception e) {}
- }
-}
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.utility;
+
+import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+// import static org.junit.Assert.*;
+
+/**
+ * Add your docs here.
+ */
+public class RobotTimeUtilityTest {
+ RobotTime robotTime = RobotTime.getInstance();
+
+ @Test
+ public void testUpdateTimes() {
+ // Arrange
+ long lastTime;
+ robotTime.m_deltaTime = 0;
+ robotTime.m_robotTime = 0;
+ robotTime.m_lastRobotTime = 0;
+ robotTime.m_frameNumber = 0;
+ robotTime.endMatchTime();
+ robotTime.m_lastMatchTime = 0;
+
+ // Assert
+ assertEquals(0, robotTime.m_deltaTime);
+ assertEquals(0, robotTime.m_robotTime);
+ assertEquals(0, robotTime.m_lastRobotTime);
+ assertEquals(0, robotTime.m_frameNumber);
+ lastTime = robotTime.m_robotTime;
+
+ // Act
+ wait(1);
+ robotTime.updateTimes();
+
+ // Assert
+ assertEquals(true, robotTime.m_deltaTime > 0);
+ assertEquals(true, robotTime.m_robotTime > 0);
+ assertEquals(lastTime, robotTime.m_lastRobotTime);
+ assertEquals(1, robotTime.m_frameNumber);
+ lastTime = robotTime.m_robotTime;
+
+ // Act
+ wait(1);
+ robotTime.updateTimes();
+
+ // Assert
+ assertEquals(true, robotTime.m_deltaTime > 0);
+ assertEquals(true, robotTime.m_robotTime > 0);
+ assertEquals(lastTime, robotTime.m_lastRobotTime);
+ assertEquals(2, robotTime.m_frameNumber);
+ }
+
+ @Test
+ public void testMatchTime() {
+ // Arrange
+ long lastTime;
+
+ // Assert
+ assertEquals(0, robotTime.m_matchTime);
+ assertEquals(0, robotTime.m_lastMatchTime);
+ lastTime = robotTime.m_matchTime;
+
+ // Act
+ robotTime.startMatchTime();
+ wait(1);
+ robotTime.updateTimes();
+
+ // Assert
+ assertEquals(true, robotTime.m_matchTime > 0);
+ assertEquals(lastTime, robotTime.m_lastMatchTime);
+ lastTime = robotTime.m_matchTime;
+
+ // Act
+ wait(1);
+ robotTime.updateTimes();
+ robotTime.endMatchTime();
+
+ // Assert
+ assertEquals(0, robotTime.m_matchTime);
+ assertEquals(lastTime, robotTime.m_lastMatchTime);
+ lastTime = robotTime.m_matchTime;
+
+ // Act
+ wait(1);
+ robotTime.updateTimes();
+
+ // Assert
+ assertEquals(0, robotTime.m_matchTime);
+ assertEquals(lastTime, robotTime.m_lastMatchTime);
+ }
+
+ private void wait(int millis) {
+ try {
+ Thread.sleep(millis);
+ } catch (Exception e) {}
+ }
+}
diff --git a/todo.txt b/todo.txt
index 2224b57..c645264 100644
--- a/todo.txt
+++ b/todo.txt
@@ -1,8 +1,8 @@
-Revised Autosystem:
- - Autoplayback
- - Fix Arm Respool
- - Via Commands
- - Arm
- - Sequential Command System
- - Manual Auto Creation
- - Shuffleboard IO
\ No newline at end of file
+Revised Autosystem:
+ - Autoplayback
+ \ Fix Arm Respool
+ x Via Commands
+ - Arm
+ - Sequential Command System
+ - Manual Auto Creation
+ - Shuffleboard IO
diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json
index 4bd4424..76b56f5 100644
--- a/vendordeps/Phoenix.json
+++ b/vendordeps/Phoenix.json
@@ -1,423 +1,423 @@
-{
- "fileName": "Phoenix.json",
- "name": "CTRE-Phoenix (v5)",
- "version": "5.30.2",
- "frcYear": 2023,
- "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
- "mavenUrls": [
- "https://maven.ctr-electronics.com/release/"
- ],
- "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json",
- "javaDependencies": [
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "api-java",
- "version": "5.30.2"
- },
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "wpiapi-java",
- "version": "5.30.2"
- }
- ],
- "jniDependencies": [
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "cci",
- "version": "5.30.2",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenix.sim",
- "artifactId": "cci-sim",
- "version": "5.30.2",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro",
- "artifactId": "tools",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "tools-sim",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simTalonSRX",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simTalonFX",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simVictorSPX",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simPigeonIMU",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simCANCoder",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simProTalonFX",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simProCANcoder",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simProPigeon2",
- "version": "23.0.1",
- "isJar": false,
- "skipInvalidPlatforms": true,
- "validPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- }
- ],
- "cppDependencies": [
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "wpiapi-cpp",
- "version": "5.30.2",
- "libName": "CTRE_Phoenix_WPI",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "api-cpp",
- "version": "5.30.2",
- "libName": "CTRE_Phoenix",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenix",
- "artifactId": "cci",
- "version": "5.30.2",
- "libName": "CTRE_PhoenixCCI",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenixpro",
- "artifactId": "tools",
- "version": "23.0.1",
- "libName": "CTRE_PhoenixTools",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "linuxathena"
- ],
- "simMode": "hwsim"
- },
- {
- "groupId": "com.ctre.phoenix.sim",
- "artifactId": "wpiapi-cpp-sim",
- "version": "5.30.2",
- "libName": "CTRE_Phoenix_WPISim",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenix.sim",
- "artifactId": "api-cpp-sim",
- "version": "5.30.2",
- "libName": "CTRE_PhoenixSim",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenix.sim",
- "artifactId": "cci-sim",
- "version": "5.30.2",
- "libName": "CTRE_PhoenixCCISim",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "tools-sim",
- "version": "23.0.1",
- "libName": "CTRE_PhoenixTools_Sim",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simTalonSRX",
- "version": "23.0.1",
- "libName": "CTRE_SimTalonSRX",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simTalonFX",
- "version": "23.0.1",
- "libName": "CTRE_SimTalonFX",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simVictorSPX",
- "version": "23.0.1",
- "libName": "CTRE_SimVictorSPX",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simPigeonIMU",
- "version": "23.0.1",
- "libName": "CTRE_SimPigeonIMU",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simCANCoder",
- "version": "23.0.1",
- "libName": "CTRE_SimCANCoder",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simProTalonFX",
- "version": "23.0.1",
- "libName": "CTRE_SimProTalonFX",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simProCANcoder",
- "version": "23.0.1",
- "libName": "CTRE_SimProCANcoder",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- },
- {
- "groupId": "com.ctre.phoenixpro.sim",
- "artifactId": "simProPigeon2",
- "version": "23.0.1",
- "libName": "CTRE_SimProPigeon2",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxx86-64",
- "osxuniversal"
- ],
- "simMode": "swsim"
- }
- ]
+{
+ "fileName": "Phoenix.json",
+ "name": "CTRE-Phoenix (v5)",
+ "version": "5.30.2",
+ "frcYear": 2023,
+ "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+ "mavenUrls": [
+ "https://maven.ctr-electronics.com/release/"
+ ],
+ "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "api-java",
+ "version": "5.30.2"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "wpiapi-java",
+ "version": "5.30.2"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "cci",
+ "version": "5.30.2",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "cci-sim",
+ "version": "5.30.2",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro",
+ "artifactId": "tools",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "tools-sim",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simTalonSRX",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simTalonFX",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simVictorSPX",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simCANCoder",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simProTalonFX",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simProCANcoder",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simProPigeon2",
+ "version": "23.0.1",
+ "isJar": false,
+ "skipInvalidPlatforms": true,
+ "validPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "wpiapi-cpp",
+ "version": "5.30.2",
+ "libName": "CTRE_Phoenix_WPI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "api-cpp",
+ "version": "5.30.2",
+ "libName": "CTRE_Phoenix",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix",
+ "artifactId": "cci",
+ "version": "5.30.2",
+ "libName": "CTRE_PhoenixCCI",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro",
+ "artifactId": "tools",
+ "version": "23.0.1",
+ "libName": "CTRE_PhoenixTools",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "linuxathena"
+ ],
+ "simMode": "hwsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "wpiapi-cpp-sim",
+ "version": "5.30.2",
+ "libName": "CTRE_Phoenix_WPISim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "api-cpp-sim",
+ "version": "5.30.2",
+ "libName": "CTRE_PhoenixSim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenix.sim",
+ "artifactId": "cci-sim",
+ "version": "5.30.2",
+ "libName": "CTRE_PhoenixCCISim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "tools-sim",
+ "version": "23.0.1",
+ "libName": "CTRE_PhoenixTools_Sim",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simTalonSRX",
+ "version": "23.0.1",
+ "libName": "CTRE_SimTalonSRX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simTalonFX",
+ "version": "23.0.1",
+ "libName": "CTRE_SimTalonFX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simVictorSPX",
+ "version": "23.0.1",
+ "libName": "CTRE_SimVictorSPX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simPigeonIMU",
+ "version": "23.0.1",
+ "libName": "CTRE_SimPigeonIMU",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simCANCoder",
+ "version": "23.0.1",
+ "libName": "CTRE_SimCANCoder",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simProTalonFX",
+ "version": "23.0.1",
+ "libName": "CTRE_SimProTalonFX",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simProCANcoder",
+ "version": "23.0.1",
+ "libName": "CTRE_SimProCANcoder",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ },
+ {
+ "groupId": "com.ctre.phoenixpro.sim",
+ "artifactId": "simProPigeon2",
+ "version": "23.0.1",
+ "libName": "CTRE_SimProPigeon2",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal"
+ ],
+ "simMode": "swsim"
+ }
+ ]
}
\ No newline at end of file
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
index f2d0b7d..41d55ec 100644
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -1,73 +1,73 @@
-{
- "fileName": "REVLib.json",
- "name": "REVLib",
- "version": "2023.1.3",
- "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
- "mavenUrls": [
- "https://maven.revrobotics.com/"
- ],
- "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json",
- "javaDependencies": [
- {
- "groupId": "com.revrobotics.frc",
- "artifactId": "REVLib-java",
- "version": "2023.1.3"
- }
- ],
- "jniDependencies": [
- {
- "groupId": "com.revrobotics.frc",
- "artifactId": "REVLib-driver",
- "version": "2023.1.3",
- "skipInvalidPlatforms": true,
- "isJar": false,
- "validPlatforms": [
- "windowsx86-64",
- "windowsx86",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- }
- ],
- "cppDependencies": [
- {
- "groupId": "com.revrobotics.frc",
- "artifactId": "REVLib-cpp",
- "version": "2023.1.3",
- "libName": "REVLib",
- "headerClassifier": "headers",
- "sharedLibrary": false,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "windowsx86",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- },
- {
- "groupId": "com.revrobotics.frc",
- "artifactId": "REVLib-driver",
- "version": "2023.1.3",
- "libName": "REVLibDriver",
- "headerClassifier": "headers",
- "sharedLibrary": false,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "windowsx86",
- "linuxarm64",
- "linuxx86-64",
- "linuxathena",
- "linuxarm32",
- "osxuniversal"
- ]
- }
- ]
+{
+ "fileName": "REVLib.json",
+ "name": "REVLib",
+ "version": "2023.1.3",
+ "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+ "mavenUrls": [
+ "https://maven.revrobotics.com/"
+ ],
+ "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-java",
+ "version": "2023.1.3"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2023.1.3",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-cpp",
+ "version": "2023.1.3",
+ "libName": "REVLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2023.1.3",
+ "libName": "REVLibDriver",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ]
}
\ No newline at end of file
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
index 65dcc03..da4bc52 100644
--- a/vendordeps/WPILibNewCommands.json
+++ b/vendordeps/WPILibNewCommands.json
@@ -1,37 +1,37 @@
-{
- "fileName": "WPILibNewCommands.json",
- "name": "WPILib-New-Commands",
- "version": "1.0.0",
- "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
- "mavenUrls": [],
- "jsonUrl": "",
- "javaDependencies": [
- {
- "groupId": "edu.wpi.first.wpilibNewCommands",
- "artifactId": "wpilibNewCommands-java",
- "version": "wpilib"
- }
- ],
- "jniDependencies": [],
- "cppDependencies": [
- {
- "groupId": "edu.wpi.first.wpilibNewCommands",
- "artifactId": "wpilibNewCommands-cpp",
- "version": "wpilib",
- "libName": "wpilibNewCommands",
- "headerClassifier": "headers",
- "sourcesClassifier": "sources",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "linuxathena",
- "linuxarm32",
- "linuxarm64",
- "windowsx86-64",
- "windowsx86",
- "linuxx86-64",
- "osxuniversal"
- ]
- }
- ]
-}
+{
+ "fileName": "WPILibNewCommands.json",
+ "name": "WPILib-New-Commands",
+ "version": "1.0.0",
+ "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "javaDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-cpp",
+ "version": "wpilib",
+ "libName": "wpilibNewCommands",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json
index 1718692..d628017 100644
--- a/vendordeps/navx_frc.json
+++ b/vendordeps/navx_frc.json
@@ -1,35 +1,35 @@
-{
- "fileName": "navx_frc.json",
- "name": "KauaiLabs_navX_FRC",
- "version": "4.0.447",
- "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
- "mavenUrls": [
- "https://repo1.maven.org/maven2/"
- ],
- "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json",
- "javaDependencies": [
- {
- "groupId": "com.kauailabs.navx.frc",
- "artifactId": "navx-java",
- "version": "4.0.447"
- }
- ],
- "jniDependencies": [],
- "cppDependencies": [
- {
- "groupId": "com.kauailabs.navx.frc",
- "artifactId": "navx-cpp",
- "version": "4.0.447",
- "headerClassifier": "headers",
- "sourcesClassifier": "sources",
- "sharedLibrary": false,
- "libName": "navx_frc",
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "linuxathena",
- "linuxraspbian",
- "windowsx86-64"
- ]
- }
- ]
+{
+ "fileName": "navx_frc.json",
+ "name": "KauaiLabs_navX_FRC",
+ "version": "4.0.447",
+ "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+ "mavenUrls": [
+ "https://repo1.maven.org/maven2/"
+ ],
+ "jsonUrl": "https://www.kauailabs.com/dist/frc/2022/navx_frc.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.kauailabs.navx.frc",
+ "artifactId": "navx-java",
+ "version": "4.0.447"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.kauailabs.navx.frc",
+ "artifactId": "navx-cpp",
+ "version": "4.0.447",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": false,
+ "libName": "navx_frc",
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxraspbian",
+ "windowsx86-64"
+ ]
+ }
+ ]
}
\ No newline at end of file
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index dad3105..f4d4ae6 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,41 +1,41 @@
-{
- "fileName": "photonlib.json",
- "name": "photonlib",
- "version": "v2023.4.2",
- "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
- "mavenUrls": [
- "https://maven.photonvision.org/repository/internal",
- "https://maven.photonvision.org/repository/snapshots"
- ],
- "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
- "jniDependencies": [],
- "cppDependencies": [
- {
- "groupId": "org.photonvision",
- "artifactId": "PhotonLib-cpp",
- "version": "v2023.4.2",
- "libName": "Photon",
- "headerClassifier": "headers",
- "sharedLibrary": true,
- "skipInvalidPlatforms": true,
- "binaryPlatforms": [
- "windowsx86-64",
- "linuxathena",
- "linuxx86-64",
- "osxuniversal"
- ]
- }
- ],
- "javaDependencies": [
- {
- "groupId": "org.photonvision",
- "artifactId": "PhotonLib-java",
- "version": "v2023.4.2"
- },
- {
- "groupId": "org.photonvision",
- "artifactId": "PhotonTargeting-java",
- "version": "v2023.4.2"
- }
- ]
+{
+ "fileName": "photonlib.json",
+ "name": "photonlib",
+ "version": "v2023.4.2",
+ "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
+ "mavenUrls": [
+ "https://maven.photonvision.org/repository/internal",
+ "https://maven.photonvision.org/repository/snapshots"
+ ],
+ "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonLib-cpp",
+ "version": "v2023.4.2",
+ "libName": "Photon",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonLib-java",
+ "version": "v2023.4.2"
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonTargeting-java",
+ "version": "v2023.4.2"
+ }
+ ]
}
\ No newline at end of file