diff --git a/build.gradle b/build.gradle
index 55600bc..25822fe 100644
--- a/build.gradle
+++ b/build.gradle
@@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
plugins {
id "java"
- id "edu.wpi.first.GradleRIO" version "2022.2.1"
+ id "edu.wpi.first.GradleRIO" version "2022.4.1"
}
sourceCompatibility = JavaVersion.VERSION_11
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 5ae5339..7475b4f 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -39,8 +39,8 @@ public final class Constants {
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // redundant constant?
-
- //IDs
+
+ // IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
@@ -54,24 +54,34 @@ public final class Constants {
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
public static final int GYRO_ID = 14;
-
+
// offsets are in degrees
// NATHAN if you truncate or round or simplify these i will cry
- public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0;
- public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0;
- public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0;
- public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45;
+ // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45 - 3.30;//
+ // 181.7578125;//180.0;//315.0 +45;//180.0;
+ // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.0625 +
+ // 0.18;// 360.-59.0625;//315.0;//224.296875 +
+ // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.222;//
+ // 308.408203125;//225.0;//45.87890625;//360.0
+ // public static final double RIGHT_BACK_ENCODER_OFFSET = 360. + 2.15 - 3.637;//
+ // 180-2.021484375;//0.0;//134.384765625
+
+ public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 - 90) % 360.;
+ public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 - 90) % 360.;
+ public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50 - 90) % 360.;
+ public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180 - 90) % 360.;
// swerve PID constants
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final int SWERVE_TIMEOUT_MS = 30;
- public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 1.0, 0.0, 0, 1.0);
+ public static final Gains SWERVE_GAINS = new Gains(1.5, 0.0, 0.0, 0.0, 0, 1.0);
// swerve auto constants
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
- public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController (15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
+ public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3,
+ new TrapezoidProfile.Constraints(Math.PI, Math.PI));
public static final boolean PATH_RECORD_VELOCITY = true;
public static final double PATH_MAX_VELOCITY = 5.0;
public static final double PATH_MAX_ACCELERATION = 5.0;
@@ -89,19 +99,19 @@ public final class Constants {
// wheel diameter: official = 4 in, measured = 3.8 in
/* Ratio Calculation */
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
- public static final double MOTOR_REV_PER_WHEEL_REV = 5.142857;
+ public static final double MOTOR_REV_PER_WHEEL_REV = 6.12;// 5.142857;
public static final double WHEEL_DIAMETER_INCHES = 4.0;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double INCHES_PER_METER = 39.370;
- public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
-
- public static final double WHEEL_REV_PER_MOTOR_REV = 1/MOTOR_REV_PER_WHEEL_REV;
+ public static final double METERS_PER_INCH = 1 / INCHES_PER_METER;
+
+ 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 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 = 0.1;
- public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
+ public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
// misc
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
@@ -132,20 +142,22 @@ public final class Constants {
public static final int SHOOTER_TIMEOUT_MS = 32;
public static final int SHOOTER_SLOT_IDX = 0;
public static final int SHOOTER_PID_LOOP_IDX = 1;
- public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5);
+ public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(
+ true, 60, 40, 0.5);
public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23;
- public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24;
+ public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24;
public static final int SHOOTER_ROTATE_ID = 31; // TODO: find this value
public static final double TURRET_SPEED_MULTIPLIER = 0.1d;
public static final int DEGREES_PER_ROT = 0;
public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0;
public static final int TURRET_MOTOR_ROTS_PER_ROT = 0;
public static final double ENCODER_TICKS_PER_REV = 2048;
-
-
-
+
+ // Shoot Command Constants
+ public static final Gains SHOOT_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0);
+
/* Turret Constants */
- //ID
+ // ID
public static final int TURRET_MOTOR_CAN_ID = 30;
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
@@ -161,25 +173,26 @@ public final class Constants {
public static final double DIG_DEADZONE_RIGHT = 60.0;
public static final int SHOOTER_FALCON_BALLER_ID = 0; // TODO: find
- public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"//
-
- public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0); // TODO: tune values
+ public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; // "//
+
+ public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values
/* Hood Constants */
public static final int SHOOTER_ANGLE_ADJUST_ID = 32;
- public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find
- public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find
- public static final float HOOD_FORWARD_LIMIT = 0; //TODO: find
- public static final float HOOD_REVERSE_LIMIT = 0; //TODO: find
-
+ public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find
+ public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find
+ public static final float HOOD_FORWARD_LIMIT = 0; // TODO: find
+ public static final float HOOD_REVERSE_LIMIT = 0; // TODO: find
}
+
public static final class VisionConstants {
// public static final double TURN_P_VALUE = 0.8;
// public static final double X_ANGLE_ERROR = 0.5;
// public static final double GRAV = 385.83;
// public static final double TARGET_HEIGHT = 67.5;
// public static final double FOV = 29.8; //Field of view limelight
+
public static final double LIME_ANGLE = 24.7;
public static final String NAME = "photonCamera";
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index c4edf74..1e1ae82 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -32,7 +32,7 @@ import frc4388.utility.RobotTime;
public class Robot extends TimedRobot {
private static final Logger LOGGER = Logger.getLogger(Robot.class.getSimpleName());
Command m_autonomousCommand;
-
+
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
@@ -50,21 +50,32 @@ public class Robot extends TimedRobot {
LOGGER.log(Level.FINE, "Logging Test 6/8");
LOGGER.log(Level.FINER, "Logging Test 7/8");
LOGGER.log(Level.FINEST, "Logging Test 8/8");
- Errors.log().run(() -> { throw new Throwable("Exception Test"); });
+ Errors.log().run(() -> {
+ throw new Throwable("Exception Test");
+ });
- // var path = PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move Forward.path").toFile());
+ // var path =
+ // PathPlannerUtil.Path.read(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("Move
+ // Forward.path").toFile());
// LOGGER.finest(path::toString);
LOGGER.fine("robotInit()");
// LOGGER.fine("Sssssssssh.");
// DriverStation.silenceJoystickConnectionWarning(true);
- // Instantiate our RobotContainer. This will perform all our button bindings, and put our
+ // Instantiate our RobotContainer. This will perform all our button bindings,
+ // and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
// addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
SmartDashboard.putData(CommandScheduler.getInstance());
- SmartDashboard.putData("JVM Memory", new RunCommand(() -> {}) {
- @Override public boolean runsWhenDisabled() { return true; }
- @Override public String getName() {
+ SmartDashboard.putData("JVM Memory", new RunCommand(() -> {
+ }) {
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public String getName() {
if (isScheduled()) {
Runtime runtime = Runtime.getRuntime();
long totalMemory = runtime.totalMemory() / 1_000_000;
@@ -75,12 +86,20 @@ public class Robot extends TimedRobot {
return "Not Running";
}
});
- SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {}) {
- @Override public boolean runsWhenDisabled() { return true; }
- @Override public String getName() {
+ SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {
+ }) {
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
+
+ @Override
+ public String getName() {
if (isScheduled()) {
File deploy = Filesystem.getDeployDirectory();
- long usedSpace = Errors.suppress().getWithDefault(() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), 0l) / 1_000_000;
+ long usedSpace = Errors.suppress().getWithDefault(
+ () -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(),
+ 0l) / 1_000_000;
long usableSpace = deploy.getUsableSpace() / 1_000_000;
return usedSpace + " MB / " + usableSpace + " MB";
}
@@ -94,19 +113,24 @@ public class Robot extends TimedRobot {
* 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
+ *
+ * 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
+ // 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();
- // print odometry data to smart dashboard for debugging (if causing timeout errors, you can comment it)
+ // print odometry data to smart dashboard for debugging (if causing timeout
+ // errors, you can comment it)
SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
@@ -123,7 +147,8 @@ public class Robot extends TimedRobot {
m_robotTime.endMatchTime();
if (isTest()) {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
- File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("recording." + System.currentTimeMillis() + ".path").toFile();
+ File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner")
+ .resolve("recording." + System.currentTimeMillis() + ".path").toFile();
if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
m_robotContainer.createPath(null, null, false).write(outputFile);
LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
@@ -137,7 +162,8 @@ public class Robot extends TimedRobot {
}
/**
- * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+ * This autonomous runs the autonomous command selected by your
+ * {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index ef07b7e..75cb3f6 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -58,6 +58,7 @@ import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.AimToCenter;
+import frc4388.robot.commands.Shoot;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.LED;
@@ -84,12 +85,14 @@ public class RobotContainer {
private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
- public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
+ public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack,
+ m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
private final LED m_robotLED = new LED(m_robotMap.LEDController);
private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
private final Hood m_robotHood = new Hood();
private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
+
private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
// private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
@@ -104,11 +107,13 @@ public class RobotContainer {
private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
- private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH-mm-ss.SSS'.path'");
+ private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter
+ .ofPattern("'Recording' yyyy-MM-dd HH-mm-ss.SSS'.path'");
private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault());
private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner");
// Function that removes the ".path" from the end of a string.
- private static final Function PATH_EXTENSION_REMOVER = ((Function) Pattern.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
+ private static final Function PATH_EXTENSION_REMOVER = ((Function) Pattern
+ .compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -116,11 +121,12 @@ public class RobotContainer {
public RobotContainer() {
configureButtonBindings();
/* Default Commands */
-
- // continually sends updates to the Blinkin LED controller to keep the lights on
- // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
- //Turret default command
+ // continually sends updates to the Blinkin LED controller to keep the lights on
+ // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED,
+ // m_robotLED));
+
+ // Turret default command
m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
@@ -134,9 +140,13 @@ public class RobotContainer {
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// continually sends updates to the Blinkin LED controller to keep the lights on
- m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
- autoInit();
- recordInit();
+ /*
+ * m_robotLED
+ * .setDefaultCommand(new RunCommand(m_robotLED::updateLED,
+ * m_robotLED).withName("LED update defaultCommand"));
+ * autoInit();
+ * recordInit();
+ */
}
/**
@@ -149,32 +159,41 @@ public class RobotContainer {
/* Driver Buttons */
// "XboxController.Button.kBack" was undefined yet, 7 works just fine
new JoystickButton(getDriverController(), 7)
- .whenPressed(m_robotSwerveDrive::resetGyro);
+ .whenPressed(m_robotSwerveDrive::resetGyro);
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
- // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
+ // new XboxControllerRawButton(m_driverXbox,
+ // XboxControllerRaw.LEFT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
- // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
+ // new XboxControllerRawButton(m_driverXbox,
+ // XboxControllerRaw.RIGHT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
+ new JoystickButton(getDriverController(), XboxController.Button.kX.value)
+ .whenPressed(() -> m_robotMap.leftFront.reset())
+ .whenPressed(() -> m_robotMap.rightFront.reset())
+ .whenPressed(() -> m_robotMap.leftBack.reset())
+ .whenPressed(() -> m_robotMap.rightBack.reset());
/* Operator Buttons */
// activates "Lit Mode"
- new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
- .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
- .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
- // activates "BoomBoom"
- new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
- .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1))
- .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0));
- // activates hood
- new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
- .whenPressed(() -> m_robotHood.runHood(0.5d))
- .whenReleased(() -> m_robotHood.runHood(0.d));
+ /*
+ * new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+ * .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+ * .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+ *
+ * new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+ * .whenPressed(new InstantCommand());
+ *
+ * // activates "BoomBoom"
+ * new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+ * .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret,
+ * m_robotHood));
+ */
}
/**
@@ -200,7 +219,8 @@ public class RobotContainer {
new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
} else {
LOGGER.severe("No auto selected.");
- return new RunCommand(() -> {}).withName("No Autonomous Path");
+ return new RunCommand(() -> {
+ }).withName("No Autonomous Path");
}
}
@@ -210,6 +230,7 @@ public class RobotContainer {
/**
* Get odometry.
+ *
* @return Odometry
*/
public Pose2d getOdometry() {
@@ -218,6 +239,7 @@ public class RobotContainer {
/**
* Set odometry to given pose.
+ *
* @param pose Pose to set odometry to.
*/
public void resetOdometry(Pose2d pose) {
@@ -229,16 +251,23 @@ public class RobotContainer {
}
/**
- * Creates a WatchKey for the path planner directory and registers it with the WatchService.
- * Then creates a NotifierCommand that will update the auto chooser with the latest path files.
+ * Creates a WatchKey for the path planner directory and registers it with the
+ * WatchService.
+ * Then creates a NotifierCommand that will update the auto chooser with the
+ * latest path files.
* Finally, adds the existing path files to the auto chooser
*/
private void autoInit() {
try {
- WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, StandardWatchEventKinds.ENTRY_DELETE);
+ WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(),
+ StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
+ StandardWatchEventKinds.ENTRY_DELETE);
// TODO: Store this and other commands as fields so they can be rescheduled.
new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) {
- @Override public boolean runsWhenDisabled() { return true; }
+ @Override
+ public boolean runsWhenDisabled() {
+ return true;
+ }
}.withName("Path Watcher").schedule();
} catch (IOException exception) {
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
@@ -250,7 +279,8 @@ public class RobotContainer {
}
/**
- * Creates a button on the SmartDashboard that will record the path of the robot.
+ * Creates a button on the SmartDashboard that will record the path of the
+ * robot.
*/
public void recordInit() {
SmartDashboard.putData("Recording",
@@ -271,19 +301,22 @@ public class RobotContainer {
* Called when a file is created, modified, or deleted.
* Adds newly created .path files to the SendableChooser.
* Reloads the path if the currently selected file is modified.
+ *
* @param watchKey The WatchKey that is being observed.
*/
private void updateAutoChooser(WatchKey watchKey) {
List> watchEvents = watchKey.pollEvents();
if (!watchEvents.isEmpty()) {
- List> pathWatchEvents = watchEvents.stream().filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
+ List> pathWatchEvents = watchEvents.stream()
+ .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
for (WatchEvent pathWatchEvent : pathWatchEvents) {
Path watchEventPath = (Path) pathWatchEvent.context();
File watchEventFile = watchEventPath.toFile();
String watchEventFileName = watchEventFile.getName();
if (watchEventFileName.endsWith(".path")) {
if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
- LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", watchEventFileName);
+ LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.",
+ watchEventFileName);
autoChooser.addOption(watchEventFile.getName(), watchEventFile);
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
@@ -292,7 +325,9 @@ public class RobotContainer {
loadPath(watchEventFileName);
}
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
- LOGGER.log(Level.SEVERE, "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", watchEventFileName);
+ LOGGER.log(Level.SEVERE,
+ "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.",
+ watchEventFileName);
}
}
}
@@ -304,13 +339,15 @@ public class RobotContainer {
private void loadPath(String pathName) {
LOGGER.warning("Loading path " + pathName);
loadedPathTrajectory = null;
- loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
+ loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")),
+ SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
LOGGER.info("Done loading");
}
private void saveRecording() {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
- File outputFile = PATHPLANNER_DIRECTORY.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
+ File outputFile = PATHPLANNER_DIRECTORY
+ .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
// TODO: Change to use measured maximum velocity and acceleration.
@@ -329,14 +366,18 @@ public class RobotContainer {
public void recordPeriodic() {
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
- // FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity.
- Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
- Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
+ // FIXME: Chassis speeds are created from joystick inputs and do not reflect
+ // actual robot velocity.
+ Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond,
+ m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
+ Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
+ SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
pathPoints.add(waypoint);
}
public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
- // Remove points whose angles to neighboring points are less than 10 degrees apart.
+ // Remove points whose angles to neighboring points are less than 10 degrees
+ // apart.
int j = 0;
for (int i = 1; i < pathPoints.size() - 1; i++) {
var prev = pathPoints.get(j).anchorPoint.orElseThrow();
@@ -346,20 +387,26 @@ public class RobotContainer {
var toNext = next.minus(current);
var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY());
var angleToNext = new Rotation2d(toNext.getX(), toNext.getY());
- if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
+ if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE
+ || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE
+ && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false)))
pathPoints.set(i, null);
else
j = i;
}
pathPoints.removeIf(Objects::isNull);
// Make control points
- pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
+ pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(),
+ pathPoints.get(1).anchorPoint.orElseThrow()).getSecond());
for (int i = 1; i < pathPoints.size() - 1; i++) {
- var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
+ var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(),
+ pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow());
pathPoints.get(i).prevControl = Optional.of(controls.getFirst());
pathPoints.get(i).nextControl = Optional.of(controls.getSecond());
}
- pathPoints.get(pathPoints.size() - 1).prevControl = Optional.of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
+ pathPoints.get(pathPoints.size() - 1).prevControl = Optional
+ .of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(),
+ pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst());
// Create the path
PathPlannerUtil.Path path = new PathPlannerUtil.Path();
path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
@@ -370,7 +417,8 @@ public class RobotContainer {
return path;
}
- private static Pair makeControlPoints(Translation2d prev, Translation2d current, Translation2d next) {
+ private static Pair makeControlPoints(Translation2d prev, Translation2d current,
+ Translation2d next) {
var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4);
return Pair.of(current.minus(line), current.plus(line));
}
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
index 4f1478f..f7825bd 100644
--- a/src/main/java/frc4388/robot/RobotMap.java
+++ b/src/main/java/frc4388/robot/RobotMap.java
@@ -29,9 +29,9 @@ import frc4388.robot.subsystems.SwerveModule;
public class RobotMap {
public RobotMap() {
- configureLEDMotorControllers();
+ // configureLEDMotorControllers();
configureSwerveMotorControllers();
- configureShooterMotorControllers();
+ // configureShooterMotorControllers();
}
/* LED Subsystem */
@@ -74,61 +74,89 @@ public class RobotMap {
rightBackSteerMotor.configFactoryDefault();
rightBackWheelMotor.configFactoryDefault();
- leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
- rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
NeutralMode mode = NeutralMode.Coast;
leftFrontSteerMotor.setNeutralMode(mode);
- leftFrontWheelMotor.setNeutralMode(mode);//Coast
+ leftFrontWheelMotor.setNeutralMode(mode);// Coast
rightFrontSteerMotor.setNeutralMode(mode);
- rightFrontWheelMotor.setNeutralMode(mode);//Coast
+ rightFrontWheelMotor.setNeutralMode(mode);// Coast
leftBackSteerMotor.setNeutralMode(mode);
- leftBackWheelMotor.setNeutralMode(mode);//Coast
+ leftBackWheelMotor.setNeutralMode(mode);// Coast
rightBackSteerMotor.setNeutralMode(mode);
- rightBackWheelMotor.setNeutralMode(mode);//Coast
+ rightBackWheelMotor.setNeutralMode(mode);// Coast
- leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
- leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
- rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
- rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
+ leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder,
+ SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
+ leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder,
+ SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
+ rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder,
+ SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
+ rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder,
+ SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
// config cancoder as remote encoder for swerve steer motors
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
- RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
- SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(),
- RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
- SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(),
- RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
- SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(),
- RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
- SwerveDriveConstants.SWERVE_TIMEOUT_MS);
+ RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
+ SwerveDriveConstants.SWERVE_TIMEOUT_MS);
}
// Shooter Config
@@ -148,9 +176,12 @@ public class RobotMap {
shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
- shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
- shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
- shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
+ shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
+ ShooterConstants.SHOOTER_TIMEOUT_MS);
+ shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
+ ShooterConstants.SHOOTER_TIMEOUT_MS);
+ shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
+ ShooterConstants.SHOOTER_TIMEOUT_MS);
// RIGHT FALCON
shooterFalconRight.setInverted(false);
@@ -158,15 +189,25 @@ public class RobotMap {
shooterFalconRight.configFactoryDefault();
shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
- // m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
- shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
- shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS);
- shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
-
- /* Turret Subsytem */
- shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore
- shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore
+ // m_shooterFalconRight.configPeakOutputForward(0,
+ // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary)
+ shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX,
+ ShooterConstants.SHOOTER_TIMEOUT_MS);
+ shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS,
+ ShooterConstants.SHOOTER_TIMEOUT_MS);
+ shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG,
+ ShooterConstants.SHOOTER_TIMEOUT_MS);
+
+ // /* Turret Subsytem */
+ // shooterFalconRight.configStatorCurrentLimit(new
+ // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers
+ // out of our ass anymore
+ // shooterFalconLeft.configSupplyCurrentLimit(new
+ // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull
+ // numbers out of our ass anymore
}
-}
+ }
+
+
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java
index 8fc6500..be04921 100644
--- a/src/main/java/frc4388/robot/commands/Shoot.java
+++ b/src/main/java/frc4388/robot/commands/Shoot.java
@@ -4,12 +4,17 @@
package frc4388.robot.commands;
+import edu.wpi.first.hal.simulation.SimulatorJNI;
+import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
import edu.wpi.first.wpilibj2.command.CommandBase;
-
+import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
+import frc4388.utility.DummySensor;
+import frc4388.utility.Gains;
public class Shoot extends CommandBase {
@@ -34,17 +39,24 @@ public class Shoot extends CommandBase {
// pid
public double error;
public double prevError;
+ public Gains shootGains = ShooterConstants.SHOOT_GAINS;
public double kP, kI, kD;
public double proportional, integral, derivative;
public double time;
public double output;
public double tolerance = 5.0;
- // // dummy motor
- // public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420);
- // public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration();
+ // testing
+ public DummySensor dummy = new DummySensor(0);
- /** Creates a new Shoot. */
+ /**
+ * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball
+ * TODO: Velocity Correction
+ * @param sDrive Drive Train
+ * @param sShooter Shooter Drum
+ * @param sTurret Shooter Turret
+ * @param sHood Shooter Hood
+ */
public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
// Use addRequirements() here to declare subsystem dependencies.
m_swerve = sDrive;
@@ -54,14 +66,16 @@ public class Shoot extends CommandBase {
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
- kP = 0.1;
- kI = 0.0;
- kD = 0.0;
+ kP = shootGains.kP;
+ kI = shootGains.kI;
+ kD = shootGains.kD;
proportional = 0;
integral = 0;
derivative = 0;
time = 0.02;
+
+ DummySensor.resetAll();
}
/**
@@ -83,8 +97,13 @@ public class Shoot extends CommandBase {
// get targets (shooter tables)
m_targetVel = m_boomBoom.getVelocity(m_distance);
m_targetHood = m_boomBoom.getHood(m_distance);
+
+ // target angle tests
+ m_gyroAngle = 0;
+ m_odoX = -1;
+ m_odoY = 1;
+
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
- m_driveTargetAngle = m_swerve.getRegGyro().getDegrees();
// deadzone processing
if (AimToCenter.isHardwareDeadzone(m_targetAngle)) {
@@ -93,37 +112,14 @@ public class Shoot extends CommandBase {
if (AimToCenter.isDigitalDeadzone(m_targetAngle)) {
// this should rotate the entire swerve drive by 20 degrees, so shoot can now proceed like normal. idk if this will work
- m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle + 20), Math.sin(m_driveTargetAngle + 20), true);
+ m_swerve.driveWithInput(0, 0, Math.cos(m_gyroAngle + 20), Math.sin(m_gyroAngle + 20), true);
}
-
- // // normal (i think) PID stuff
- // dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice();
- // dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID();
- // dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
-
- // dummyConfiguration.slot0.kP = 0.1;
- // dummyConfiguration.slot0.kI = 0;
- // dummyConfiguration.slot0.kD = 0;
- // dummyConfiguration.slot0.kF = 0;
-
- // // weird PID stuff
- // dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice();
- // dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID;
- // dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
- // // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0;
-
- // dummyConfiguration.slot1.kP = 0.1;
- // dummyConfiguration.slot1.kI = 0;
- // dummyConfiguration.slot1.kD = 0;
- // dummyConfiguration.slot1.kF = 0;
-
- // dummy.configAllSettings(dummyConfiguration);
-
+
// initial error
updateError();
+ System.out.println("Error: " + error);
prevError = error;
}
-
/**
* Run custom PID.
*/
@@ -140,17 +136,12 @@ public class Shoot extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
- // dummy.selectProfileSlot(0, 0);
- // dummy.selectProfileSlot(1, 1);
- // dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle);
- // m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true);
- // m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch
// custom pid
runPID();
- m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the
- // entire swerve drive or its the commented line below
- // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
+ // m_swerve.driveWithInput(0, 0, output, true); // i have no idea if this is how you rotate the
+ // entire swerve drive or its the line below
+ m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true);
m_hood.runAngleAdjustPID(m_targetHood);
m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java
index c02de5e..e1286e5 100644
--- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java
+++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java
@@ -181,10 +181,10 @@ public class BoomBoom extends SubsystemBase {
public void setShooterGains() {
m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
- m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
- m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
- m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
- m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+ m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+ m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+ m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+ m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
}
public void runDrumShooterVelocityPID(double targetVel) {
diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java
index 5b1d2ff..c9e963f 100644
--- a/src/main/java/frc4388/robot/subsystems/Hood.java
+++ b/src/main/java/frc4388/robot/subsystems/Hood.java
@@ -21,31 +21,31 @@ import frc4388.utility.Gains;
public class Hood extends SubsystemBase {
public BoomBoom m_shooterSubsystem;
- // public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+ public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
- // public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
+ public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
- // public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
+ public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
- //public boolean m_isHoodReady = false;
+ public boolean m_isHoodReady = false;
public double m_fireAngle;
/** Creates a new Hood. */
public Hood() {
- // m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
+ m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
- // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
- // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
- // m_hoodUpLimitSwitch.enableLimitSwitch(true);
- // m_hoodDownLimitSwitch.enableLimitSwitch(true);
+ m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+ m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+ m_hoodUpLimitSwitch.enableLimitSwitch(true);
+ m_hoodDownLimitSwitch.enableLimitSwitch(true);
- // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
- // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
+ m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
+ m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
setHoodSoftLimits(true);
}
@@ -60,30 +60,30 @@ public double m_fireAngle;
* @param set Boolean to set soft limits to.
*/
public void setHoodSoftLimits(boolean set) {
- // m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
- // m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
+ m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
+ m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
}
public void runAngleAdjustPID(double targetAngle)
{
//Set PID Coefficients
- // m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP);
- // m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI);
- // m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD);
- // m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone);
- // m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF);
- // m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput);
+ m_angleAdjusterPIDController.setP(m_angleAdjusterGains.kP);
+ m_angleAdjusterPIDController.setI(m_angleAdjusterGains.kI);
+ m_angleAdjusterPIDController.setD(m_angleAdjusterGains.kD);
+ m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.kIzone);
+ m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.kF);
+ m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.kPeakOutput);
- // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
+ m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
}
public void runHood(double input) {
- // m_angleAdjusterMotor.set(input);
+ m_angleAdjusterMotor.set(input);
}
public void resetGyroAngleAdj(){
- // m_angleEncoder.setPosition(0);
+ m_angleEncoder.setPosition(0);
}
public double getAnglePosition(){
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index c32e833..785e8b5 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -38,19 +38,27 @@ public class SwerveDrive extends SubsystemBase {
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
- Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
- Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
- Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
- Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
-
- public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+ Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight),
+ Units.inchesToMeters(halfWidth));
+ Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight),
+ Units.inchesToMeters(-halfWidth));
+ Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
+ Units.inchesToMeters(halfWidth));
+ Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
+ Units.inchesToMeters(-halfWidth));
+
+ public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
+ m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
public WPI_PigeonIMU m_gyro;
protected FusionStatus fstatus = new FusionStatus();
- /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
- below are robot specific, and should be tuned. */
+ /*
+ * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
+ * The numbers used
+ * below are robot specific, and should be tuned.
+ */
public SwerveDrivePoseEstimator m_poseEstimator;
public SwerveDriveOdometry m_odometry;
public VisionOdometry m_visionOdometry;
@@ -62,7 +70,8 @@ public class SwerveDrive extends SubsystemBase {
private final Field2d m_field = new Field2d();
- public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
+ public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
+ WPI_PigeonIMU gyro) {
m_leftFront = leftFront;
m_leftBack = leftBack;
@@ -70,53 +79,55 @@ public class SwerveDrive extends SubsystemBase {
m_rightBack = rightBack;
m_gyro = gyro;
- modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
+ modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack };
- m_poseEstimator =
- new SwerveDrivePoseEstimator(
- m_gyro.getRotation2d(),
- new Pose2d(),
- m_kinematics,
- VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
- VecBuilder.fill(Units.degreesToRadians(1)),
- VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
+ m_poseEstimator = new SwerveDrivePoseEstimator(
+ m_gyro.getRotation2d(),
+ new Pose2d(),
+ m_kinematics,
+ VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
+ VecBuilder.fill(Units.degreesToRadians(1)),
+ VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
- m_gyro.reset();
+ m_gyro.reset();
SmartDashboard.putData("Field", m_field);
}
-//https://github.com/ZachOrr/MK3-Swerve-Example
- /**
- * Method to drive the robot using joystick info.
- *
- * @param speeds[0] Speed of the robot in the x direction (forward).
- * @param speeds[1] Speed of the robot in the y direction (sideways).
- * @param rot Angular rate of the robot.
- * @param fieldRelative Whether the provided x and y speeds are relative to the field.
- */
- public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative)
- {
- if (speedX == 0 && speedY == 0 && rot == 0) ignoreAngles = true;
- else ignoreAngles = false;
- Translation2d speed = new Translation2d(speedX, speedY);
+
+ // https://github.com/ZachOrr/MK3-Swerve-Example
+ /**
+ * Method to drive the robot using joystick info.
+ *
+ * @param speeds[0] Speed of the robot in the x direction (forward).
+ * @param speeds[1] Speed of the robot in the y direction (sideways).
+ * @param rot Angular rate of the robot.
+ * @param fieldRelative Whether the provided x and y speeds are relative to the
+ * field.
+ */
+ public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
+ if (speedX == 0 && speedY == 0 && rot == 0)
+ ignoreAngles = true;
+ else
+ ignoreAngles = false;
+ Translation2d speed = new Translation2d(-speedX, speedY);
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
- SwerveModuleState[] states =
- m_kinematics.toSwerveModuleStates(
- fieldRelative
- ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
- : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
+ SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
+ rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
+ rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
- public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
- {
+ public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
- Translation2d speed = new Translation2d(leftX, leftY);
+ Translation2d speed = new Translation2d(-leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
@@ -124,20 +135,22 @@ public class SwerveDrive extends SubsystemBase {
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
- ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
- : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
- SwerveModuleState[] states =
- m_kinematics.toSwerveModuleStates(
- chassisSpeeds);
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
+ rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
+ SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
+ chassisSpeeds);
setModuleStates(states);
}
/**
* 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_SEC));
+ SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
+ Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
@@ -165,13 +178,16 @@ public class SwerveDrive extends SubsystemBase {
SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees());
// chassis speeds
- // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
+ // TODO: find the actual max velocity in m/s of the robot in fast mode to have
+ // accurate chassis speeds
SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
+
/**
* Gets the distance between two given poses.
+ *
* @param p1 The first pose.
* @param p2 The second pose.
* @return Absolute distance between p1 and p2.
@@ -184,13 +200,14 @@ public class SwerveDrive extends SubsystemBase {
* Returns a scalar from your distance to the hub to your target distance.
*
* @param target_dist The target distance.
- * @return A scalar that multiplies your distance from the hub to get your target distance.
+ * @return A scalar that multiplies your distance from the hub to get your
+ * target distance.
*/
public Pose2d poseGivenDist(double target_dist) {
Pose2d p1 = m_poseEstimator.getEstimatedPosition();
Pose2d p2 = SwerveDriveConstants.HUB_POSE;
- double scalar = target_dist/distBtwPoses(p1, p2);
+ double scalar = target_dist / distBtwPoses(p1, p2);
Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation());
return new_pose;
@@ -198,6 +215,7 @@ public class SwerveDrive extends SubsystemBase {
/**
* Gets the current pose of the robot.
+ *
* @return Robot's current pose.
*/
public Pose2d getOdometry() {
@@ -207,6 +225,7 @@ public class SwerveDrive extends SubsystemBase {
/**
* Gets the current gyro using regression formula.
+ *
* @return Rotation2d object holding current gyro in radians
*/
public Rotation2d getRegGyro() {
@@ -221,8 +240,9 @@ public class SwerveDrive extends SubsystemBase {
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
}
- /** Updates the field relative position of the robot.
- */
+ /**
+ * Updates the field relative position of the robot.
+ */
public void updateOdometry() {
m_poseEstimator.update( getRegGyro(),
modules[0].getState(),
@@ -243,7 +263,7 @@ public class SwerveDrive extends SubsystemBase {
/**
* Resets pigeon.
*/
- public void resetGyro(){
+ public void resetGyro() {
m_gyro.reset();
rotTarget = new Rotation2d(0);
}
@@ -260,13 +280,13 @@ public class SwerveDrive extends SubsystemBase {
/**
* Switches speed modes.
+ *
* @param shift True if fast mode, false if slow mode.
*/
- public void highSpeed(boolean shift){
- if (shift){
+ public void highSpeed(boolean shift) {
+ if (shift) {
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
- }
- else{
+ } else {
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
}
}
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index 44afb70..bd1d8ee 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -30,7 +30,7 @@ public class SwerveModule extends SubsystemBase {
private static double kEncoderTicksPerRotation = 4096;
private SwerveModuleState state;
private double canCoderFeedbackCoefficient;
-
+
public long m_currentTime;
public long m_lastTime;
public double m_deltaTime;
@@ -47,21 +47,22 @@ public class SwerveModule extends SubsystemBase {
TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
- angleTalonFXConfiguration.slot0.kP = m_swerveGains.m_kP;
- angleTalonFXConfiguration.slot0.kI = m_swerveGains.m_kI;
- angleTalonFXConfiguration.slot0.kD = m_swerveGains.m_kD;
+ angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
+ angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
+ angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD;
// Use the CANCoder as the remote sensor for the primary TalonFX PID
angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleTalonFXConfiguration);
-
+ // angleMotor.setInverted(true);
// TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
// driveTalonFXConfiguration.slot0.kP = 0.05;
// driveTalonFXConfiguration.slot0.kI = 0.0;
// driveTalonFXConfiguration.slot0.kD = 0.0;
- // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor;
+ // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor =
+ // FeedbackDevice.IntegratedSensor;
driveMotor.configFactoryDefault();
driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
driveMotor.configNominalOutputForward(0, 30);
@@ -69,15 +70,17 @@ public class SwerveModule extends SubsystemBase {
driveMotor.configPeakOutputForward(1, 30);
driveMotor.configPeakOutputReverse(-1, 30);
driveMotor.configAllowableClosedloopError(0, 0, 30);
- driveMotor.config_kP(0, 0.5, 30);
+ // driveMotor.setInverted(true);
+ driveMotor.config_kP(0, 0, 30);
driveMotor.config_kI(0, 0, 30);
driveMotor.config_kD(0, 0, 30);
// maybe try a feedforward value?
-
+
// driveMotor.configAllSettings(driveTalonFXConfiguration);
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
canCoderConfiguration.magnetOffsetDegrees = offset;
+ canCoderConfiguration.sensorDirection = true;
canCoder.configAllSettings(canCoderConfiguration);
m_currentTime = System.currentTimeMillis();
@@ -87,40 +90,50 @@ public class SwerveModule extends SubsystemBase {
}
private Rotation2d getAngle() {
- // Note: This assumes the CANCoders are setup with the default feedback coefficient
+ // Note: This assumes the CANCoders are setup with the default feedback
+ // coefficient
// and the sensor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
}
/**
* Set the speed + rotation of the swerve module from a SwerveModuleState object
- * @param desiredState - A SwerveModuleState representing the desired new state of the module
+ *
+ * @param desiredState - A SwerveModuleState representing the desired new state
+ * of the module
*/
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
Rotation2d currentRotation = getAngle();
- // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
+ // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(),
+ // currentRotation.getDegrees());
state = SwerveModuleState.optimize(desiredState, currentRotation);
- // Find the difference between our current rotational position + our new rotational position
+ // Find the difference between our current rotational position + our new
+ // rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
- // Find the new absolute position of the module based on the difference in rotation
+ // Find the new absolute position of the module based on the difference in
+ // rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
// Convert the CANCoder from it's position reading back to ticks
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
double desiredTicks = currentTicks + deltaTicks;
- if (!ignoreAngle){
+ if (!ignoreAngle) {
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
}
+ // Please work
double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
// double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
-
- // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+
+ // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection +
+ // (Units.metersToInches(state.speedMetersPerSecond) *
+ // SwerveDriveConstants.TICKS_PER_INCH) / 10);
driveMotor.set(normFtPerSec);// - angleMotor.get());
- // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
+ // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio
+ // between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
// m_currentTime = System.currentTimeMillis();
// m_deltaTime = (double) (m_currentTime - m_lastTime);
@@ -129,10 +142,14 @@ public class SwerveModule extends SubsystemBase {
// m_currentPos = driveMotor.getSelectedSensorPosition();
// double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity();
- // double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % 2048;
- // double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - (m_deltaTime * driveMotor.getSelectedSensorVelocity());
- // double m_actualDesiredPos = m_deltaTime * ((Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
-
+ // double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) %
+ // 2048;
+ // double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) -
+ // (m_deltaTime * driveMotor.getSelectedSensorVelocity());
+ // double m_actualDesiredPos = m_deltaTime *
+ // ((Units.metersToInches(state.speedMetersPerSecond) *
+ // SwerveDriveConstants.TICKS_PER_INCH) / 10);
+
// System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition());
// System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos);
// System.out.println("Last Pos: " + m_lastPos);
@@ -150,7 +167,8 @@ public class SwerveModule extends SubsystemBase {
*/
public SwerveModuleState getState() {
// return state;
- return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
+ return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK
+ * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
}
/**
@@ -160,11 +178,18 @@ public class SwerveModule extends SubsystemBase {
driveMotor.set(0);
angleMotor.set(0);
}
+
@Override
- public void periodic(){
+ public void periodic() {
Rotation2d currentRotation = getAngle();
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
- SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
+ SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
+ ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
+ }
+
+ public void reset() {
+ canCoder.setPositionToAbsolute();
+ // canCoder.configSensorInitializationStrategy(initializationStrategy)
}
}
diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java
index 3a00f02..7a0f45e 100644
--- a/src/main/java/frc4388/robot/subsystems/Turret.java
+++ b/src/main/java/frc4388/robot/subsystems/Turret.java
@@ -63,12 +63,12 @@ public class Turret extends SubsystemBase {
m_boomBoomRotateMotor.setInverted(false);
- m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP);
- m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI);
- m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD);
- m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF);
- m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone);
- m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput);
+ m_boomBoomRotatePIDController.setP(m_shooterTGains.kP);
+ m_boomBoomRotatePIDController.setI(m_shooterTGains.kI);
+ m_boomBoomRotatePIDController.setD(m_shooterTGains.kD);
+ m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF);
+ m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone);
+ m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput);
}
@Override
diff --git a/src/main/java/frc4388/utility/DummySensor.java b/src/main/java/frc4388/utility/DummySensor.java
new file mode 100644
index 0000000..dadef80
--- /dev/null
+++ b/src/main/java/frc4388/utility/DummySensor.java
@@ -0,0 +1,74 @@
+// 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;
+
+import java.util.ArrayList;
+
+public class DummySensor {
+
+ private double value;
+ public double start;
+ public static ArrayList instances = new ArrayList();
+
+ /**
+ * Creates a new DummySensor, which is a helper class for conducting PID tests without a physical robot.
+ * @param init The start "position" of the sensor (default is 0).
+ */
+ public DummySensor(double init) {
+ value = init;
+ start = init;
+ instances.add(this);
+ }
+
+ /**
+ * Creates a new DummySensor, which is a helper class for conducting PID tests without a physical robot.
+ */
+ public DummySensor() {
+ value = 0;
+ start = 0;
+ instances.add(this);
+ }
+
+ /**
+ * Reset the "position" of the DummySensor to its starting value.
+ */
+ public void reset() {
+ value = start;
+ }
+
+ /**
+ * Reset the "position" of the DummySensor to a given value.
+ * @param val The "position" to reset the DummySensor to.
+ */
+ public void reset(double val) {
+ value = val;
+ }
+
+ /**
+ * Reset all instances of DummySensor to their starting values.
+ */
+ public static void resetAll() {
+ for (DummySensor instance : instances) {
+ instance.reset();
+ }
+ }
+
+ /**
+ * Get the "position" of the DummySensor.
+ * @return The current "position".
+ */
+ public double get() {
+ return value;
+ }
+
+ /**
+ * Apply an input to the DummySensor, changing its "position".
+ * @param input The input to apply.
+ */
+ public void apply(double input) {
+ value = value + input;
+ }
+
+}
diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java
index 930e34f..13144a4 100644
--- a/src/main/java/frc4388/utility/Gains.java
+++ b/src/main/java/frc4388/utility/Gains.java
@@ -6,14 +6,14 @@ package frc4388.utility;
/** Add your docs here. */
public class Gains {
- public double m_kP;
- public double m_kI;
- public double m_kD;
- public double m_kF;
- public int m_kIzone;
- public double m_kPeakOutput;
- public double m_kmaxOutput;
- public double m_kminOutput;
+ 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
@@ -26,14 +26,14 @@ public class Gains {
*/
public Gains(double kP, double kI, double kD, double kF, int kIzone, double kPeakOutput)
{
- m_kP = kP;
- m_kI = kI;
- m_kD = kD;
- m_kF = kF;
- m_kIzone = kIzone;
- m_kPeakOutput = kPeakOutput;
- m_kmaxOutput = m_kPeakOutput;
- m_kminOutput = -m_kPeakOutput;
+ this.kP = kP;
+ this.kI = kI;
+ this.kD = kD;
+ this.kF = kF;
+ this.kIzone = kIzone;
+ this.kPeakOutput = kPeakOutput;
+ this.kmaxOutput = this.kPeakOutput;
+ this.kminOutput = -this.kPeakOutput;
}
/**
@@ -48,13 +48,13 @@ public class Gains {
*/
public Gains(double kP, double kI, double kD, double kF, int kIzone, double kMinOutput, double kMaxOutput)
{
- m_kP = kP;
- m_kI = kI;
- m_kD = kD;
- m_kF = kF;
- m_kIzone = kIzone;
- m_kminOutput = kMinOutput;
- m_kmaxOutput = kMaxOutput;
- m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
+ this.kP = kP;
+ this.kI = kI;
+ this.kD = kD;
+ this.kF = kF;
+ this.kIzone = kIzone;
+ this.kminOutput = kMinOutput;
+ this.kmaxOutput = kMaxOutput;
+ this.kPeakOutput = (Math.abs(this.kminOutput) > Math.abs(this.kmaxOutput)) ? Math.abs(this.kminOutput) : Math.abs(this.kmaxOutput);
}
}