Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

formatting

Browse Source
This commit is contained in:
Connor Peach
2023-10-26 17:33:22 -06:00
parent fc6ad901ef
commit c16471688f
53 changed files with 4880 additions and 4865 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+124 -124
View File
@@ -25,154 +25,154 @@ import frc4388.utility.RobotUnits;
* constants are needed, to reduce verbosity. * constants are needed, to reduce verbosity.
*/ */
public final class Constants { public final class Constants {
public static final class SwerveDriveConstants { public static final class SwerveDriveConstants {
public static final double MAX_ROT_SPEED = 1.5; public static final double MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 0.8; public static final double MIN_ROT_SPEED = 0.8;
public static double ROTATION_SPEED = MAX_ROT_SPEED; public static double ROTATION_SPEED = MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50; public static final double CORRECTION_MAX = 50;
public static final double SLOW_SPEED = 0.8; public static final double SLOW_SPEED = 0.8;
public static final double FAST_SPEED = 1.0; public static final double FAST_SPEED = 1.0;
public static final double TURBO_SPEED = 4.0; public static final double TURBO_SPEED = 4.0;
public static final class IDs { public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2; public static final int LEFT_FRONT_WHEEL_ID = 2;
public static final int LEFT_FRONT_STEER_ID = 3; public static final int LEFT_FRONT_STEER_ID = 3;
public static final int LEFT_FRONT_ENCODER_ID = 10; public static final int LEFT_FRONT_ENCODER_ID = 10;
public static final int RIGHT_FRONT_WHEEL_ID = 4; public static final int RIGHT_FRONT_WHEEL_ID = 4;
public static final int RIGHT_FRONT_STEER_ID = 5; public static final int RIGHT_FRONT_STEER_ID = 5;
public static final int RIGHT_FRONT_ENCODER_ID = 11; public static final int RIGHT_FRONT_ENCODER_ID = 11;
public static final int LEFT_BACK_WHEEL_ID = 6; public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7; public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12; public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int RIGHT_BACK_WHEEL_ID = 8; public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9; public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13; public static final int RIGHT_BACK_ENCODER_ID = 13;
}
public static final class PIDConstants {
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
}
public static final class AutoConstants {
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
}
public static final class Conversions {
public static final int CANCODER_TICKS_PER_ROTATION = 4096;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double WHEEL_DIAMETER_INCHES = 3.9;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
public static final double TICK_TIME_TO_SECONDS = 10;
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
}
public static final class Configurations {
public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
}
public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
// dimensions
public static final double WIDTH = 18.5;
public static final double HEIGHT = 18.5;
public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d;
// misc
public static final int TIMEOUT_MS = 30;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
} }
public static final class PIDConstants { public static final class GyroConstants {
public static final int SWERVE_SLOT_IDX = 0; public static final int ID = 14;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
} }
public static final class AutoConstants { public static final class ArmConstants {
public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); public static final double MIN_ARM_LEN = 1;
public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); public static final double MAX_ARM_LEN = 2;
public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); public static final double ARM_HEIGHT = 1;
public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune public static final double CURVE_POWER = 2;
public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value public static final double TELE_TICKS_PER_SECOND = (-5);
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value
public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value
public static final double TELE_FORWARD_SOFT_LIMIT = 100;
public static final double TELE_REVERSE_SOFT_LIMIT = 0;
public static final double kP = 0;
public static final double kI = 0;
public static final double kD = 0;
public static final double OFFSET = 0;
} }
public static final class Conversions { public static final class LEDConstants {
public static final int CANCODER_TICKS_PER_ROTATION = 4096; // public static final int LED_SPARK_ID = 0;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double WHEEL_DIAMETER_INCHES = 3.9;
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
public static final double TICK_TIME_TO_SECONDS = 10;
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
} }
public static final class Configurations { public static final class OIConstants {
public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value public static final int XBOX_DRIVER_ID = 0;
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value public static final int XBOX_OPERATOR_ID = 1;
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value public static final double LEFT_AXIS_DEADBAND = 0.1;
public static final double RIGHT_AXIS_DEADBAND = 0.6;
public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
} }
public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value public static final class VisionConstants {
public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value public static final String NAME = "photonCamera";
// dimensions public static final int LIME_HIXELS = 640;
public static final double WIDTH = 18.5; public static final int LIME_VIXELS = 480;
public static final double HEIGHT = 18.5;
public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d;
// misc public static final double H_FOV = 59.6;
public static final int TIMEOUT_MS = 30; public static final double V_FOV = 45.7;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class GyroConstants { public static final double LIME_HEIGHT = 6.0;
public static final int ID = 14; public static final double LIME_ANGLE = 55.0;
}
public static final class ArmConstants { // public static final double HIGH_TARGET_HEIGHT = 46.0;
public static final double MIN_ARM_LEN = 1; public static final double HIGH_TAPE_HEIGHT = 44.0;
public static final double MAX_ARM_LEN = 2;
public static final double ARM_HEIGHT = 1;
public static final double CURVE_POWER = 2;
public static final double TELE_TICKS_PER_SECOND = (-5); // public static final double MID_TARGET_HEIGHT = 34.0;
public static final double MID_TAPE_HEIGHT = 24.0;
public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value
public static final double TELE_FORWARD_SOFT_LIMIT = 100; }
public static final double TELE_REVERSE_SOFT_LIMIT = 0;
public static final double kP = 0;
public static final double kI = 0;
public static final double kD = 0;
public static final double OFFSET = 0;
}
public static final class LEDConstants {
// public static final int LED_SPARK_ID = 0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final double LEFT_AXIS_DEADBAND = 0.1;
public static final double RIGHT_AXIS_DEADBAND = 0.6;
public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
}
public static final class VisionConstants {
public static final String NAME = "photonCamera";
public static final int LIME_HIXELS = 640;
public static final int LIME_VIXELS = 480;
public static final double H_FOV = 59.6;
public static final double V_FOV = 45.7;
public static final double LIME_HEIGHT = 6.0;
public static final double LIME_ANGLE = 55.0;
// public static final double HIGH_TARGET_HEIGHT = 46.0;
public static final double HIGH_TAPE_HEIGHT = 44.0;
// public static final double MID_TARGET_HEIGHT = 34.0;
public static final double MID_TAPE_HEIGHT = 24.0;
public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
}
} }
src/main/java/frc4388/robot/Main.java
+10 -10
View File
@@ -15,15 +15,15 @@ import edu.wpi.first.wpilibj.RobotBase;
* change the parameter class to the startRobot call. * change the parameter class to the startRobot call.
*/ */
public final class Main { public final class Main {
private Main() { private Main() {
} }
/** /**
* Main initialization function. Do not perform any initialization here. * Main initialization function. Do not perform any initialization here.
* *
* <p>If you change your main robot class, change the parameter type. * <p>If you change your main robot class, change the parameter type.
*/ */
public static void main(String... args) { public static void main(String... args) {
RobotBase.startRobot(Robot::new); RobotBase.startRobot(Robot::new);
} }
} }
src/main/java/frc4388/robot/Robot.java
+98 -98
View File
@@ -22,113 +22,113 @@ import frc4388.utility.RobotTime;
* project. * project.
*/ */
public class Robot extends TimedRobot { public class Robot extends TimedRobot {
Command m_autonomousCommand; Command m_autonomousCommand;
private RobotTime m_robotTime = RobotTime.getInstance(); private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer; private RobotContainer m_robotContainer;
/** /**
* This function is run when the robot is first started up and should be * This function is run when the robot is first started up and should be
* used for any initialization code. * used for any initialization code.
*/ */
@Override @Override
public void robotInit() { public void robotInit() {
// 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. // autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer(); m_robotContainer = new RobotContainer();
SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser); SmartDashboard.putData("AutoPlayback Chooser", m_robotContainer.chooser);
}
/**
* This function is called every robot packet, no matter the mode. Use
* this for items like diagnostics that you want ran during disabled,
* autonomous, teleoperated and test.
*
* <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
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
* You can use it to reset any subsystem information you want to clear when
* the robot is disabled.
*/
@Override
public void disabledInit() {
m_robotTime.endMatchTime();
}
@Override
public void disabledPeriodic() {}
@Override
public void disabledExit() {
DeferredBlock.execute();
super.disabledExit();
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_robotContainer.m_robotSwerveDrive.resetGyro();
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
} }
m_robotTime.startMatchTime();
}
/** /**
* This function is called periodically during autonomous. * This function is called every robot packet, no matter the mode. Use
*/ * this for items like diagnostics that you want ran during disabled,
@Override * autonomous, teleoperated and test.
public void autonomousPeriodic() { *
} * <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
@Override // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
public void teleopInit() { // commands, running already-scheduled commands, removing finished or interrupted commands,
m_robotContainer.m_robotSwerveDrive.resetGyro(); // 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.
// This makes sure that the autonomous stops running when CommandScheduler.getInstance().run();
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
} }
m_robotContainer.m_robotSwerveDrive.resetGyro();
m_robotTime.startMatchTime();
m_robotContainer.m_robotMap.restart_motor_tests(); /**
} * This function is called once each time the robot enters Disabled mode.
* You can use it to reset any subsystem information you want to clear when
* the robot is disabled.
*/
@Override
public void disabledInit() {
m_robotTime.endMatchTime();
}
/** @Override
* This function is called periodically during operator control. public void disabledPeriodic() {}
*/
@Override
public void teleopPeriodic() {}
/** @Override
* This function is called periodically during test mode. public void disabledExit() {
*/ DeferredBlock.execute();
@Override super.disabledExit();
public void testPeriodic() { }
m_robotContainer.m_robotMap.run_periodic_tests();
} /**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_robotContainer.m_robotSwerveDrive.resetGyro();
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
m_robotTime.startMatchTime();
}
/**
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
}
@Override
public void teleopInit() {
m_robotContainer.m_robotSwerveDrive.resetGyro();
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
m_robotContainer.m_robotSwerveDrive.resetGyro();
m_robotTime.startMatchTime();
m_robotContainer.m_robotMap.restart_motor_tests();
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
m_robotContainer.m_robotMap.run_periodic_tests();
}
} }
src/main/java/frc4388/robot/RobotContainer.java
+72 -72
View File
@@ -1,4 +1,4 @@
/*----------------------------------------------------------------------------*/ x/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */ /* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */ /* must be accompanied by the FIRST BSD license file in the root directory of */
@@ -94,13 +94,13 @@ public class RobotContainer {
private SequentialCommandGroup alignToPole = private SequentialCommandGroup alignToPole =
new SequentialCommandGroup( new SequentialCommandGroup(
new RotateToAngle(m_robotSwerveDrive, 0.0), new RotateToAngle(m_robotSwerveDrive, 0.0),
new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight), new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04), new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
new RotateToAngle(m_robotSwerveDrive, 0.0), new RotateToAngle(m_robotSwerveDrive, 0.0),
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
// new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape()) // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)); ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
// private SequentialCommandGroup alignToShelf = // private SequentialCommandGroup alignToShelf =
// new SequentialCommandGroup( // new SequentialCommandGroup(
@@ -113,12 +113,12 @@ public class RobotContainer {
private SequentialCommandGroup alignToShelf = private SequentialCommandGroup alignToShelf =
new SequentialCommandGroup( new SequentialCommandGroup(
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight), new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04), new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight), new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive) new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)); ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
public SequentialCommandGroup place = null; public SequentialCommandGroup place = null;
@@ -129,41 +129,41 @@ public class RobotContainer {
// TODO: find actual values // TODO: find actual values
private SequentialCommandGroup placeCubeHigh = private SequentialCommandGroup placeCubeHigh =
new SequentialCommandGroup( new SequentialCommandGroup(
// new InstantCommand(() -> System.out.println("Placing cone high")), // new InstantCommand(() -> System.out.println("Placing cone high")),
new PivotCommand(m_robotArm, 64 + 135), new PivotCommand(m_robotArm, 64 + 135),
new InstantCommand(() -> m_robotArm.setRotVel(0)), new InstantCommand(() -> m_robotArm.setRotVel(0)),
new TeleCommand(m_robotArm, 95642), new TeleCommand(m_robotArm, 95642),
toggleClaw.asProxy(), toggleClaw.asProxy(),
armToHome.asProxy() armToHome.asProxy()
); );
private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup( private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 70 + 135), new PivotCommand(m_robotArm, 70 + 135),
new TeleCommand(m_robotArm, 32866), new TeleCommand(m_robotArm, 32866),
toggleClaw.asProxy(), toggleClaw.asProxy(),
armToHome.asProxy() armToHome.asProxy()
); );
private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup( private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 0),
new TeleCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0),
toggleClaw.asProxy(), toggleClaw.asProxy(),
armToHome.asProxy() armToHome.asProxy()
); );
private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 0),
new TeleCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0),
toggleClaw.asProxy(), toggleClaw.asProxy(),
armToHome.asProxy() armToHome.asProxy()
); );
private SequentialCommandGroup placeLow = new SequentialCommandGroup( private SequentialCommandGroup placeLow = new SequentialCommandGroup(
new PivotCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 0),
new TeleCommand(m_robotArm, 0), new TeleCommand(m_robotArm, 0),
toggleClaw.asProxy(), toggleClaw.asProxy(),
armToHome.asProxy() armToHome.asProxy()
); );
/** /**
@@ -174,17 +174,17 @@ public class RobotContainer {
// * Default Commands // * Default Commands
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(), getDeadbandedDriverController().getRight(),
true); true);
}, m_robotSwerveDrive) }, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand")); .withName("SwerveDrive DefaultCommand"));
m_robotArm.setDefaultCommand(new RunCommand(() -> { m_robotArm.setDefaultCommand(new RunCommand(() -> {
m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY()); m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY()); m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
}, m_robotArm) }, m_robotArm)
.withName("Arm DefaultCommand")); .withName("Arm DefaultCommand"));
// * Auto Commands // * Auto Commands
chooser.setDefaultOption("NoAuto", emptyCommand); chooser.setDefaultOption("NoAuto", emptyCommand);
@@ -230,22 +230,22 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
// .onTrue(new InstantCommand(() -> { // .onTrue(new InstantCommand(() -> {
// tap.gearTapped = true; // tap.gearTapped = true;
// tap.gearTime = System.currentTimeMillis(); // tap.gearTime = System.currentTimeMillis();
// })) // }))
// .whileTrue(new RunCommand(() -> { // .whileTrue(new RunCommand(() -> {
// if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) { // if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) {
// m_robotSwerveDrive.setToTurbo(); // m_robotSwerveDrive.setToTurbo();
// tap.gearTapped = false; // tap.gearTapped = false;
// } // }
// })) // }))
// .onFalse(new InstantCommand(() -> { // .onFalse(new InstantCommand(() -> {
// if (tap.gearTapped) // if (tap.gearTapped)
// m_robotSwerveDrive.setToFast(); // m_robotSwerveDrive.setToFast();
// else // else
// m_robotSwerveDrive.setToSlow(); // m_robotSwerveDrive.setToSlow();
// })); // }));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
@@ -306,18 +306,18 @@ public class RobotContainer {
// place high // place high
new POVButton(getDeadbandedOperatorController(), 0) new POVButton(getDeadbandedOperatorController(), 0)
.onTrue(new ConditionalCommand( .onTrue(new ConditionalCommand(
new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true), new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true),
emptyCommand.asProxy(), emptyCommand.asProxy(),
() -> readyForPlacement == true) () -> readyForPlacement == true)
); );
// place mid // place mid
new POVButton(getDeadbandedOperatorController(), 270) new POVButton(getDeadbandedOperatorController(), 270)
.onTrue(new ConditionalCommand( .onTrue(new ConditionalCommand(
new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true), new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true),
emptyCommand.asProxy(), emptyCommand.asProxy(),
() -> readyForPlacement == true) () -> readyForPlacement == true)
); );
// place low // place low
new POVButton(getDeadbandedOperatorController(), 180) new POVButton(getDeadbandedOperatorController(), 180)
src/main/java/frc4388/robot/RobotMap.java
+197 -197
View File
@@ -30,250 +30,250 @@ import frc4388.utility.RobotGyro;
* testing and modularization. * testing and modularization.
*/ */
public class RobotMap { public class RobotMap {
private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
public RobotGyro gyro = new RobotGyro(m_pigeon2); public RobotGyro gyro = new RobotGyro(m_pigeon2);
public SwerveModule leftFront; public SwerveModule leftFront;
public SwerveModule rightFront; public SwerveModule rightFront;
public SwerveModule leftBack; public SwerveModule leftBack;
public SwerveModule rightBack; public SwerveModule rightBack;
public RobotMap() { public RobotMap() {
configureLEDMotorControllers(); configureLEDMotorControllers();
configureDriveMotors(); configureDriveMotors();
configArmMotors(); configArmMotors();
try { try {
setup_motor_tests(); setup_motor_tests();
} catch (IllegalArgumentException | IllegalAccessException e) { } catch (IllegalArgumentException | IllegalAccessException e) {
// TODO Auto-generated catch block // TODO Auto-generated catch block
e.printStackTrace(); e.printStackTrace();
}
} }
}
/* LED Subsystem */ /* LED Subsystem */
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() { void configureLEDMotorControllers() {
} }
// swerve drive subsystem // swerve drive subsystem
public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
void configureDriveMotors() { void configureDriveMotors() {
// config factory default // config factory default
leftFrontWheel.configFactoryDefault(); leftFrontWheel.configFactoryDefault();
leftFrontSteer.configFactoryDefault(); leftFrontSteer.configFactoryDefault();
rightFrontWheel.configFactoryDefault(); rightFrontWheel.configFactoryDefault();
rightFrontSteer.configFactoryDefault(); rightFrontSteer.configFactoryDefault();
leftBackWheel.configFactoryDefault(); leftBackWheel.configFactoryDefault();
leftBackSteer.configFactoryDefault(); leftBackSteer.configFactoryDefault();
rightBackWheel.configFactoryDefault(); rightBackWheel.configFactoryDefault();
rightBackSteer.configFactoryDefault(); rightBackSteer.configFactoryDefault();
// config open loop ramp // config open loop ramp
leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config closed loop ramp // config closed loop ramp
leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config neutral deadband // config neutral deadband
leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// set neutral mode // set neutral mode
leftFrontWheel.setNeutralMode(NeutralMode.Brake); leftFrontWheel.setNeutralMode(NeutralMode.Brake);
rightFrontWheel.setNeutralMode(NeutralMode.Brake); rightFrontWheel.setNeutralMode(NeutralMode.Brake);
leftBackWheel.setNeutralMode(NeutralMode.Brake); leftBackWheel.setNeutralMode(NeutralMode.Brake);
rightBackWheel.setNeutralMode(NeutralMode.Brake); rightBackWheel.setNeutralMode(NeutralMode.Brake);
leftFrontSteer.setNeutralMode(NeutralMode.Brake); leftFrontSteer.setNeutralMode(NeutralMode.Brake);
rightFrontSteer.setNeutralMode(NeutralMode.Brake); rightFrontSteer.setNeutralMode(NeutralMode.Brake);
leftBackSteer.setNeutralMode(NeutralMode.Brake); leftBackSteer.setNeutralMode(NeutralMode.Brake);
rightBackSteer.setNeutralMode(NeutralMode.Brake); rightBackSteer.setNeutralMode(NeutralMode.Brake);
// initialize SwerveModules // initialize SwerveModules
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
}
// arm stuff
public WPI_TalonFX pivot = new WPI_TalonFX(15);
public WPI_TalonFX tele = new WPI_TalonFX(16);
public CANCoder pivotEncoder = new CANCoder(17);
public void configArmMotors() {
// config factory default
pivot.configFactoryDefault();
tele.configFactoryDefault();
// config open loop ramp
pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config closed loop ramp
pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config neutral mode to brake
pivot.setNeutralMode(NeutralMode.Brake);
tele.setNeutralMode(NeutralMode.Brake);
// soft limits
pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
pivot.configForwardSoftLimitEnable(false);
pivot.configReverseSoftLimitEnable(false);
tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
tele.configForwardSoftLimitEnable(false);
tele.configReverseSoftLimitEnable(false);
}
// claw stuff (WHAT IS A SOAP ENGINEER)
PWM leftClaw = new PWM(0);
PWM rightClaw = new PWM(1);
CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
// ============================================
// =============== AUTO TESTING ===============
// ============================================
static class MotorTest {
boolean result;
String name;
enum MotorType {FALCON, NEO}
// why no union :(
MotorType motor_type;
WPI_TalonFX as_falcon;
CANSparkMax as_neo;
}
private static final long TEST_TIME = 500;
public ArrayList<MotorTest> motor_tests = new ArrayList<>();
private int test_count = 0;
private long test_time = 0;
private double motor_position = 0;
public void restart_motor_tests() {
for (MotorTest test : motor_tests) {
test.result = false;
} }
test_time = 0; // arm stuff
test_count = 0; public WPI_TalonFX pivot = new WPI_TalonFX(15);
motor_position = 0; public WPI_TalonFX tele = new WPI_TalonFX(16);
} public CANCoder pivotEncoder = new CANCoder(17);
public boolean run_periodic_tests() { public void configArmMotors() {
if (test_count >= motor_tests.size()) return true; // config factory default
pivot.configFactoryDefault();
tele.configFactoryDefault();
MotorTest current_test = motor_tests.get(test_count); // config open loop ramp
pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
if (test_time == 0) { // config closed loop ramp
test_time = System.currentTimeMillis(); pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
switch (current_test.motor_type) { // config neutral mode to brake
case FALCON: pivot.setNeutralMode(NeutralMode.Brake);
motor_position = current_test.as_falcon.getSelectedSensorPosition(); tele.setNeutralMode(NeutralMode.Brake);
current_test.as_falcon.set(.1);
break;
case NEO: // soft limits
// TODO: destroy robot pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
break; pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT);
} pivot.configForwardSoftLimitEnable(false);
pivot.configReverseSoftLimitEnable(false);
tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
tele.configForwardSoftLimitEnable(false);
tele.configReverseSoftLimitEnable(false);
} }
if (System.currentTimeMillis() - test_time >= TEST_TIME) { // claw stuff (WHAT IS A SOAP ENGINEER)
switch (current_test.motor_type) { PWM leftClaw = new PWM(0);
case FALCON: PWM rightClaw = new PWM(1);
{ CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
double new_pos = current_test.as_falcon.getSelectedSensorPosition();
if (Math.abs(new_pos - motor_position) > .1) {
current_test.result = true;
} else {
System.out.printf("[DISCONNECTED] %s\n", current_test.name);
}
break; // ============================================
} // =============== AUTO TESTING ===============
// ============================================
case NEO: break; static class MotorTest {
} boolean result;
String name;
test_time = 0; enum MotorType {FALCON, NEO}
test_count++;
// why no union :(
MotorType motor_type;
WPI_TalonFX as_falcon;
CANSparkMax as_neo;
} }
return false; private static final long TEST_TIME = 500;
}
private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException { public ArrayList<MotorTest> motor_tests = new ArrayList<>();
Class map_clazz = this.getClass(); private int test_count = 0;
private long test_time = 0;
private double motor_position = 0;
for (Field field : map_clazz.getFields()) { public void restart_motor_tests() {
if (field.getType().equals(WPI_TalonFX.class)) { for (MotorTest test : motor_tests) {
MotorTest test = new MotorTest(); test.result = false;
test.result = false; }
test.name = field.getName();
test.motor_type = MotorTest.MotorType.FALCON; test_time = 0;
test.as_falcon = (WPI_TalonFX) field.get(this); test_count = 0;
} motor_position = 0;
}
public boolean run_periodic_tests() {
if (test_count >= motor_tests.size()) return true;
MotorTest current_test = motor_tests.get(test_count);
if (test_time == 0) {
test_time = System.currentTimeMillis();
switch (current_test.motor_type) {
case FALCON:
motor_position = current_test.as_falcon.getSelectedSensorPosition();
current_test.as_falcon.set(.1);
break;
case NEO:
// TODO: destroy robot
break;
}
}
if (System.currentTimeMillis() - test_time >= TEST_TIME) {
switch (current_test.motor_type) {
case FALCON:
{
double new_pos = current_test.as_falcon.getSelectedSensorPosition();
if (Math.abs(new_pos - motor_position) > .1) {
current_test.result = true;
} else {
System.out.printf("[DISCONNECTED] %s\n", current_test.name);
}
break;
}
case NEO: break;
}
test_time = 0;
test_count++;
}
return false;
}
private void setup_motor_tests() throws IllegalArgumentException, IllegalAccessException {
Class map_clazz = this.getClass();
for (Field field : map_clazz.getFields()) {
if (field.getType().equals(WPI_TalonFX.class)) {
MotorTest test = new MotorTest();
test.result = false;
test.name = field.getName();
test.motor_type = MotorTest.MotorType.FALCON;
test.as_falcon = (WPI_TalonFX) field.get(this);
}
}
} }
}
} }
src/main/java/frc4388/robot/commands/Arm/PivotCommand.java
+18 -18
View File
@@ -9,25 +9,25 @@ import frc4388.robot.commands.PelvicInflammatoryDisease;
import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Arm;
public class PivotCommand extends PelvicInflammatoryDisease { public class PivotCommand extends PelvicInflammatoryDisease {
private final Arm arm; private final Arm arm;
private final double target; private final double target;
/** Creates a new ArmCommand. */ /** Creates a new ArmCommand. */
public PivotCommand(Arm arm, double target) { public PivotCommand(Arm arm, double target) {
super(6, 1.5, 0, 0, 0.015); super(6, 1.5, 0, 0, 0.015);
this.arm = arm; this.arm = arm;
this.target = target; this.target = target;
addRequirements(arm); addRequirements(arm);
} }
@Override @Override
public double getError() { public double getError() {
return (target - arm.getArmRotation()) / 360; return (target - arm.getArmRotation()) / 360;
} }
@Override @Override
public void runWithOutput(double output) { public void runWithOutput(double output) {
SmartDashboard.putNumber("pivot output", output); SmartDashboard.putNumber("pivot output", output);
arm.setRotVel(output); arm.setRotVel(output);
} }
} }
src/main/java/frc4388/robot/commands/Arm/TeleCommand.java
+23 -23
View File
@@ -2,7 +2,7 @@
// Open Source Software; you can modify and/or share it under the terms of // 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. // the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.Arm; package frc4388.robot.commands.Arm;
import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.ArmConstants; import frc4388.robot.Constants.ArmConstants;
@@ -10,30 +10,30 @@ import frc4388.robot.commands.PelvicInflammatoryDisease;
import frc4388.robot.subsystems.Arm; import frc4388.robot.subsystems.Arm;
public class TeleCommand extends CommandBase { public class TeleCommand extends CommandBase {
private final Arm arm; private final Arm arm;
private final double target; private final double target;
private boolean goIn; private boolean goIn;
/** Creates a new ArmCommand. */ /** Creates a new ArmCommand. */
public TeleCommand(Arm arm, double target) { public TeleCommand(Arm arm, double target) {
this.arm = arm; this.arm = arm;
this.target = target; this.target = target;
addRequirements(arm); addRequirements(arm);
} }
@Override @Override
public void initialize() { public void initialize() {
this.goIn = target < arm.getArmLength(); this.goIn = target < arm.getArmLength();
} }
@Override @Override
public void execute() { public void execute() {
arm.setTeleVel(goIn ? 1 : -1); arm.setTeleVel(goIn ? 1 : -1);
} }
@Override @Override
public boolean isFinished() { public boolean isFinished() {
if (goIn) return arm.getArmLength() < target; if (goIn) return arm.getArmLength() < target;
else return arm.getArmLength() > target; else return arm.getArmLength() > target;
} }
} }
src/main/java/frc4388/robot/commands/Autos/AutoBalance.java
+21 -21
View File
@@ -12,31 +12,31 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.RobotGyro; import frc4388.utility.RobotGyro;
public class AutoBalance extends PelvicInflammatoryDisease { public class AutoBalance extends PelvicInflammatoryDisease {
RobotGyro gyro; RobotGyro gyro;
SwerveDrive drive; SwerveDrive drive;
/** Creates a new AutoBalance. */ /** Creates a new AutoBalance. */
public AutoBalance(RobotGyro gyro, SwerveDrive drive) { public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
super(0.6, 0, 0, 0, 0); super(0.6, 0, 0, 0, 0);
this.gyro = gyro; this.gyro = gyro;
this.drive = drive; this.drive = drive;
addRequirements(drive); addRequirements(drive);
} }
@Override @Override
public double getError() { public double getError() {
var pitch = gyro.getRoll(); var pitch = gyro.getRoll();
SmartDashboard.putNumber("pitch", pitch); SmartDashboard.putNumber("pitch", pitch);
return pitch; return pitch;
} }
@Override @Override
public void runWithOutput(double output) { public void runWithOutput(double output) {
double out2 = MathUtil.clamp(output / 40, -.5, .5); double out2 = MathUtil.clamp(output / 40, -.5, .5);
if (Math.abs(getError()) < 3) out2 = 0; if (Math.abs(getError()) < 3) out2 = 0;
drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false); drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
} }
} }
src/main/java/frc4388/robot/commands/BooleanCommand.java
+42 -42
View File
@@ -11,57 +11,57 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
public class BooleanCommand extends CommandBase { public class BooleanCommand extends CommandBase {
private Supplier<Command> onTrue; private Supplier<Command> onTrue;
private Supplier<Command> onFalse; private Supplier<Command> onFalse;
private Supplier<Boolean> condition; private Supplier<Boolean> condition;
private Supplier<Command> selected; private Supplier<Command> selected;
/** Creates a new BooleanCommand. */ /** Creates a new BooleanCommand. */
public BooleanCommand(Supplier<Command> onTrue, Supplier<Command> onFalse, Supplier<Boolean> condition) { public BooleanCommand(Supplier<Command> onTrue, Supplier<Command> onFalse, Supplier<Boolean> condition) {
this.onTrue = onTrue; this.onTrue = onTrue;
this.onFalse = onFalse; this.onFalse = onFalse;
this.condition = condition; this.condition = condition;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
if (condition.get()) {
selected = onTrue;
} else {
selected = onFalse;
} }
if (selected.get() != null) {
selected.get().initialize();
}
}
// Called every time the scheduler runs while the command is scheduled. // Called when the command is initially scheduled.
@Override @Override
public void execute() { public void initialize() {
if (selected.get() != null) { if (condition.get()) {
selected.get().execute(); selected = onTrue;
} else {
selected = onFalse;
}
if (selected.get() != null) {
selected.get().initialize();
}
} }
}
// Called once the command ends or is interrupted. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void end(boolean interrupted) { public void execute() {
if (selected.get() != null) { if (selected.get() != null) {
selected.get().end(interrupted); selected.get().execute();
}
} }
}
// Returns true when the command should end. // Called once the command ends or is interrupted.
@Override @Override
public boolean isFinished() { public void end(boolean interrupted) {
if (selected.get() != null) { if (selected.get() != null) {
return selected.get().isFinished(); selected.get().end(interrupted);
} else { }
return true; }
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (selected.get() != null) {
return selected.get().isFinished();
} else {
return true;
}
} }
}
} }
src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
+39 -39
View File
@@ -8,53 +8,53 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.utility.Gains; import frc4388.utility.Gains;
public abstract class PelvicInflammatoryDisease extends CommandBase { public abstract class PelvicInflammatoryDisease extends CommandBase {
protected Gains gains; protected Gains gains;
private double output = 0; private double output = 0;
private double tolerance = 0; private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */ /** Creates a new PelvicInflammatoryDisease. */
public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) { public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0); gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance; this.tolerance = tolerance;
} }
public PelvicInflammatoryDisease(Gains gains, double tolerance) { public PelvicInflammatoryDisease(Gains gains, double tolerance) {
this.gains = gains; this.gains = gains;
this.tolerance = tolerance; this.tolerance = tolerance;
} }
/** produces the error from the setpoint */ /** produces the error from the setpoint */
public abstract double getError(); public abstract double getError();
/** figure it out bitch */ /** figure it out bitch */
public abstract void runWithOutput(double output); public abstract void runWithOutput(double output);
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @Override
public final void initialize() { public final void initialize() {
output = 0; output = 0;
} }
private double prevError, cumError = 0; private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public final void execute() { public final void execute() {
double error = getError(); double error = getError();
cumError += error * .02; // 20 ms cumError += error * .02; // 20 ms
double delta = error - prevError; double delta = error - prevError;
output = error * gains.kP; output = error * gains.kP;
output += cumError * gains.kI; output += cumError * gains.kI;
output += delta * gains.kD; output += delta * gains.kD;
output += gains.kF; output += gains.kF;
runWithOutput(output); runWithOutput(output);
} }
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public final boolean isFinished() { public final boolean isFinished() {
return Math.abs(getError()) < tolerance; return Math.abs(getError()) < tolerance;
} }
} }
src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java
+21 -21
View File
@@ -11,32 +11,32 @@ import frc4388.robot.subsystems.SwerveDrive;
public class AprilRotAlign extends PelvicInflammatoryDisease { public class AprilRotAlign extends PelvicInflammatoryDisease {
SwerveDrive drive; SwerveDrive drive;
Limelight lime; Limelight lime;
/** Creates a new AprilRotAlign. */ /** Creates a new AprilRotAlign. */
public AprilRotAlign(SwerveDrive drive, Limelight lime) { public AprilRotAlign(SwerveDrive drive, Limelight lime) {
super(0.1, 0.2, 0.0, 0.0, 0.0); super(0.1, 0.2, 0.0, 0.0, 0.0);
this.drive = drive; this.drive = drive;
this.lime = lime; this.lime = lime;
addRequirements(drive, lime); addRequirements(drive, lime);
} }
@Override @Override
public double getError() { public double getError() {
double err = 0.0; double err = 0.0;
try { try {
err = lime.getAprilSkew(); err = lime.getAprilSkew();
} catch (NullPointerException ex) {} } catch (NullPointerException ex) {}
return err; return err;
} }
@Override @Override
public void runWithOutput(double output) { public void runWithOutput(double output) {
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true); drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true);
} }
} }
src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java
+22 -22
View File
@@ -13,32 +13,32 @@ import frc4388.robot.subsystems.SwerveDrive;
public class DriveToLimeDistance extends PelvicInflammatoryDisease { public class DriveToLimeDistance extends PelvicInflammatoryDisease {
SwerveDrive drive; SwerveDrive drive;
Limelight lime; Limelight lime;
double targetDistance; double targetDistance;
DoubleSupplier ds; DoubleSupplier ds;
/** Creates a new DriveToLimeDistance. */ /** Creates a new DriveToLimeDistance. */
public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) { public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) {
super(0.5, 0.0, 0.0, 0.0, 1); super(0.5, 0.0, 0.0, 0.0, 1);
this.drive = drive; this.drive = drive;
this.lime = lime; this.lime = lime;
this.targetDistance = targetDistance; this.targetDistance = targetDistance;
this.ds = ds; this.ds = ds;
addRequirements(drive, lime); addRequirements(drive, lime);
} }
@Override @Override
public double getError() { public double getError() {
return targetDistance - ds.getAsDouble(); return targetDistance - ds.getAsDouble();
} }
@Override @Override
public void runWithOutput(double output) { public void runWithOutput(double output) {
System.out.println(output / Math.abs(getError())); System.out.println(output / Math.abs(getError()));
drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true); drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true);
} }
} }
src/main/java/frc4388/robot/commands/Placement/LimeAlign.java
+23 -23
View File
@@ -14,35 +14,35 @@ import frc4388.robot.subsystems.SwerveDrive;
public class LimeAlign extends PelvicInflammatoryDisease { public class LimeAlign extends PelvicInflammatoryDisease {
SwerveDrive drive; SwerveDrive drive;
Limelight lime; Limelight lime;
DoubleSupplier ds; DoubleSupplier ds;
public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) { public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) {
super(0.4, 0.4, 0.0, 0.0, tolerance); super(0.4, 0.4, 0.0, 0.0, tolerance);
this.drive = drive; this.drive = drive;
this.lime = lime; this.lime = lime;
this.ds = ds; this.ds = ds;
addRequirements(drive, lime); addRequirements(drive, lime);
} }
@Override @Override
public double getError() { public double getError() {
double err = 0.0; double err = 0.0;
try { try {
System.out.println(ds.getAsDouble()); System.out.println(ds.getAsDouble());
err = ds.getAsDouble() / (VisionConstants.H_FOV / 2); err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
} catch (NullPointerException ex) {} } catch (NullPointerException ex) {}
return err; return err;
} }
@Override @Override
public void runWithOutput(double output) { public void runWithOutput(double output) {
drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true); drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
} }
} }
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
+111 -111
View File
@@ -14,128 +14,128 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput; import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickPlayback extends CommandBase { public class JoystickPlayback extends CommandBase {
private final SwerveDrive swerve; private final SwerveDrive swerve;
private String filename; private String filename;
private int mult = 1; private int mult = 1;
private Scanner input; private Scanner input;
private final ArrayList<TimedOutput> outputs = new ArrayList<>(); private final ArrayList<TimedOutput> outputs = new ArrayList<>();
private int counter = 0; private int counter = 0;
private long startTime = 0; private long startTime = 0;
private long playbackTime = 0; private long playbackTime = 0;
private int lastIndex; private int lastIndex;
private boolean m_finished = false; // ! find a better way private boolean m_finished = false; // ! find a better way
/** Creates a new JoystickPlayback. */ /** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { public JoystickPlayback(SwerveDrive swerve, String filename, int mult) {
this.swerve = swerve; this.swerve = swerve;
this.filename = filename; this.filename = filename;
this.mult = mult; this.mult = mult;
addRequirements(this.swerve); addRequirements(this.swerve);
}
/** Creates a new JoystickPlayback. */
public JoystickPlayback(SwerveDrive swerve, String filename) {
this(swerve, filename, 1);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
outputs.clear();
m_finished = false;
startTime = System.currentTimeMillis();
playbackTime = 0;
lastIndex = 0;
try {
input = new Scanner(new File("/home/lvuser/autos/" + filename));
String line = "";
while (input.hasNextLine()) {
line = input.nextLine();
if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
continue;
}
String[] values = line.split(",");
System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
var out = new TimedOutput();
out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]);
out.timedOffset = Long.parseLong(values[4]);
outputs.add(out);
}
input.close();
} catch (FileNotFoundException e) {
e.printStackTrace();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
startTime = System.currentTimeMillis();
playbackTime = 0;
} else {
playbackTime = System.currentTimeMillis() - startTime;
} }
// skip to reasonable time frame /** Creates a new JoystickPlayback. */
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing public JoystickPlayback(SwerveDrive swerve, String filename) {
{ this(swerve, filename, 1);
int i = lastIndex == 0 ? 1 : lastIndex;
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
i++;
}
if (i >= outputs.size()) {
m_finished = true; // ! kind of a hack
return;
}
lastIndex = i;
} }
TimedOutput lastOut = outputs.get(lastIndex - 1); // Called when the command is initially scheduled.
TimedOutput out = outputs.get(lastIndex); @Override
public void initialize() {
outputs.clear();
m_finished = false;
double deltaTime = out.timedOffset - lastOut.timedOffset; startTime = System.currentTimeMillis();
double playbackDelta = playbackTime - lastOut.timedOffset; playbackTime = 0;
lastIndex = 0;
try {
input = new Scanner(new File("/home/lvuser/autos/" + filename));
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); String line = "";
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); while (input.hasNextLine()) {
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); line = input.nextLine();
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), if (line.isEmpty() || line.isBlank() || line.equals("\n")) {
// new Translation2d(out.rightX, out.rightY), continue;
// true); }
this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), String[] values = line.split(",");
new Translation2d(lerpRX, lerpRY), System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]);
true);
counter++; var out = new TimedOutput();
} out.leftX = Double.parseDouble(values[0]) * mult;
out.leftY = Double.parseDouble(values[1]);
out.rightX = Double.parseDouble(values[2]);
out.rightY = Double.parseDouble(values[3]);
// Called once the command ends or is interrupted. out.timedOffset = Long.parseLong(values[4]);
@Override
public void end(boolean interrupted) {
input.close();
swerve.stopModules();
}
// Returns true when the command should end. outputs.add(out);
@Override }
public boolean isFinished() {
return m_finished; input.close();
} } catch (FileNotFoundException e) {
e.printStackTrace();
}
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
startTime = System.currentTimeMillis();
playbackTime = 0;
} else {
playbackTime = System.currentTimeMillis() - startTime;
}
// skip to reasonable time frame
// too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing
{
int i = lastIndex == 0 ? 1 : lastIndex;
while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) {
i++;
}
if (i >= outputs.size()) {
m_finished = true; // ! kind of a hack
return;
}
lastIndex = i;
}
TimedOutput lastOut = outputs.get(lastIndex - 1);
TimedOutput out = outputs.get(lastIndex);
double deltaTime = out.timedOffset - lastOut.timedOffset;
double playbackDelta = playbackTime - lastOut.timedOffset;
double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime);
double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime);
double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime);
double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime);
// this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY),
// new Translation2d(out.rightX, out.rightY),
// true);
this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY),
new Translation2d(lerpRX, lerpRY),
true);
counter++;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
input.close();
swerve.stopModules();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return m_finished;
}
} }
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+25 -23
View File
@@ -14,29 +14,23 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput; import frc4388.utility.UtilityStructs.TimedOutput;
import frc4388.utility.controller.XboxController;
public class JoystickRecorder extends CommandBase { public class JoystickRecorder extends CommandBase {
public final SwerveDrive swerve; public final SwerveDrive swerve;
public final XboxController driveXbox;
public final XboxController operatorXbox;
public final Supplier<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
private String filename; private String filename;
public final ArrayList<TimedOutput> outputs = new ArrayList<>(); public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1; private long startTime = -1;
/** Creates a new JoystickRecorder. */ /** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY, public JoystickRecorder(SwerveDrive swerve, XboxController driveXbox,
Supplier<Double> rightX, Supplier<Double> rightY, XboxController operatorXbox, String filename) {
String filename)
{
this.swerve = swerve; this.swerve = swerve;
this.leftX = leftX; this.driveXbox = driveXbox;
this.leftY = leftY; this.operatorXbox = operatorXbox;
this.rightX = rightX;
this.rightY = rightY;
this.filename = filename; this.filename = filename;
addRequirements(this.swerve); addRequirements(this.swerve);
@@ -56,16 +50,22 @@ public class JoystickRecorder extends CommandBase {
@Override @Override
public void execute() { public void execute() {
var inputs = new TimedOutput(); var inputs = new TimedOutput();
inputs.leftX = leftX.get(); inputs.driverLeftX = driveXbox.getLeftXAxis();
inputs.leftY = leftY.get(); inputs.driverLeftY = driveXbox.getLeftYAxis();
inputs.rightX = rightX.get(); inputs.driverRightX = driveXbox.getRightXAxis();
inputs.rightY = rightY.get(); inputs.driverRightY = driveXbox.getRightYAxis();
inputs.operatorLeftX = driveXbox.getLeftXAxis();
inputs.operatorLeftY = driveXbox.getLeftYAxis();
inputs.operatorRightX = driveXbox.getRightXAxis();
inputs.operatorRightY = driveXbox.getRightYAxis();
inputs.timedOffset = System.currentTimeMillis() - startTime; inputs.timedOffset = System.currentTimeMillis() - startTime;
outputs.add(inputs); outputs.add(inputs);
swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY), swerve.driveWithInput(new Translation2d(inputs.driverLeftX, inputs.driverLeftX),
new Translation2d(inputs.rightX, inputs.rightY), new Translation2d(inputs.driverRightX, inputs.driverRightY),
true); true);
System.out.println("RECORDING"); System.out.println("RECORDING");
@@ -78,8 +78,10 @@ public class JoystickRecorder extends CommandBase {
try (PrintWriter writer = new PrintWriter(output)) { try (PrintWriter writer = new PrintWriter(output)) {
for (var input : outputs) { for (var input : outputs) {
writer.println( input.leftX + "," + input.leftY + "," + writer.println( input.driverLeftX + "," + input.driverLeftX + "," +
input.rightX + "," + input.rightY + "," + input.driverRightX + "," + input.driverRightY + "," +
input.operatorLeftX + "," + input.operatorLeftX + "," +
input.operatorRightX + "," + input.operatorRightY + "," +
input.timedOffset); input.timedOffset);
} }
src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java
+17 -17
View File
@@ -10,26 +10,26 @@ import frc4388.robot.subsystems.SwerveDrive;
public class RotateToAngle extends PelvicInflammatoryDisease { public class RotateToAngle extends PelvicInflammatoryDisease {
SwerveDrive drive; SwerveDrive drive;
double targetAngle; double targetAngle;
/** Creates a new RotateToAngle. */ /** Creates a new RotateToAngle. */
public RotateToAngle(SwerveDrive drive, double targetAngle) { public RotateToAngle(SwerveDrive drive, double targetAngle) {
super(0.3, 0.0, 0.0, 0.0, 1); super(0.3, 0.0, 0.0, 0.0, 1);
this.drive = drive; this.drive = drive;
this.targetAngle = targetAngle; this.targetAngle = targetAngle;
addRequirements(drive); addRequirements(drive);
} }
@Override @Override
public double getError() { public double getError() {
return targetAngle - drive.getGyroAngle(); return targetAngle - drive.getGyroAngle();
} }
@Override @Override
public void runWithOutput(double output) { public void runWithOutput(double output) {
drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);
} }
} }
src/main/java/frc4388/robot/subsystems/Arm.java
+3 -3
View File
@@ -65,14 +65,14 @@ public class Arm extends SubsystemBase {
if (rot > 1 || rot < 0) return; if (rot > 1 || rot < 0) return;
// Move arm code // Move arm code
m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) + m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) +
ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
} }
public void armSetLength(double len) { public void armSetLength(double len) {
if (len > 1 || len < 0) return; if (len > 1 || len < 0) return;
// Move arm code // Move arm code
m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) + m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) +
ArmConstants.TELE_FORWARD_SOFT_LIMIT); ArmConstants.TELE_FORWARD_SOFT_LIMIT);
if (m_tele.isRevLimitSwitchClosed() == 1) { if (m_tele.isRevLimitSwitchClosed() == 1) {
m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT); m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
@@ -92,7 +92,7 @@ public class Arm extends SubsystemBase {
public void runPivotTele(double pivot, double tele) { public void runPivotTele(double pivot, double tele) {
double abs_pivot = Math.toRadians(getArmRotation() - 135); double abs_pivot = Math.toRadians(getArmRotation() - 135);
double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) / double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
(ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) { if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
setRotVel(pivot); setRotVel(pivot);
src/main/java/frc4388/robot/subsystems/Claw.java
+34 -34
View File
@@ -10,52 +10,52 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Claw extends SubsystemBase { public class Claw extends SubsystemBase {
private final PWM m_leftMotor; private final PWM m_leftMotor;
private final PWM m_rightMotor; private final PWM m_rightMotor;
private final CANSparkMax m_spinnyspin; private final CANSparkMax m_spinnyspin;
private boolean m_open = false; private boolean m_open = false;
// Opens claw // Opens claw
public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) { public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
m_leftMotor = leftMotor; m_leftMotor = leftMotor;
m_rightMotor = rightMotor; m_rightMotor = rightMotor;
m_spinnyspin = spinnyspin; m_spinnyspin = spinnyspin;
setClaw(true); setClaw(true);
} }
public void setClaw(boolean open) { public void setClaw(boolean open) {
// Open claw // Open claw
m_open = open; m_open = open;
m_leftMotor.setRaw(m_open ? 1000 : 2000); m_leftMotor.setRaw(m_open ? 1000 : 2000);
m_rightMotor.setRaw(m_open ? 2000 : 1000); m_rightMotor.setRaw(m_open ? 2000 : 1000);
if (!m_open) { if (!m_open) {
m_spinnyspin.set(-0.2); m_spinnyspin.set(-0.2);
new Timer().schedule(new TimerTask() { new Timer().schedule(new TimerTask() {
@Override @Override
public void run() { public void run() {
nospinnyspin(); nospinnyspin();
} }
}, 750); }, 750);
} }
} }
public void toggle() { public void toggle() {
setClaw(!m_open); setClaw(!m_open);
} }
public boolean isClawOpen() { public boolean isClawOpen() {
return m_open; return m_open;
} }
public void yesspinnyspin() { public void yesspinnyspin() {
m_spinnyspin.set(0.2); m_spinnyspin.set(0.2);
} }
public void nospinnyspin() { public void nospinnyspin() {
m_spinnyspin.set(0); m_spinnyspin.set(0);
} }
} }
src/main/java/frc4388/robot/subsystems/LED.java
+35 -35
View File
@@ -20,44 +20,44 @@ import frc4388.utility.LEDPatterns;
*/ */
public class LED extends SubsystemBase { public class LED extends SubsystemBase {
private LEDPatterns m_currentPattern; private LEDPatterns m_currentPattern;
private Spark m_LEDController; private Spark m_LEDController;
/** /**
* Add your docs here. * Add your docs here.
*/ */
public LED(Spark LEDController){ public LED(Spark LEDController){
m_LEDController = LEDController; m_LEDController = LEDController;
setPattern(LEDConstants.DEFAULT_PATTERN); setPattern(LEDConstants.DEFAULT_PATTERN);
updateLED(); updateLED();
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
} }
@Override @Override
public void periodic(){ public void periodic(){
SmartDashboard.putNumber("LED", m_currentPattern.getValue()); SmartDashboard.putNumber("LED", m_currentPattern.getValue());
} }
/** /**
* Add your docs here. * Add your docs here.
*/ */
public void updateLED(){ public void updateLED(){
m_LEDController.set(m_currentPattern.getValue()); m_LEDController.set(m_currentPattern.getValue());
} }
/** /**
* Add your docs here. * Add your docs here.
*/ */
public void setPattern(LEDPatterns pattern){ public void setPattern(LEDPatterns pattern){
m_currentPattern = pattern; m_currentPattern = pattern;
m_LEDController.set(m_currentPattern.getValue()); m_LEDController.set(m_currentPattern.getValue());
} }
/** /**
* Add your docs here. * Add your docs here.
* @return * @return
*/ */
public LEDPatterns getPattern() { public LEDPatterns getPattern() {
return m_currentPattern; return m_currentPattern;
} }
} }
src/main/java/frc4388/robot/subsystems/Limelight.java
+121 -121
View File
@@ -29,137 +29,137 @@ import frc4388.robot.Constants.VisionConstants;
public class Limelight extends SubsystemBase { public class Limelight extends SubsystemBase {
private PhotonCamera cam; private PhotonCamera cam;
private PhotonPoseEstimator photonPoseEstimator; private PhotonPoseEstimator photonPoseEstimator;
private boolean lightOn; private boolean lightOn;
/** Creates a new Limelight. */ /** Creates a new Limelight. */
public Limelight() { public Limelight() {
cam = new PhotonCamera(VisionConstants.NAME); cam = new PhotonCamera(VisionConstants.NAME);
cam.setDriverMode(false); cam.setDriverMode(false);
}
public void setLEDs(boolean on) {
lightOn = on;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void toggleLEDs() {
lightOn = !lightOn;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
}
public void setDriverMode(boolean driverMode) {
cam.setDriverMode(driverMode);
}
public void setToLimePipeline() {
cam.setPipelineIndex(1);
setLEDs(true);
}
public void setToAprilPipeline() {
cam.setPipelineIndex(0);
setLEDs(false);
}
public PhotonTrackedTarget getAprilPoint() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
return result.getBestTarget();
}
private List<TargetCorner> getAprilCorners() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
return result.getBestTarget().getDetectedCorners();
}
public double getAprilSkew() {
List<TargetCorner> corners = getAprilCorners();
ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
if (bottomSide == null) return 0;
TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
return bottomLeft.y - bottomRight.y;
}
private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
if (box == null) return null;
ArrayList<TargetCorner> bottomSide = new ArrayList<>();
TargetCorner l1 = new TargetCorner(-1, -1);
TargetCorner l2 = new TargetCorner(-1, -1);
for (TargetCorner c : box) {
if (c.y > l1.y) l1 = c;
} }
for (TargetCorner c : box) { public void setLEDs(boolean on) {
if (c.y == l1.y) continue; lightOn = on;
if (c.y > l2.y) l2 = c; cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
} }
bottomSide.add(l1); public void toggleLEDs() {
bottomSide.add(l2); lightOn = !lightOn;
cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
return bottomSide;
}
public double getDistanceToApril() {
PhotonTrackedTarget aprilPoint = getAprilPoint();
if (aprilPoint == null) return -1;
double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + aprilPoint.getPitch();
double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
return distanceToApril;
}
public PhotonTrackedTarget getLowestTape() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
PhotonTrackedTarget lowest = points.get(0);
for (PhotonTrackedTarget point : points) {
if (point.getPitch() < lowest.getPitch()) {
lowest = point;
}
} }
return lowest; public void setDriverMode(boolean driverMode) {
} cam.setDriverMode(driverMode);
}
public double getDistanceToTape() { public void setToLimePipeline() {
PhotonTrackedTarget tapePoint = getLowestTape(); cam.setPipelineIndex(1);
if (tapePoint == null) return -1; setLEDs(true);
}
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT; public void setToAprilPipeline() {
double theta = 35.0 + tapePoint.getPitch(); cam.setPipelineIndex(0);
setLEDs(false);
}
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta)); public PhotonTrackedTarget getAprilPoint() {
return distanceToTape; if (!cam.isConnected()) return null;
}
@Override PhotonPipelineResult result = cam.getLatestResult();
public void periodic() {}
if (!result.hasTargets()) return null;
return result.getBestTarget();
}
private List<TargetCorner> getAprilCorners() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
return result.getBestTarget().getDetectedCorners();
}
public double getAprilSkew() {
List<TargetCorner> corners = getAprilCorners();
ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
if (bottomSide == null) return 0;
TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
return bottomLeft.y - bottomRight.y;
}
private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
if (box == null) return null;
ArrayList<TargetCorner> bottomSide = new ArrayList<>();
TargetCorner l1 = new TargetCorner(-1, -1);
TargetCorner l2 = new TargetCorner(-1, -1);
for (TargetCorner c : box) {
if (c.y > l1.y) l1 = c;
}
for (TargetCorner c : box) {
if (c.y == l1.y) continue;
if (c.y > l2.y) l2 = c;
}
bottomSide.add(l1);
bottomSide.add(l2);
return bottomSide;
}
public double getDistanceToApril() {
PhotonTrackedTarget aprilPoint = getAprilPoint();
if (aprilPoint == null) return -1;
double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + aprilPoint.getPitch();
double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
return distanceToApril;
}
public PhotonTrackedTarget getLowestTape() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
PhotonTrackedTarget lowest = points.get(0);
for (PhotonTrackedTarget point : points) {
if (point.getPitch() < lowest.getPitch()) {
lowest = point;
}
}
return lowest;
}
public double getDistanceToTape() {
PhotonTrackedTarget tapePoint = getLowestTape();
if (tapePoint == null) return -1;
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + tapePoint.getPitch();
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
return distanceToTape;
}
@Override
public void periodic() {}
} }
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+151 -151
View File
@@ -16,180 +16,180 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class SwerveDrive extends SubsystemBase { public class SwerveDrive extends SubsystemBase {
private SwerveModule leftFront; private SwerveModule leftFront;
private SwerveModule rightFront; private SwerveModule rightFront;
private SwerveModule leftBack; private SwerveModule leftBack;
private SwerveModule rightBack; private SwerveModule rightBack;
private SwerveModule[] modules; private SwerveModule[] modules;
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
private RobotGyro gyro; private RobotGyro gyro;
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public double rotTarget = 0.0; public double rotTarget = 0.0;
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */ /** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
this.leftFront = leftFront; this.leftFront = leftFront;
this.rightFront = rightFront; this.rightFront = rightFront;
this.leftBack = leftBack; this.leftBack = leftBack;
this.rightBack = rightBack; this.rightBack = rightBack;
this.gyro = gyro; this.gyro = gyro;
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
boolean stopped = false;
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0;
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
}
}
public boolean rotateToTarget(double angle) {
double currentAngle = getGyroAngle();
double error = angle - currentAngle;
driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
if (Math.abs(angle - getGyroAngle()) < 5.0) {
return true;
} }
return false; boolean stopped = false;
} public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
public double getGyroAngle() { double rot = 0;
return gyro.getAngle();
}
public void resetGyro() { if (rightStick.getNorm() > 0.05) {
gyro.reset(); rotTarget = gyro.getAngle();
rotTarget = 0.0; rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
} SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
stopModules();
stopped = true;
}
public void stopModules() { SmartDashboard.putBoolean("drift correction", true);
for (SwerveModule module : this.modules) { rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
module.stop();
}
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
// Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
} }
}
public SwerveDriveKinematics getKinematics() { /**
return this.kinematics; * Set each module of the swerve drive to the corresponding desired state.
} * @param desiredStates Array of module states to set.
*/
@Override public void setModuleStates(SwerveModuleState[] desiredStates) {
public void periodic() { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
// This method will be called once per scheduler run\ for (int i = 0; i < desiredStates.length; i++) {
SmartDashboard.putNumber("Gyro", getGyroAngle()); SwerveModule module = modules[i];
} SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
public void shiftDown() { }
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} }
}
public void setToSlow() { public boolean rotateToTarget(double angle) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; double currentAngle = getGyroAngle();
System.out.println("SLOW"); double error = angle - currentAngle;
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
}
public void setToFast() { driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
}
public void setToTurbo() { if (Math.abs(angle - getGyroAngle()) < 5.0) {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; return true;
System.out.println("TURBO"); }
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
}
public void shiftUp() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
} else {
return false;
} }
}
public void toggleGear(double angle) { public double getGyroAngle() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { return gyro.getAngle();
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; }
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
} else { public void resetGyro() {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; gyro.reset();
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; rotTarget = 0.0;
}
public void stopModules() {
for (SwerveModule module : this.modules) {
module.stop();
}
}
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
@Override
public void periodic() {
// This method will be called once per scheduler run\
SmartDashboard.putNumber("Gyro", getGyroAngle());
}
public void shiftDown() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
} else {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
}
}
public void setToSlow() {
this.speedAdjust = SwerveDriveConstants.SLOW_SPEED;
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
System.out.println("SLOW");
}
public void setToFast() {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
System.out.println("FAST");
}
public void setToTurbo() {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
System.out.println("TURBO");
}
public void shiftUp() {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.FAST_SPEED;
} else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) {
this.speedAdjust = SwerveDriveConstants.TURBO_SPEED;
} else {
}
}
public void toggleGear(double angle) {
if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
}
} }
}
} }
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+3 -3
View File
@@ -109,9 +109,9 @@ public class SwerveModule extends SubsystemBase {
*/ */
public SwerveModuleState getState() { public SwerveModuleState getState() {
return new SwerveModuleState( return new SwerveModuleState(
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
getAngle() getAngle()
); );
} }
/** /**
src/main/java/frc4388/utility/LEDPatterns.java
+18 -18
View File
@@ -4,8 +4,8 @@ package frc4388.utility;
* Add your docs here. * Add your docs here.
*/ */
public enum LEDPatterns { public enum LEDPatterns {
/* PALLETTE PATTERNS */ /* PALLETTE PATTERNS */
RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f),
RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f), RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f),
PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f), PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f),
PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f), PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f),
@@ -15,31 +15,31 @@ public enum LEDPatterns {
BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f), BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f),
GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f), GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f),
/* COLOR 1 PATTERNS */ /* COLOR 1 PATTERNS */
C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f),
C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f), C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f),
/* COLOR 2 PATTERNS */ /* COLOR 2 PATTERNS */
C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f),
C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f), C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f),
/* COLOR 1 AND 2 PATTERNS */ /* COLOR 1 AND 2 PATTERNS */
C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f),
C1C2_WAVES(0.53f), C1C2_SINELON(0.55f), C1C2_WAVES(0.53f), C1C2_SINELON(0.55f),
/* SOLID COLORS */ /* SOLID COLORS */
SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f),
SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f), SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f),
SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f), SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f),
SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f), SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f),
SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f); SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f);
/* GETTERS/SETTERS */ /* GETTERS/SETTERS */
private final float id; private final float id;
LEDPatterns(float id) { LEDPatterns(float id) {
this.id = id; this.id = id;
} }
public float getValue() { public float getValue() {
return id; return id;
} }
} }
src/main/java/frc4388/utility/RobotGyro.java
+7 -7
View File
@@ -105,12 +105,12 @@ public class RobotGyro implements Gyro {
} }
/** /**
* Get Yaw, Pitch, and Roll data. * Get Yaw, Pitch, and Roll data.
* *
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees. * Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees. * Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees. * Roll is within [-90,+90] degrees.
*/ */
private double[] getPigeonAngles() { private double[] getPigeonAngles() {
double[] ypr = new double[3]; double[] ypr = new double[3];
@@ -129,7 +129,7 @@ public class RobotGyro implements Gyro {
} }
public double getYaw() { public double getYaw() {
return this.getAngle(); return this.getAngle();
} }
/** /**
src/main/java/frc4388/utility/RobotUnits.java
+10 -10
View File
@@ -8,20 +8,20 @@ package frc4388.utility;
* @author Aarav Shah */ * @author Aarav Shah */
public class RobotUnits { public class RobotUnits {
// constants // constants
// angle conversions // angle conversions
public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;} public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;} public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
// falcon conversions // falcon conversions
public static double falconTicksToRotations(final double ticks) {return ticks / 2048;} public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;} public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
// distance conversions // distance conversions
public static double metersToFeet(final double meters) {return meters * 3.28084;} public static double metersToFeet(final double meters) {return meters * 3.28084;}
public static double feetToMeters(final double feet) {return feet / 3.28084;} public static double feetToMeters(final double feet) {return feet / 3.28084;}
} }
src/main/java/frc4388/utility/UtilityStructs.java
+9 -4
View File
@@ -2,10 +2,15 @@ package frc4388.utility;
public class UtilityStructs { public class UtilityStructs {
public static class TimedOutput { public static class TimedOutput {
public double leftX = 0.0; public double driverLeftX = 0.0;
public double leftY = 0.0; public double driverLeftY = 0.0;
public double rightX = 0.0; public double driverRightX = 0.0;
public double rightY = 0.0; public double driverRightY = 0.0;
public double operatorLeftX = 0.0;
public double operatorLeftY = 0.0;
public double operatorRightX = 0.0;
public double operatorRightY = 0.0;
public long timedOffset = 0; public long timedOffset = 0;
} }
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+13 -13
View File
@@ -6,22 +6,22 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController;
public class DeadbandedXboxController extends XboxController { public class DeadbandedXboxController extends XboxController {
public DeadbandedXboxController(int port) { super(port); } public DeadbandedXboxController(int port) { super(port); }
@Override public double getLeftX() { return getLeft().getX(); } @Override public double getLeftX() { return getLeft().getX(); }
@Override public double getLeftY() { return getLeft().getY(); } @Override public double getLeftY() { return getLeft().getY(); }
@Override public double getRightX() { return getRight().getX(); } @Override public double getRightX() { return getRight().getX(); }
@Override public double getRightY() { return getRight().getY(); } @Override public double getRightY() { return getRight().getY(); }
public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); } public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
public static Translation2d skewToDeadzonedCircle(double x, double y) { public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y); Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm(); double magnitude = translation2d.getNorm();
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d; return translation2d;
} }
} }
src/main/java/frc4388/utility/controller/IHandController.java
+7 -7
View File
@@ -5,17 +5,17 @@ package frc4388.utility.controller;
*/ */
public interface IHandController { public interface IHandController {
public double getLeftXAxis(); public double getLeftXAxis();
public double getLeftYAxis(); public double getLeftYAxis();
public double getRightXAxis(); public double getRightXAxis();
public double getRightYAxis(); public double getRightYAxis();
public double getLeftTriggerAxis(); public double getLeftTriggerAxis();
public double getRightTriggerAxis(); public double getRightTriggerAxis();
public int getDpadAngle(); public int getDpadAngle();
} }
src/main/java/frc4388/utility/controller/XboxController.java
+160 -160
View File
@@ -9,210 +9,210 @@ import edu.wpi.first.wpilibj.Joystick;
*/ */
public class XboxController implements IHandController public class XboxController implements IHandController
{ {
public static final int LEFT_X_AXIS = 0; public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1; public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2; public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3; public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4; public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5; public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6; public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6; public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int A_BUTTON = 1; public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2; public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3; public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4; public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5; public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6; public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7; public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8; public static final int START_BUTTON = 8;
public static final int LEFT_JOYSTICK_BUTTON = 9; public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10; public static final int RIGHT_JOYSTICK_BUTTON = 10;
private static final double LEFT_DPAD_TOLERANCE = -0.9; private static final double LEFT_DPAD_TOLERANCE = -0.9;
private static final double RIGHT_DPAD_TOLERANCE = 0.9; private static final double RIGHT_DPAD_TOLERANCE = 0.9;
private static final double BOTTOM_DPAD_TOLERANCE = -0.9; private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
private static final double TOP_DPAD_TOLERANCE = 0.9; private static final double TOP_DPAD_TOLERANCE = 0.9;
private static final double LEFT_TRIGGER_TOLERANCE = 0.5; private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double DEADZONE = 0.1; private static final double DEADZONE = 0.1;
private Joystick m_stick; private Joystick m_stick;
/** /**
* Add your docs here. * Add your docs here.
*/ */
public XboxController(int portNumber){ public XboxController(int portNumber){
m_stick = new Joystick(portNumber); m_stick = new Joystick(portNumber);
} }
/** /**
* Add your docs here. * Add your docs here.
*/ */
public Joystick getJoyStick() { public Joystick getJoyStick() {
return m_stick; return m_stick;
}
/**
* Checks if the input falls within the deadzone.
* @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
private boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE);
}
/**
* Updates an input to have a deadzone around the 0 position
* @param input from an axis on the controller
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
} }
}
/** public boolean getAButton(){
* Checks if the input falls within the deadzone. return m_stick.getRawButton(A_BUTTON);
* @param input from an axis on the controller }
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
private boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE);
}
/** public boolean getXButton(){
* Updates an input to have a deadzone around the 0 position return m_stick.getRawButton(X_BUTTON);
* @param input from an axis on the controller }
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
}
}
public boolean getAButton(){ public boolean getBButton(){
return m_stick.getRawButton(A_BUTTON); return m_stick.getRawButton(B_BUTTON);
} }
public boolean getXButton(){ public boolean getYButton(){
return m_stick.getRawButton(X_BUTTON); return m_stick.getRawButton(Y_BUTTON);
} }
public boolean getBButton(){ public boolean getBackButton(){
return m_stick.getRawButton(B_BUTTON); return m_stick.getRawButton(BACK_BUTTON);
} }
public boolean getYButton(){ public boolean getStartButton(){
return m_stick.getRawButton(Y_BUTTON); return m_stick.getRawButton(START_BUTTON);
} }
public boolean getBackButton(){ public boolean getLeftBumperButton(){
return m_stick.getRawButton(BACK_BUTTON); return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
} }
public boolean getStartButton(){ public boolean getRightBumperButton(){
return m_stick.getRawButton(START_BUTTON); return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
} }
public boolean getLeftBumperButton(){ public boolean getLeftJoystickButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON); return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
} }
public boolean getRightBumperButton(){ public boolean getRightJoystickButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
} }
public boolean getLeftJoystickButton(){ public double getLeftXAxis(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
} }
public boolean getRightJoystickButton(){ public double getLeftYAxis(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
} }
public double getLeftXAxis(){ public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
} }
public double getLeftYAxis(){ public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
} }
public double getRightXAxis(){ public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
} }
public double getRightYAxis(){ public double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
} }
public double getLeftTriggerAxis(){ /**
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); * Get the angle input from the dpad.
} * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
*/
public int getDpadAngle() {
return m_stick.getPOV(0);
}
public double getRightTriggerAxis(){ public boolean getDPadLeft(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
} }
/** public boolean getDPadRight(){
* Get the angle input from the dpad. return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc. }
*/
public int getDpadAngle() {
return m_stick.getPOV(0);
}
public boolean getDPadLeft(){ public boolean getDPadTop(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
} }
public boolean getDPadRight(){ public boolean getDPadBottom(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
} }
public boolean getDPadTop(){ public boolean getLeftTrigger(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
} }
public boolean getDPadBottom(){ public boolean getRightTrigger(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
} }
public boolean getLeftTrigger(){ public boolean getRightAxisUpTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
} }
public boolean getRightTrigger(){ public boolean getRightAxisDownTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
} }
public boolean getRightAxisUpTrigger(){ public boolean getRightAxisLeftTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
} }
public boolean getRightAxisDownTrigger(){ public boolean getRightAxisRightTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
} }
public boolean getRightAxisLeftTrigger(){ public boolean getLeftAxisUpTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
} }
public boolean getRightAxisRightTrigger(){ public boolean getLeftAxisDownTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
} }
public boolean getLeftAxisUpTrigger(){ public boolean getLeftAxisLeftTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
} }
public boolean getLeftAxisDownTrigger(){ public boolean getLeftAxisRightTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
} }
public boolean getLeftAxisLeftTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
}
public boolean getLeftAxisRightTrigger(){
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
}
} }
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+53 -53
View File
@@ -8,61 +8,61 @@ import edu.wpi.first.wpilibj2.command.button.Button;
* exceeds a tolerance defined in {@link XboxController}. * exceeds a tolerance defined in {@link XboxController}.
*/ */
public class XboxTriggerButton extends Button { public class XboxTriggerButton extends Button {
public static final int RIGHT_TRIGGER = 0; public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1; public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2; public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6; public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7; public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9; public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
private XboxController m_controller; private XboxController m_controller;
private int m_trigger; private int m_trigger;
/** /**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger * Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/ */
public XboxTriggerButton(XboxController controller, int trigger) { public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller; m_controller = controller;
m_trigger = trigger; m_trigger = trigger;
}
/** {@inheritDoc} */
// @Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();
} }
else if (m_trigger == LEFT_TRIGGER) {
/** {@inheritDoc} */ return m_controller.getLeftTrigger();
// @Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();
}
else if (m_trigger == LEFT_TRIGGER) {
return m_controller.getLeftTrigger();
}
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
return m_controller.getRightAxisUpTrigger();
}
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
return m_controller.getRightAxisDownTrigger();
}
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
return m_controller.getRightAxisRightTrigger();
}
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
return m_controller.getRightAxisLeftTrigger();
}
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
return m_controller.getLeftAxisUpTrigger();
}
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
return m_controller.getLeftAxisDownTrigger();
}
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
return m_controller.getLeftAxisRightTrigger();
}
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
return m_controller.getLeftAxisLeftTrigger();
}
return false;
} }
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
return m_controller.getRightAxisUpTrigger();
}
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
return m_controller.getRightAxisDownTrigger();
}
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
return m_controller.getRightAxisRightTrigger();
}
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
return m_controller.getRightAxisLeftTrigger();
}
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
return m_controller.getLeftAxisUpTrigger();
}
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
return m_controller.getLeftAxisDownTrigger();
}
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
return m_controller.getLeftAxisRightTrigger();
}
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
return m_controller.getLeftAxisLeftTrigger();
}
return false;
}
} }
src/test/java/frc4388/utility/Delay.java
+8
View File
@@ -0,0 +1,8 @@
// package frc4388.utility;
// public class Delay {
// void delaymillis(long millis) {
// delay(millis)
// }
// }
todo.txt
+2 -2
View File
@@ -1,7 +1,7 @@
Revised Autosystem: Revised Autosystem:
- Autoplayback - Autoplayback
- Fix Arm Respool \ Fix Arm Respool
- Via Commands x Via Commands
- Arm - Arm
- Sequential Command System - Sequential Command System
- Manual Auto Creation - Manual Auto Creation