mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
Real robo changes
This commit is contained in:
evan
@@ -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,19 @@ 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;
|
||||
|
||||
|
||||
|
||||
|
||||
/* 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 +170,25 @@ 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 FOV = 29.8; // Field of view limelight
|
||||
public static final double LIME_ANGLE = 24.7;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
* <p>This runs after the mode specific periodic functions, but before
|
||||
* <p>
|
||||
* 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() {
|
||||
|
||||
@@ -83,14 +83,19 @@ 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 Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom);
|
||||
/* Controllers */
|
||||
/*
|
||||
* 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 Vision m_robotVison = new Vision(m_robotTurret,
|
||||
* // m_robotBoomBoom);
|
||||
* /* Controllers
|
||||
*/
|
||||
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
@@ -101,11 +106,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<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
|
||||
private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern
|
||||
.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
@@ -113,13 +120,15 @@ 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));
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED,
|
||||
// m_robotLED));
|
||||
|
||||
//Turret default command
|
||||
// Turret default command
|
||||
|
||||
// m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
|
||||
// m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret,
|
||||
// m_robotSwerveDrive));
|
||||
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
@@ -131,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();
|
||||
*/
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -146,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.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));
|
||||
*/
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -197,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");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -207,6 +230,7 @@ public class RobotContainer {
|
||||
|
||||
/**
|
||||
* Get odometry.
|
||||
*
|
||||
* @return Odometry
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
@@ -215,6 +239,7 @@ public class RobotContainer {
|
||||
|
||||
/**
|
||||
* Set odometry to given pose.
|
||||
*
|
||||
* @param pose Pose to set odometry to.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
@@ -226,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);
|
||||
@@ -247,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",
|
||||
@@ -268,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<WatchEvent<?>> watchEvents = watchKey.pollEvents();
|
||||
if (!watchEvents.isEmpty()) {
|
||||
List<WatchEvent<?>> pathWatchEvents = watchEvents.stream().filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList());
|
||||
List<WatchEvent<?>> 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);
|
||||
@@ -289,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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -301,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.
|
||||
@@ -326,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();
|
||||
@@ -343,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));
|
||||
@@ -367,7 +417,8 @@ public class RobotContainer {
|
||||
return path;
|
||||
}
|
||||
|
||||
private static Pair<Translation2d, Translation2d> makeControlPoints(Translation2d prev, Translation2d current, Translation2d next) {
|
||||
private static Pair<Translation2d, Translation2d> 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));
|
||||
}
|
||||
|
||||
@@ -29,9 +29,9 @@ import frc4388.robot.subsystems.SwerveModule;
|
||||
public class RobotMap {
|
||||
|
||||
public RobotMap() {
|
||||
configureLEDMotorControllers();
|
||||
// configureLEDMotorControllers();
|
||||
configureSwerveMotorControllers();
|
||||
configureShooterMotorControllers();
|
||||
// configureShooterMotorControllers();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
@@ -74,99 +74,148 @@ 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
|
||||
/* Boom Boom Subsystem */
|
||||
public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
|
||||
public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
|
||||
|
||||
// public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
||||
|
||||
// Create motor CANSparkMax
|
||||
void configureShooterMotorControllers() {
|
||||
|
||||
// LEFT FALCON
|
||||
shooterFalconLeft.configFactoryDefault();
|
||||
shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
||||
shooterFalconLeft.setInverted(true);
|
||||
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);
|
||||
|
||||
// RIGHT FALCON
|
||||
shooterFalconRight.setInverted(false);
|
||||
shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
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
|
||||
|
||||
}
|
||||
/*
|
||||
* public final WPI_TalonFX shooterFalconLeft = new
|
||||
* WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID);
|
||||
* public final WPI_TalonFX shooterFalconRight = new
|
||||
* WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID);
|
||||
*
|
||||
* // public final CANSparkMax shooterTurret = new
|
||||
* CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless);
|
||||
*
|
||||
* // Create motor CANSparkMax
|
||||
* void configureShooterMotorControllers() {
|
||||
*
|
||||
* // LEFT FALCON
|
||||
* shooterFalconLeft.configFactoryDefault();
|
||||
* shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
|
||||
* shooterFalconLeft.setInverted(true);
|
||||
* 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);
|
||||
*
|
||||
* // RIGHT FALCON
|
||||
* shooterFalconRight.setInverted(false);
|
||||
* shooterFalconRight.setNeutralMode(NeutralMode.Coast);
|
||||
* 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
|
||||
*/
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
@@ -36,19 +36,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;
|
||||
|
||||
@@ -59,7 +67,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;
|
||||
@@ -67,53 +76,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));
|
||||
@@ -121,20 +132,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];
|
||||
@@ -162,13 +175,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.
|
||||
@@ -181,13 +197,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;
|
||||
@@ -195,6 +212,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
/**
|
||||
* Gets the current pose of the robot.
|
||||
*
|
||||
* @return Robot's current pose.
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
@@ -204,6 +222,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
/**
|
||||
* Gets the current gyro using regression formula.
|
||||
*
|
||||
* @return Rotation2d object holding current gyro in radians
|
||||
*/
|
||||
public Rotation2d getRegGyro() {
|
||||
@@ -218,26 +237,28 @@ 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(),
|
||||
modules[1].getState(),
|
||||
modules[2].getState(),
|
||||
modules[3].getState());
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
// m_poseEstimator.addVisionMeasurement(
|
||||
// m_poseEstimator.getEstimatedPosition(),
|
||||
// Timer.getFPGATimestamp() - 0.1);
|
||||
}
|
||||
|
||||
m_poseEstimator.update(getRegGyro(),
|
||||
modules[0].getState(),
|
||||
modules[1].getState(),
|
||||
modules[2].getState(),
|
||||
modules[3].getState());
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example
|
||||
// -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
// m_poseEstimator.addVisionMeasurement(
|
||||
// m_poseEstimator.getEstimatedPosition(),
|
||||
// Timer.getFPGATimestamp() - 0.1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets pigeon.
|
||||
*/
|
||||
public void resetGyro(){
|
||||
public void resetGyro() {
|
||||
m_gyro.reset();
|
||||
rotTarget = new Rotation2d(0);
|
||||
}
|
||||
@@ -254,13 +275,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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
@@ -56,12 +56,13 @@ public class SwerveModule extends SubsystemBase {
|
||||
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)
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user