From fe366feb30e2c745928e13d7cb4a44d0b56dc7d6 Mon Sep 17 00:00:00 2001 From: ProfessorAtomicManiac <59379639+ProfessorAtomicManiac@users.noreply.github.com> Date: Sun, 1 Oct 2023 03:55:05 +0000 Subject: [PATCH] alt + shift + f, autoformatting everything --- .../robotcode2023/Constants.java | 318 ++++++++++-------- .../carlmontrobotics/robotcode2023/Robot.java | 42 ++- .../robotcode2023/RobotContainer.java | 255 ++++++++------ .../robotcode2023/commands/ArmTeleop.java | 27 +- .../robotcode2023/subsystems/Roller.java | 53 ++- 5 files changed, 390 insertions(+), 305 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java b/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java index 4d04b286..4813e476 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java @@ -17,107 +17,117 @@ import edu.wpi.first.wpilibj.XboxController.Button; /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared * globally (i.e. public static). Do not put anything functional in this class. * - *

It is advised to statically import this class (or one of its inner classes) wherever the + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the * constants are needed, to reduce verbosity. */ public final class Constants { - public static final double g = 9.81; //meters per second squared + public static final double g = 9.81; // meters per second squared public static final class Drivetrain { - //#region Subsystem Constants + // #region Subsystem Constants public static final double wheelBase = Units.inchesToMeters(19.75); public static final double trackWidth = Units.inchesToMeters(28.75); - // "swerveRadius" is the distance from the center of the robot to one of the modules + // "swerveRadius" is the distance from the center of the robot to one of the + // modules public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); // The gearing reduction from the drive motor controller to the wheels // Gearing for the Swerve Modules is 6.75 : 1 public static final double driveGearing = 6.75; public static final double driveModifier = 1; - public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36/7.65 /* empirical correction */; - public static final double mu = 0.5; /* 70/83.2; */ + public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* + * empirical correction + */; + public static final double mu = 0.5; /* 70/83.2; */ - public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s + public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s // Angular speed to translational speed --> v = omega * r / gearing public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; public static final double maxForward = maxSpeed; public static final double maxStrafe = maxSpeed; - // seconds it takes to go from 0 to 12 volts(aka MAX) + // seconds it takes to go from 0 to 12 volts(aka MAX) public static final double secsPer12Volts = 0.1; - - - // maxRCW is the angular velocity of the robot. - // Calculated by looking at one of the motors and treating it as a point mass moving around in a circle. - // Tangential speed of this point mass is maxSpeed and the radius of the circle is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) + // Calculated by looking at one of the motors and treating it as a point mass + // moving around in a circle. + // Tangential speed of this point mass is maxSpeed and the radius of the circle + // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) // Angular velocity = Tangential speed / radius public static final double maxRCW = maxSpeed / swerveRadius; - public static final boolean[] reversed = {false, false, false, false}; + public static final boolean[] reversed = { false, false, false, false }; // public static final boolean[] reversed = {true, true, true, true}; // Determine correct turnZero constants (FL, FR, BL, BR) - public static final double[] turnZero = RobotBase.isSimulation() ? - new double[] {0, 0, 0, 0} : - new double[] {85.7812, 85.0782 , -96.9433, -162.9492}; + public static final double[] turnZero = RobotBase.isSimulation() ? new double[] { 0, 0, 0, 0 } + : new double[] { 85.7812, 85.0782, -96.9433, -162.9492 }; - // kP, kI, and kD constants for turn motor controllers in the order of front-left, front-right, back-left, back-right. + // kP, kI, and kD constants for turn motor controllers in the order of + // front-left, front-right, back-left, back-right. // Determine correct turn PID constants - public static final double[] turnkP = {0.00374, 0.00374, 0.00374, 0.00374}; - public static final double[] turnkI = {0, 0, 0, 0}; - public static final double[] turnkD = {0, 0, 0, 0}; - public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; + public static final double[] turnkP = { 0.00374, 0.00374, 0.00374, 0.00374 }; + public static final double[] turnkI = { 0, 0, 0, 0 }; + public static final double[] turnkD = { 0, 0, 0, 0 }; + public static final double[] turnkS = { 0.2, 0.2, 0.2, 0.2 }; // V = kS + kV * v + kA * a // 12 = 0.2 + 0.00463 * v // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s - public static final double[] turnkV = {0.00463, 0.00463, 0.00463, 0.00463}; - public static final double[] turnkA = {0.000115, 0.000115, 0.000115, 0.000115}; + public static final double[] turnkV = { 0.00463, 0.00463, 0.00463, 0.00463 }; + public static final double[] turnkA = { 0.000115, 0.000115, 0.000115, 0.000115 }; // kP is an average of the forward and backward kP values // Forward: 1.72, 1.71, 1.92, 1.94 // Backward: 1.92, 1.92, 2.11, 1.89 - public static final double[] drivekP = {1.82, 1.815, 2.015, 1.915}; - public static final double[] drivekI = {0, 0, 0, 0}; - public static final double[] drivekD = {0, 0, 0, 0}; - public static final boolean[] driveInversion = {false, false, false, false}; - public static final boolean[] turnInversion = {true, true, true, true}; - - public static final double[] kForwardVolts = {0.129, 0.108, 0.14, 0.125}; - public static final double[] kBackwardVolts = {0.115, 0.169, 0.13, 0.148}; - - public static final double[] kForwardVels = {2.910/1.1, 2.970/1.1, 2.890/1.1, 2.930/1.1}; - public static final double[] kBackwardVels = {2.890/1.1, 2.800/1.1, 2.850/1.1, 2.820/1.1}; - public static final double[] kForwardAccels = {0.145, 0.149, 0.192, 0.198}; - public static final double[] kBackwardAccels = {0.192, 0.187, 0.264, 0.176}; - - public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second - public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 - public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java - // The maximum acceleration the robot can achieve is equal to the coefficient of static friction times the gravitational acceleration + public static final double[] drivekP = { 1.82, 1.815, 2.015, 1.915 }; + public static final double[] drivekI = { 0, 0, 0, 0 }; + public static final double[] drivekD = { 0, 0, 0, 0 }; + public static final boolean[] driveInversion = { false, false, false, false }; + public static final boolean[] turnInversion = { true, true, true, true }; + + public static final double[] kForwardVolts = { 0.129, 0.108, 0.14, 0.125 }; + public static final double[] kBackwardVolts = { 0.115, 0.169, 0.13, 0.148 }; + + public static final double[] kForwardVels = { 2.910 / 1.1, 2.970 / 1.1, 2.890 / 1.1, 2.930 / 1.1 }; + public static final double[] kBackwardVels = { 2.890 / 1.1, 2.800 / 1.1, 2.850 / 1.1, 2.820 / 1.1 }; + public static final double[] kForwardAccels = { 0.145, 0.149, 0.192, 0.198 }; + public static final double[] kBackwardAccels = { 0.192, 0.187, 0.264, 0.176 }; + + public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second + public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 + public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java + // The maximum acceleration the robot can achieve is equal to the coefficient of + // static friction times the gravitational acceleration // a = mu * 9.8 m/s^2 public static final double autoCentripetalAccel = mu * g * 2; public static final boolean isGyroReversed = true; // PID values are listed in the order kP, kI, and kD - public static final double[] xPIDController = {4, 0.0, 0.0}; - public static final double[] yPIDController = {4, 0.0, 0.0}; - public static final double[] thetaPIDController = {0.10, 0.0, 0.001}; + public static final double[] xPIDController = { 4, 0.0, 0.0 }; + public static final double[] yPIDController = { 4, 0.0, 0.0 }; + public static final double[] thetaPIDController = { 0.10, 0.0, 0.001 }; - public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZero, driveInversion, reversed, driveModifier, turnInversion); + public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, + autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, + kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZero, + driveInversion, reversed, driveModifier, turnInversion); public static final Limelight.Transform limelightTransformForPoseEstimation = Transform.BOTPOSE_WPIBLUE; - //#endregion + // #endregion - //#region Ports + // #region Ports public static final int driveFrontLeftPort = 8; public static final int driveFrontRightPort = 13; @@ -134,9 +144,9 @@ public static final class Drivetrain { public static final int canCoderPortBL = 3; public static final int canCoderPortBR = 4; - //#endregion + // #endregion - //#region Command Constants + // #region Command Constants public static final double kNormalDriveSpeed = 1; // Percent Multiplier public static final double kNormalDriveRotation = 0.5; // Percent Multiplier @@ -144,24 +154,30 @@ public static final class Drivetrain { public static final double kSlowDriveRotation = 0.250; // Percent Multiplier public static final double kBabyDriveSpeed = 0.3; // Percent Multiplier public static final double kBabyTurnSpeed = 0.2; // Percent Multiplier - public static final double kAlignMultiplier = 1D/3D; + public static final double kAlignMultiplier = 1D / 3D; public static final double kAlignForward = 0.6; public static final double chargeStationAlignToleranceDeg = 2.5; public static final double chargeStationAlignSpeedMpSPerDeg = 0.3 / 20; public static final double chargeStationAlignTime = 500; public static final double chargeStationAlignFFMpS = 0; - public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to orient the wheels to the correct angle. This should be very small to avoid actually moving the robot. - - public static final double[] positionTolerance = {Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5}; // Meters, Meters, Degrees - public static final double[] velocityTolerance = {Units.inchesToMeters(1), Units.inchesToMeters(1), 5}; // Meters, Meters, Degrees/Second - - //#endregion + public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to + // orient the wheels to the correct angle. This should + // be very small to avoid actually moving the robot. + + public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, + // Meters, + // Degrees + public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, + // Meters, + // Degrees/Second + + // #endregion } public static final class Arm { - //#region Subsystem Constants + // #region Subsystem Constants // Array Indexes (Just to make things easier to read) public static final int ARM = 0; @@ -173,17 +189,19 @@ public static final class Arm { // Feedforward // Arm, Wrist - public static final double[] kS = {0.20642, .084199}; - public static final double[] kG = {0.6697, 0.34116}; - public static final double[] kV = {4.3735, 2.008}; - public static final double[] kA = {0.24914, 0.041502}; + public static final double[] kS = { 0.20642, .084199 }; + public static final double[] kG = { 0.6697, 0.34116 }; + public static final double[] kV = { 4.3735, 2.008 }; + public static final double[] kA = { 0.24914, 0.041502 }; public static final double kG_WRIST = 0.34116; // (V) // PID // Arm, Wrist - public static double[] kP = {4.2736, 4.8804}; // 4.2736 for arm from sysid was tested and it worked fine (V / rad) - public static double[] kI = {0, 0}; // (V / (rad * s) ) - public static double[] kD = {0.1, 0.90262}; // 0 for arm from sysid was tested and it worked fine (V / (rad / s) ) + public static double[] kP = { 4.2736, 4.8804 }; // 4.2736 for arm from sysid was tested and it worked fine (V / + // rad) + public static double[] kI = { 0, 0 }; // (V / (rad * s) ) + public static double[] kD = { 0.1, 0.90262 }; // 0 for arm from sysid was tested and it worked fine (V / (rad / + // s) ) // Arm, Wrist public static double[] posToleranceRad = { .07, .07 }; // rad @@ -197,7 +215,7 @@ public static final class Arm { public static final double ARM_MASS_KG = Units.lbsToKilograms(6.841); public static final double ARM_LENGTH_METERS = Units.inchesToMeters(38.25); - // Distance from the arm motor to the center of mass of the arm + // Distance from the arm motor to the center of mass of the arm public static final double COM_ARM_LENGTH_METERS = Units.inchesToMeters(14.23); public static final double ROLLER_MASS_KG = Units.lbsToKilograms(15); @@ -214,28 +232,31 @@ public static final class Arm { public static final double DT_EXTENSION_FOR_ROLLER = Units.inchesToMeters(14); // TODO: Replace these values with Design's actual values - public static final double MARGIN_OF_ERROR = Math.PI/18; + public static final double MARGIN_OF_ERROR = Math.PI / 18; public static final double ARM_LOWER_LIMIT_RAD = -3.569 + MARGIN_OF_ERROR; public static final double ARM_UPPER_LIMIT_RAD = .36 - MARGIN_OF_ERROR; public static final double ARM_DISCONTINUITY_RAD = (ARM_LOWER_LIMIT_RAD + ARM_UPPER_LIMIT_RAD) / 2 - Math.PI; public static final double WRIST_LOWER_LIMIT_RAD = -2.933 + MARGIN_OF_ERROR; public static final double WRIST_UPPER_LIMIT_RAD = 2.605 - MARGIN_OF_ERROR; - public static final double WRIST_DISCONTINUITY_RAD = (WRIST_LOWER_LIMIT_RAD + WRIST_UPPER_LIMIT_RAD) / 2 - Math.PI; + public static final double WRIST_DISCONTINUITY_RAD = (WRIST_LOWER_LIMIT_RAD + WRIST_UPPER_LIMIT_RAD) / 2 + - Math.PI; // TODO: Determine actual max vel/accel // public static double[] MAX_FF_VEL = {.25, .25}; // rad / s - //Arm, Wrist - public static double[] MAX_FF_VEL_BABY = {0.675, 0.7}; - public static double[] MAX_FF_ACCEL_BABY = {4, 4}; - public static double[] MAX_FF_VEL_MANUAL = {1, 3}; // rad / s - public static double[] MAX_FF_VEL_AUTO = {1.25, 5}; // rad / s - public static double[] MAX_FF_ACCEL = {5, 5}; // rad / s^2 - public static TrapezoidProfile.Constraints armConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL_AUTO[ARM], MAX_FF_ACCEL[ARM]); - public static TrapezoidProfile.Constraints wristConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL_AUTO[WRIST], MAX_FF_ACCEL[WRIST]); + // Arm, Wrist + public static double[] MAX_FF_VEL_BABY = { 0.675, 0.7 }; + public static double[] MAX_FF_ACCEL_BABY = { 4, 4 }; + public static double[] MAX_FF_VEL_MANUAL = { 1, 3 }; // rad / s + public static double[] MAX_FF_VEL_AUTO = { 1.25, 5 }; // rad / s + public static double[] MAX_FF_ACCEL = { 5, 5 }; // rad / s^2 + public static TrapezoidProfile.Constraints armConstraints = new TrapezoidProfile.Constraints( + MAX_FF_VEL_AUTO[ARM], MAX_FF_ACCEL[ARM]); + public static TrapezoidProfile.Constraints wristConstraints = new TrapezoidProfile.Constraints( + MAX_FF_VEL_AUTO[WRIST], MAX_FF_ACCEL[WRIST]); - //#endregion + // #endregion - //#region Ports + // #region Ports public static final boolean[] motorInverted = { true, false }; public static final boolean[] encoderInverted = { true, true }; @@ -244,9 +265,9 @@ public static final class Arm { public static final int armMotorPort = 17; public static final int wristMotorPort = 19; - //#endregion + // #endregion - //#region Command Constants + // #region Command Constants public static final double WRIST_STOW_POS_RAD = WRIST_UPPER_LIMIT_RAD; public static final double WRIST_NEG_STOW_POS_RAD = WRIST_LOWER_LIMIT_RAD; @@ -257,13 +278,13 @@ public static final class Arm { public static double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = .5; public static final double DISCONNECTED_ENCODER_TIMEOUT_SEC = .25; - //#endregion + // #endregion } public static final class GoalPos { - //#region Goal Positions + // #region Goal Positions // if intake/outtake on back, the "negative" pos will be used // 0 = "Front", 1 = "Back" (i.e. front refers to the side with the battery) // 0 = CUBE, 1 = CONE @@ -271,75 +292,75 @@ public static final class GoalPos { // TODO : Get angles for front public static GoalPos[][] LOW = { - { // front - new GoalPos(-1.507131, 2.327210), // cube - new GoalPos(-1.437940, 2.123031) // cone - }, - { // back - new GoalPos(-2.387175, 1.494942), - new GoalPos(-1.657286, -2.410204) - } + { // front + new GoalPos(-1.507131, 2.327210), // cube + new GoalPos(-1.437940, 2.123031) // cone + }, + { // back + new GoalPos(-2.387175, 1.494942), + new GoalPos(-1.657286, -2.410204) + } }; public static GoalPos[][] MID = { - { - new GoalPos(-0.596292, 1.513329), - new GoalPos(0.208573, -1.690364) - }, - { - new GoalPos(-3.055642, 1.916546), - new GoalPos(-3.417581, 1.683445) - } + { + new GoalPos(-0.596292, 1.513329), + new GoalPos(0.208573, -1.690364) + }, + { + new GoalPos(-3.055642, 1.916546), + new GoalPos(-3.417581, 1.683445) + } }; public static GoalPos[][] HIGH = { - { - new GoalPos(-0.156905, 0.901323), - new GoalPos(0.205366, -0.769676) - }, - { - new GoalPos(-3.415958, 1.700500), - new GoalPos(-3.414373, 0.667) - } + { + new GoalPos(-0.156905, 0.901323), + new GoalPos(0.205366, -0.769676) + }, + { + new GoalPos(-3.415958, 1.700500), + new GoalPos(-3.414373, 0.667) + } }; // TODO: Get positions for STORED, SHELF, and SUBSTATION public static GoalPos[][] STORED = { - { - new GoalPos(-1.559094, 2.424171), - new GoalPos(-1.559094, 2.424171) - }, - { - new GoalPos(-1.559094, 2.424171), - new GoalPos(-1.559094, 2.424171) - } + { + new GoalPos(-1.559094, 2.424171), + new GoalPos(-1.559094, 2.424171) + }, + { + new GoalPos(-1.559094, 2.424171), + new GoalPos(-1.559094, 2.424171) + } }; public static GoalPos[][] SHELF = { - { - new GoalPos(0.224062, -0.800449), - new GoalPos(0.224062, -0.800449) - }, - { - new GoalPos(0.224062, -0.800449), - new GoalPos(0.224062, -0.800449) - } + { + new GoalPos(0.224062, -0.800449), + new GoalPos(0.224062, -0.800449) + }, + { + new GoalPos(0.224062, -0.800449), + new GoalPos(0.224062, -0.800449) + } }; public static GoalPos[][] SUBSTATION = { - { - new GoalPos(-1.459526, 2.417944), - new GoalPos(-1.459526, 2.417944) - }, - { - new GoalPos(-1.459526, 2.417944), - new GoalPos(-1.459526, 2.417944) - } + { + new GoalPos(-1.459526, 2.417944), + new GoalPos(-1.459526, 2.417944) + }, + { + new GoalPos(-1.459526, 2.417944), + new GoalPos(-1.459526, 2.417944) + } }; public static GoalPos[][] INTAKE = { - { - new GoalPos(-1.29, 1.32), - new GoalPos(-1.208155, 0.646987) - }, - { - new GoalPos(-1.271106, 1.303141), - new GoalPos(-1.208155, 0.646987) - } + { + new GoalPos(-1.29, 1.32), + new GoalPos(-1.208155, 0.646987) + }, + { + new GoalPos(-1.271106, 1.303141), + new GoalPos(-1.208155, 0.646987) + } }; public double armPos, wristPos; @@ -347,16 +368,17 @@ public GoalPos(double armPos, double wristPos) { this.armPos = armPos; this.wristPos = wristPos; } - //#endregion + // #endregion } public static final class Roller { - //#region Subsystem Constants + // #region Subsystem Constants public static final int ledLength = 85; - public static final double ledDefaultColorRestoreTime = 3; // The time in seconds after picking up a game piece to restore the LED color to defaultColor + public static final double ledDefaultColorRestoreTime = 3; // The time in seconds after picking up a game piece + // to restore the LED color to defaultColor public static final Color defaultColor = new Color(0, 0, 200); public static final Color pickupSuccessColor = new Color(0, 200, 0); public static final Color conePickupColor = new Color(150, 150, 0); @@ -370,18 +392,18 @@ public static final class Roller { public static final int ROLLER_MAX_CURRENT_AMPS = 20; - //#endregion + // #endregion - //#region Ports + // #region Ports public static final int rollerPort = 18; public static final int ledPort = 0; - //#endregion + // #endregion - //#region Command Constants + // #region Command Constants - //#endregion + // #endregion public static enum GameObject { CUBE, CONE, NONE diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/Robot.java b/src/main/java/org/carlmontrobotics/robotcode2023/Robot.java index df2a875d..61fbfc4c 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/Robot.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/Robot.java @@ -25,7 +25,8 @@ public void robotInit() { robot = this; DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); - if(!DriverStation.isFMSAttached()) PathPlannerServer.startServer(5811); + if (!DriverStation.isFMSAttached()) + PathPlannerServer.startServer(5811); robotContainer = new RobotContainer(); SmartDashboard.putBoolean("safeMode", false); @@ -44,16 +45,15 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); MotorErrors.printSparkMaxErrorMessages(); - //set mode to baby if baby was true - //else but don't set mode to norm if slow was true - if (SmartDashboard.getBoolean("safeMode", false)) { - RobotContainer.driverMode = RobotContainer.DriverMode.BABY; - } else if (RobotContainer.driverMode.isSlow()) { - RobotContainer.driverMode = RobotContainer.DriverMode.NORM; - } + // set mode to baby if baby was true + // else but don't set mode to norm if slow was true + if (SmartDashboard.getBoolean("safeMode", false)) { + RobotContainer.driverMode = RobotContainer.DriverMode.BABY; + } else if (RobotContainer.driverMode.isSlow()) { + RobotContainer.driverMode = RobotContainer.DriverMode.NORM; + } } - @Override public void disabledInit() { new Thread(() -> { @@ -69,10 +69,12 @@ public void disabledInit() { } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } @Override - public void disabledExit() {} + public void disabledExit() { + } @Override public void autonomousInit() { @@ -82,10 +84,12 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override - public void autonomousExit() {} + public void autonomousExit() { + } @Override public void teleopInit() { @@ -94,10 +98,12 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override - public void teleopExit() {} + public void teleopExit() { + } @Override public void testInit() { @@ -106,8 +112,10 @@ public void testInit() { } @Override - public void testPeriodic() {} + public void testPeriodic() { + } @Override - public void testExit() {} + public void testExit() { + } } diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java index 4d51ea0d..1f8c2460 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java @@ -46,10 +46,8 @@ import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; - - public class RobotContainer { - + public static DriverMode driverMode = DriverMode.NORM; public final GenericHID driverController = new GenericHID(Driver.port); @@ -73,31 +71,39 @@ public RobotContainer() { { eventMap.put("Cone High Pos.", new SetArmWristGoalPreset(GoalPos.HIGH, () -> false, () -> false, arm)); - // Command fakeArmCommand = new InstantCommand(() -> System.err.println("==============Store================="), arm); - // eventMap.put("Stored Pos.", new SequentialCommandGroup(fakeArmCommand, new WaitCommand(2))); - eventMap.put("Run Cube Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE))); + // Command fakeArmCommand = new InstantCommand(() -> + // System.err.println("==============Store================="), arm); + // eventMap.put("Stored Pos.", new SequentialCommandGroup(fakeArmCommand, new + // WaitCommand(2))); + eventMap.put("Run Cube Intake", + new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), + new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE))); eventMap.put("DriveOverChargeStation", new DriveOverChargeStation(drivetrain)); eventMap.put("Extend Arm High Cube", new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm)); eventMap.put("Extend Arm Mid Cube", new SetArmWristGoalPreset(GoalPos.MID, () -> true, () -> false, arm)); eventMap.put("Store Arm", new SetArmWristGoalPreset(GoalPos.STORED, () -> true, () -> false, arm)); eventMap.put("Cube High Pos.", new SequentialCommandGroup( - new PrintCommand("================================Cube High Pos. Started=================================="), - new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm), - new PrintCommand("================================Cube High Pos. Ended==================================") - )); + new PrintCommand("================================Cube High Pos. Started=================================="), + new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm), + new PrintCommand("================================Cube High Pos. Ended=================================="))); eventMap.put("Run Cube Outtake", new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE)); - eventMap.put("Run Cone Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); + eventMap.put("Run Cone Intake", + new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), + new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); eventMap.put("Run Cone Outtake", new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE)); - //eventMap.put("Move Arm Back", new SetArmWristPositionV3((-5*Math.PI)/8, Constants.Arm.WRIST_STOW_POS_RAD, arm)); + // eventMap.put("Move Arm Back", new SetArmWristPositionV3((-5*Math.PI)/8, + // Constants.Arm.WRIST_STOW_POS_RAD, arm)); eventMap.put("Cone Intake Pos.", new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm)); eventMap.put("Cube Intake Pos.", new SequentialCommandGroup( - new PrintCommand("================================Cube Intake Pos. Started=================================="), - new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), - new PrintCommand("================================Cube Intake Pos. Ended==================================") - )); + new PrintCommand( + "================================Cube Intake Pos. Started=================================="), + new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), + new PrintCommand( + "================================Cube Intake Pos. Ended=================================="))); eventMap.put("Field Rotate 90", new RotateToFieldRelativeAngle(new Rotation2d(Math.PI / 2), drivetrain)); - eventMap.put("Rotate 180", new ProxyCommand(new RotateToFieldRelativeAngle(new Rotation2d(Units.degreesToRadians(drivetrain.getHeadingDeg() + 180)), drivetrain))); + eventMap.put("Rotate 180", new ProxyCommand(new RotateToFieldRelativeAngle( + new Rotation2d(Units.degreesToRadians(drivetrain.getHeadingDeg() + 180)), drivetrain))); eventMap.put("Stop", stopDt()); eventMap.put("Auto-Align", new ProxyCommand(() -> new AlignChargingStation(drivetrain))); // eventMap.put("PrintAlign", new PrintCommand("Aligning")); @@ -110,87 +116,119 @@ public RobotContainer() { } autoPaths = new Command[] { - null, - new PPRobotPath("Mid Basic", drivetrain, false, eventMap).getPathCommand(true, true), - new PPRobotPath("Side Basic", drivetrain, false, eventMap).getPathCommand(true, true), - new PPRobotPath("Place Cone", drivetrain, false, eventMap).getPathCommand(true, true), - new PPRobotPath(PathPlanner.loadPathGroup("Side Basic 1", drivetrain.getMaxSpeedMps(), 1, false), drivetrain, eventMap) - .getPathCommand(true, true).andThen( - new PPRobotPath("Side Basic 2", drivetrain, false, eventMap).getPathCommand(false, true) - ), - new SequentialCommandGroup( - new PPRobotPath("Mid Basic 5-1", drivetrain, false, eventMap).getPathCommand(true, true), - new ProxyCommand(new RotateToFieldRelativeAngle(new Rotation2d(Units.degreesToRadians(drivetrain.getHeadingDeg() + 180)), drivetrain)), - new PPRobotPath("Mid Basic 5-2", drivetrain, false, eventMap).getPathCommand(true, true) - ) + null, + new PPRobotPath("Mid Basic", drivetrain, false, eventMap).getPathCommand(true, true), + new PPRobotPath("Side Basic", drivetrain, false, eventMap).getPathCommand(true, true), + new PPRobotPath("Place Cone", drivetrain, false, eventMap).getPathCommand(true, true), + new PPRobotPath(PathPlanner.loadPathGroup("Side Basic 1", drivetrain.getMaxSpeedMps(), 1, false), drivetrain, + eventMap) + .getPathCommand(true, true).andThen( + new PPRobotPath("Side Basic 2", drivetrain, false, eventMap).getPathCommand(false, true)), + new SequentialCommandGroup( + new PPRobotPath("Mid Basic 5-1", drivetrain, false, eventMap).getPathCommand(true, true), + new ProxyCommand(new RotateToFieldRelativeAngle( + new Rotation2d(Units.degreesToRadians(drivetrain.getHeadingDeg() + 180)), drivetrain)), + new PPRobotPath("Mid Basic 5-2", drivetrain, false, eventMap).getPathCommand(true, true)) }; autoSelectors = new DigitalInput[Math.min(autoPaths.length, 26)]; - for(int i = 0; i < autoSelectors.length; i++) autoSelectors[i] = new DigitalInput(i); + for (int i = 0; i < autoSelectors.length; i++) + autoSelectors[i] = new DigitalInput(i); drivetrain.setDefaultCommand(new TeleopDrive( - drivetrain, - () -> inputProcessing(getStickValue(driverController, Axis.kLeftY)), - () -> inputProcessing(getStickValue(driverController, Axis.kLeftX)), - () -> inputProcessing(getStickValue(driverController, Axis.kRightX)), - () -> {return driverMode;} - )); + drivetrain, + () -> inputProcessing(getStickValue(driverController, Axis.kLeftY)), + () -> inputProcessing(getStickValue(driverController, Axis.kLeftX)), + () -> inputProcessing(getStickValue(driverController, Axis.kRightX)), + () -> { + return driverMode; + })); configureButtonBindingsDriver(); configureButtonBindingsManipulator(); arm.setDefaultCommand(new ArmTeleop( - arm, - () -> {return (driverMode.isBaby() ? 0 : inputProcessing(getStickValue(manipulatorController, Axis.kLeftY)));}, - () -> {return (driverMode.isBaby() ? 0 : inputProcessing(getStickValue(manipulatorController, Axis.kRightY)));}, - () -> {return driverMode.isBaby();} - )); + arm, + () -> { + return (driverMode.isBaby() ? 0 : inputProcessing(getStickValue(manipulatorController, Axis.kLeftY))); + }, + () -> { + return (driverMode.isBaby() ? 0 : inputProcessing(getStickValue(manipulatorController, Axis.kRightY))); + }, + () -> { + return driverMode.isBaby(); + })); } + private void configureButtonBindingsDriver() { new JoystickButton(driverController, Driver.chargeStationAlignButton).onTrue(new AlignChargingStation(drivetrain)); - new JoystickButton(driverController, Driver.resetFieldOrientationButton).onTrue(new InstantCommand(drivetrain::resetFieldOrientation)); - new JoystickButton(driverController, Driver.toggleFieldOrientedButton).onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(!drivetrain.getFieldOriented()))); - new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle0Deg).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); - new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle90Deg).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(-90), drivetrain)); - new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle180Deg).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(180), drivetrain)); - new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle270Deg).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), drivetrain)); - - new JoystickButton(driverController, Driver.slowDriveButton).whileTrue(new InstantCommand(() -> driverMode.isSlow())); - + new JoystickButton(driverController, Driver.resetFieldOrientationButton) + .onTrue(new InstantCommand(drivetrain::resetFieldOrientation)); + new JoystickButton(driverController, Driver.toggleFieldOrientedButton) + .onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(!drivetrain.getFieldOriented()))); + new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle0Deg) + .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); + new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle90Deg) + .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(-90), drivetrain)); + new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle180Deg) + .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(180), drivetrain)); + new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle270Deg) + .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), drivetrain)); + + new JoystickButton(driverController, Driver.slowDriveButton) + .whileTrue(new InstantCommand(() -> driverMode.isSlow())); + } private void configureButtonBindingsManipulator() { - BooleanSupplier isCube = () -> new JoystickButton(manipulatorController, Manipulator.toggleCubeButton).getAsBoolean(); - BooleanSupplier isFront = () -> new JoystickButton(manipulatorController, Manipulator.toggleFrontButton).getAsBoolean(); + BooleanSupplier isCube = () -> new JoystickButton(manipulatorController, Manipulator.toggleCubeButton) + .getAsBoolean(); + BooleanSupplier isFront = () -> new JoystickButton(manipulatorController, Manipulator.toggleFrontButton) + .getAsBoolean(); BooleanSupplier isIntake = () -> !isCube.getAsBoolean(); - new JoystickButton(manipulatorController, Manipulator.storePosButton).onTrue(new SetArmWristGoalPreset(GoalPos.STORED, isCube, isFront, arm)); - //new JoystickButton(manipulatorController, Manipulator.lowPosButton).onTrue(new SetArmWristGoalPreset(GoalPos.LOW, isCube, isFront, arm)); - new JoystickButton(manipulatorController, Manipulator.midPosButton).onTrue(new SetArmWristGoalPreset(GoalPos.MID, isCube, isFront, arm)); - new JoystickButton(manipulatorController, Manipulator.highPosButton).onTrue(new SetArmWristGoalPreset(GoalPos.HIGH, isCube, isFront, arm)); - new POVButton(manipulatorController, Manipulator.shelfPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SHELF, isCube, isFront, arm)); - new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm)); - new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.STORED, isCube, isFront, arm)); - - new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm)); - new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); - new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); - new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE))); - new JoystickButton(manipulatorController, Manipulator.stopRollerButton).onTrue(new RunRoller(roller, Roller.RollerMode.STOP)); - + new JoystickButton(manipulatorController, Manipulator.storePosButton) + .onTrue(new SetArmWristGoalPreset(GoalPos.STORED, isCube, isFront, arm)); + // new JoystickButton(manipulatorController, + // Manipulator.lowPosButton).onTrue(new SetArmWristGoalPreset(GoalPos.LOW, + // isCube, isFront, arm)); + new JoystickButton(manipulatorController, Manipulator.midPosButton) + .onTrue(new SetArmWristGoalPreset(GoalPos.MID, isCube, isFront, arm)); + new JoystickButton(manipulatorController, Manipulator.highPosButton) + .onTrue(new SetArmWristGoalPreset(GoalPos.HIGH, isCube, isFront, arm)); + new POVButton(manipulatorController, Manipulator.shelfPickupPOV) + .onTrue(new SetArmWristGoalPreset(GoalPos.SHELF, isCube, isFront, arm)); + new POVButton(manipulatorController, Manipulator.intakeConePOV) + .onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm)); + new POVButton(manipulatorController, Manipulator.substationPickupPOV) + .onTrue(new SetArmWristGoalPreset(GoalPos.STORED, isCube, isFront, arm)); + + new POVButton(manipulatorController, Manipulator.intakeCubePOV) + .onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm)); + new POVButton(manipulatorController, Manipulator.intakeConePOV) + .onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm) + .andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); + new POVButton(manipulatorController, Manipulator.substationPickupPOV) + .onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm) + .andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE))); + new POVButton(manipulatorController, Manipulator.intakeCubePOV) + .onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm) + .andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE))); + new JoystickButton(manipulatorController, Manipulator.stopRollerButton) + .onTrue(new RunRoller(roller, Roller.RollerMode.STOP)); + // axisTrigger(manipulatorController, Manipulator.rollerIntakeConeButton) - // .onTrue(new RunRoller(roller, RollerMode.INTAKE_CONE, Constants.Roller.conePickupColor)); + // .onTrue(new RunRoller(roller, RollerMode.INTAKE_CONE, + // Constants.Roller.conePickupColor)); axisTrigger(manipulatorController, Manipulator.rollerIntakeCubeButton) - .onTrue(new ConditionalCommand( - new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE), - new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE), - isIntake - )); + .onTrue(new ConditionalCommand( + new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE), + new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE), + isIntake)); axisTrigger(manipulatorController, Manipulator.rollerIntakeConeButton) - .onTrue(new ConditionalCommand( - new RunRoller(roller, Roller.RollerMode.INTAKE_CONE), - new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE), - isIntake - )); + .onTrue(new ConditionalCommand( + new RunRoller(roller, Roller.RollerMode.INTAKE_CONE), + new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE), + isIntake)); } @@ -200,34 +238,37 @@ public Command stopDt() { public Command getAutonomousCommand() { Command autoPath = null; - //PPRobotPath autoPath = new PPRobotPath("Basic 7", drivetrain, false, eventMap); + // PPRobotPath autoPath = new PPRobotPath("Basic 7", drivetrain, false, + // eventMap); // Command[] autoPath2 = { - // new PPRobotPath("Mid Basic 3", drivetrain, false, eventMap).getPathCommand(true, true), - // new PPRobotPath("Mid Basic 4", drivetrain, false, eventMap).getPathCommand(false, true) + // new PPRobotPath("Mid Basic 3", drivetrain, false, + // eventMap).getPathCommand(true, true), + // new PPRobotPath("Mid Basic 4", drivetrain, false, + // eventMap).getPathCommand(false, true) // }; // Command[] commands = { - // stopDt(), - // new WaitCommand(0) + // stopDt(), + // new WaitCommand(0) // }; // SequentialCommandGroup autoCommand = new SequentialCommandGroup(); // for (int i = 0; i < autoPath2.length; i++) { - // autoCommand.addCommands(autoPath2[i]); - // //autoCommand.addCommands(commands[i]); + // autoCommand.addCommands(autoPath2[i]); + // //autoCommand.addCommands(commands[i]); // } - for(int i = 0; i < autoSelectors.length; i++) { - if(!autoSelectors[i].get()) { + for (int i = 0; i < autoSelectors.length; i++) { + if (!autoSelectors[i].get()) { System.out.println("Using Path: " + i); autoPath = autoPaths[i]; break; } } - //return autoPath == null ? new PrintCommand("No Autonomous Routine selected") : autoCommand; - return autoPath == null ? new PrintCommand("null :(") : autoPath; + // return autoPath == null ? new PrintCommand("No Autonomous Routine selected") + // : autoCommand; + return autoPath == null ? new PrintCommand("null :(") : autoPath; } - public void onEnable() { lime.getNTEntry("pipeline").setDouble(DriverStation.getAlliance() == Alliance.Red ? 1 : 0); } @@ -253,23 +294,35 @@ private double inputProcessing(double value) { } /** - * Returns a new instance of Trigger based on the given Joystick and Axis objects. - * The Trigger is triggered when the absolute value of the stick value on the specified axis + * Returns a new instance of Trigger based on the given Joystick and Axis + * objects. + * The Trigger is triggered when the absolute value of the stick value on the + * specified axis * exceeds a minimum threshold value. * * @param stick The Joystick object to retrieve stick value from. - * @param axis The Axis object to retrieve value from the Joystick. - * @return A new instance of Trigger based on the given Joystick and Axis objects. + * @param axis The Axis object to retrieve value from the Joystick. + * @return A new instance of Trigger based on the given Joystick and Axis + * objects. * @throws NullPointerException if either stick or axis is null. */ private Trigger axisTrigger(GenericHID stick, Axis axis) { return new Trigger(() -> Math.abs(getStickValue(stick, axis)) > MIN_AXIS_TRIGGER_VALUE); } - - public static enum DriverMode { - NORM,SLOW,BABY; - public boolean isNorm(){ return this==DriverMode.NORM; } - public boolean isSlow(){ return this==DriverMode.SLOW; } - public boolean isBaby(){ return this==DriverMode.BABY; } - } + + public static enum DriverMode { + NORM, SLOW, BABY; + + public boolean isNorm() { + return this == DriverMode.NORM; + } + + public boolean isSlow() { + return this == DriverMode.SLOW; + } + + public boolean isBaby() { + return this == DriverMode.BABY; + } + } } diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java index 56c4e0a4..695a306e 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java @@ -22,7 +22,7 @@ public class ArmTeleop extends CommandBase { private Arm armSubsystem; private DoubleSupplier arm; private DoubleSupplier wrist; - private BooleanSupplier baby;//is robot in baby (safe) mode? + private BooleanSupplier baby; // is robot in baby (safe) mode? private double lastTime = 0; public ArmTeleop(Arm armSubsystem, DoubleSupplier arm, DoubleSupplier wrist, BooleanSupplier baby) { @@ -30,7 +30,7 @@ public ArmTeleop(Arm armSubsystem, DoubleSupplier arm, DoubleSupplier wrist, Boo addRequirements(this.armSubsystem = armSubsystem); this.arm = arm; this.wrist = wrist; - this.baby = baby; + this.baby = baby; } // Called when the command is initially scheduled. @@ -56,11 +56,14 @@ public void execute() { goalArmRad = MathUtil.clamp(goalArmRad, ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD); goalWristRad = MathUtil.clamp(goalWristRad, WRIST_LOWER_LIMIT_RAD, WRIST_UPPER_LIMIT_RAD); - // Clamp the goal to within a certain range of the current position to prevent "lag" - goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); - goalWristRad = MathUtil.clamp(goalWristRad, armSubsystem.getWristPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, armSubsystem.getWristPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); + // Clamp the goal to within a certain range of the current position to prevent + // "lag" + goalArmRad = MathUtil.clamp(goalArmRad, armSubsystem.getArmPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, + armSubsystem.getArmPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); + goalWristRad = MathUtil.clamp(goalWristRad, armSubsystem.getWristPos() - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD, + armSubsystem.getWristPos() + ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); - if((speeds[0] != 0 || speeds[1] != 0) && !DriverStation.isAutonomous()) { + if ((speeds[0] != 0 || speeds[1] != 0) && !DriverStation.isAutonomous()) { armSubsystem.setArmTarget(goalArmRad, armSubsystem.getCurrentArmGoal().velocity); armSubsystem.setWristTarget(goalWristRad, armSubsystem.getCurrentWristGoal().velocity); } @@ -70,13 +73,13 @@ public void execute() { // Copy and pasted from drivetrain, handles input from joysticks public double[] getRequestedSpeeds() { - // Sets all values less than or equal to a very small value (determined by the + // Sets all values less than or equal to a very small value (determined by the // idle joystick state) to zero. - + double rawArmVel, rawWristVel; - // prep maximum velocity variable - double[] maxFF = baby.getAsBoolean() ? MAX_FF_VEL_BABY : MAX_FF_VEL_MANUAL; - + // prep maximum velocity variable + double[] maxFF = baby.getAsBoolean() ? MAX_FF_VEL_BABY : MAX_FF_VEL_MANUAL; + if (Math.abs(arm.getAsDouble()) <= Constants.OI.JOY_THRESH) rawArmVel = 0.0; else @@ -87,7 +90,7 @@ public double[] getRequestedSpeeds() { else rawWristVel = maxFF[WRIST] * wrist.getAsDouble(); - return new double[] {rawArmVel, rawWristVel}; + return new double[] { rawArmVel, rawWristVel }; } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java b/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java index 69a1feb3..e7b1992f 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java @@ -119,31 +119,30 @@ public double getPosition() { return motor.getEncoder().getPosition(); } - // TODO: Determine actual speeds/timings for roller - public static class RollerMode { - public static RollerMode INTAKE_CONE = new RollerMode(-0.5, .5, GameObject.CONE, conePickupColor); - public static RollerMode INTAKE_CUBE = new RollerMode(0.4, .25, GameObject.CUBE, cubePickupColor); - // The obj indicates which game object the roller is trying to intake - // if obj == NONE, that means it is trying to outtake rather than intake - public static RollerMode OUTTAKE_CONE = new RollerMode(0.5, .5, GameObject.NONE, defaultColor); - public static RollerMode OUTTAKE_CUBE = new RollerMode(-0.5, .5, GameObject.NONE, defaultColor); - public static RollerMode STOP = new RollerMode(0, .1, GameObject.NONE, defaultColor); - public double speed; - public double time; - public GameObject obj; - public Color ledColor; - - /** - * @param speed A number between -1 and 1 - * @param time Amount of time in seconds to keep the motor running after - * distance sensor has detected an object - * @param intake Whether the roller is outtaking or intaking - */ - public RollerMode(double speed, double time, GameObject obj, Color ledColor) { - this.speed = speed; - this.time = time; - this.obj = obj; - this.ledColor = ledColor; - } - } + public static class RollerMode { + public static RollerMode INTAKE_CONE = new RollerMode(-0.5, .5, GameObject.CONE, conePickupColor); + public static RollerMode INTAKE_CUBE = new RollerMode(0.4, .25, GameObject.CUBE, cubePickupColor); + // The obj indicates which game object the roller is trying to intake + // if obj == NONE, that means it is trying to outtake rather than intake + public static RollerMode OUTTAKE_CONE = new RollerMode(0.5, .5, GameObject.NONE, defaultColor); + public static RollerMode OUTTAKE_CUBE = new RollerMode(-0.5, .5, GameObject.NONE, defaultColor); + public static RollerMode STOP = new RollerMode(0, .1, GameObject.NONE, defaultColor); + public double speed; + public double time; + public GameObject obj; + public Color ledColor; + + /** + * @param speed A number between -1 and 1 + * @param time Amount of time in seconds to keep the motor running after + * distance sensor has detected an object + * @param intake Whether the roller is outtaking or intaking + */ + public RollerMode(double speed, double time, GameObject obj, Color ledColor) { + this.speed = speed; + this.time = time; + this.obj = obj; + this.ledColor = ledColor; + } + } }