diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java b/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java index 2fd118a2..34127792 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,32 +144,40 @@ 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 public static final double kSlowDriveSpeed = 0.4; // Percent Multiplier public static final double kSlowDriveRotation = 0.250; // Percent Multiplier - public static final double kAlignMultiplier = 1D/3D; + 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 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; @@ -171,23 +189,25 @@ 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 public static double[] velToleranceRadPSec = { 1, 1 }; // rad/s - public static double[] offsetRad = { 0.865, 2.93 + Math.PI / 2 }; // rad + public static double[] offsetRad = { 3.237, 4.501 }; // rad public static final double rumbleFullPower = 1; public static final double rumbleNoPower = 0; @@ -195,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); @@ -212,36 +232,42 @@ 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 - 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 = { false, true }; + public static final boolean[] encoderInverted = { true, true }; public static final double rotationToRad = 2 * Math.PI; 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; @@ -250,14 +276,16 @@ public static final class Arm { public static final int WRIST_CURRENT_LIMIT_AMP = 15; public static final double ROLLER_COM_CORRECTION_RAD = Units.degreesToRadians(18.3); public static double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = .5; + // if the encoder has been disconnected for DISCONNECTED_ENCODER_TIMEOUT, shut off the motor + 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 @@ -265,75 +293,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; @@ -341,16 +369,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); @@ -364,44 +393,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 - - //#region Command Constants - // 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, 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; - } - } + // #endregion + + // #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 dde8ad92..69f9abc7 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/Robot.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/Robot.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; public class Robot extends TimedRobot { @@ -24,8 +25,11 @@ 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); } @Override @@ -40,8 +44,13 @@ public void simulationInit() { public void robotPeriodic() { CommandScheduler.getInstance().run(); MotorErrors.printSparkMaxErrorMessages(); - } + if (SmartDashboard.getBoolean("safeMode", false)) { + RobotContainer.driverMode = RobotContainer.DriverMode.BABY; + } else if (!RobotContainer.driverMode.isSlow()) { + RobotContainer.driverMode = RobotContainer.DriverMode.NORM; + } + } @Override public void disabledInit() { @@ -58,10 +67,12 @@ public void disabledInit() { } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } @Override - public void disabledExit() {} + public void disabledExit() { + } @Override public void autonomousInit() { @@ -71,10 +82,12 @@ public void autonomousInit() { } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override - public void autonomousExit() {} + public void autonomousExit() { + } @Override public void teleopInit() { @@ -83,10 +96,12 @@ public void teleopInit() { } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override - public void teleopExit() {} + public void teleopExit() { + } @Override public void testInit() { @@ -95,8 +110,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 832efb21..0c7fec60 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java @@ -14,7 +14,6 @@ import org.carlmontrobotics.robotcode2023.Constants.GoalPos; import org.carlmontrobotics.robotcode2023.Constants.OI.Driver; import org.carlmontrobotics.robotcode2023.Constants.OI.Manipulator; -import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode; import org.carlmontrobotics.robotcode2023.commands.AlignChargingStation; import org.carlmontrobotics.robotcode2023.commands.ArmTeleop; import org.carlmontrobotics.robotcode2023.commands.DriveOverChargeStation; @@ -47,10 +46,10 @@ 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 class RobotContainer { - public final GenericHID driverController = new GenericHID(Driver.port); public final GenericHID manipulatorController = new GenericHID(Manipulator.port); @@ -72,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, 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==================================") - )); - eventMap.put("Run Cube Outtake", new RunRoller(roller, RollerMode.OUTTAKE_CUBE)); - eventMap.put("Run Cone Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), new RunRoller(roller, RollerMode.INTAKE_CONE))); - eventMap.put("Run Cone Outtake", new RunRoller(roller, RollerMode.OUTTAKE_CONE)); - //eventMap.put("Move Arm Back", new SetArmWristPositionV3((-5*Math.PI)/8, Constants.Arm.WRIST_STOW_POS_RAD, arm)); + 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 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("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")); @@ -109,84 +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)), - () -> driverController.getRawButton(Driver.slowDriveButton) - )); + 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, - () -> inputProcessing(getStickValue(manipulatorController, Axis.kLeftY)), - () -> inputProcessing(getStickValue(manipulatorController, Axis.kRightY)) - )); + arm, + () -> { + return inputProcessing(getStickValue(manipulatorController, Axis.kLeftY)); + }, + () -> { + return 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.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=DriverMode.SLOW;})); + } 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 Trigger(() -> {return arm.getForbFlag();}).onTrue(new InstantCommand(() -> {manipulatorController.setRumble(RumbleType.kBothRumble,rumbleFullPower);})) - // .onFalse(new InstantCommand(() -> {manipulatorController.setRumble(RumbleType.kBothRumble,rumbleNoPower);})); - 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, RollerMode.INTAKE_CONE))); - new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CONE))); - new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CUBE))); - new JoystickButton(manipulatorController, Manipulator.stopRollerButton).onTrue(new RunRoller(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, RollerMode.INTAKE_CUBE), - new RunRoller(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, RollerMode.INTAKE_CONE), - new RunRoller(roller, RollerMode.OUTTAKE_CONE), - isIntake - )); + .onTrue(new ConditionalCommand( + new RunRoller(roller, Roller.RollerMode.INTAKE_CONE), + new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE), + isIntake)); } @@ -196,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); } @@ -249,17 +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; + } + } } diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java b/src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java index a09b8cfb..07cde9ff 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/commands/ArmTeleop.java @@ -7,6 +7,7 @@ import static org.carlmontrobotics.robotcode2023.Constants.Arm.*; import java.util.function.DoubleSupplier; +import java.util.function.BooleanSupplier; import org.carlmontrobotics.robotcode2023.Constants; import org.carlmontrobotics.robotcode2023.subsystems.Arm; @@ -21,13 +22,15 @@ public class ArmTeleop extends CommandBase { private Arm armSubsystem; private DoubleSupplier arm; private DoubleSupplier wrist; + private BooleanSupplier baby; // is robot in baby (safe) mode? private double lastTime = 0; - public ArmTeleop(Arm armSubsystem, DoubleSupplier arm, DoubleSupplier wrist) { + public ArmTeleop(Arm armSubsystem, DoubleSupplier arm, DoubleSupplier wrist, BooleanSupplier baby) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(this.armSubsystem = armSubsystem); - this.arm = arm; - this.wrist = wrist; + this.baby = baby; + this.arm = () -> {return baby.getAsBoolean() ? 0.f : arm.getAsDouble();}; + this.wrist = () -> {return baby.getAsBoolean() ? 0.f : wrist.getAsDouble();}; } // Called when the command is initially scheduled. @@ -53,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); } @@ -67,20 +73,24 @@ public void execute() { // Copy and pasted from drivetrain, handles input from joysticks public double[] getRequestedSpeeds() { - double rawArmVel, rawWristVel; // 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; + if (Math.abs(arm.getAsDouble()) <= Constants.OI.JOY_THRESH) rawArmVel = 0.0; else - rawArmVel = MAX_FF_VEL_MANUAL[ARM] * arm.getAsDouble(); + rawArmVel = maxFF[ARM] * arm.getAsDouble(); if (Math.abs(wrist.getAsDouble()) <= Constants.OI.JOY_THRESH) rawWristVel = 0.0; else - rawWristVel = MAX_FF_VEL_MANUAL[WRIST] * wrist.getAsDouble(); + 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/commands/RunRoller.java b/src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java index be8d905c..def61edb 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java @@ -1,8 +1,7 @@ package org.carlmontrobotics.robotcode2023.commands; -import org.carlmontrobotics.robotcode2023.Constants.Roller.GameObject; -import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode; import org.carlmontrobotics.robotcode2023.subsystems.Roller; +import org.carlmontrobotics.robotcode2023.Constants.Roller.GameObject; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -12,9 +11,9 @@ public class RunRoller extends CommandBase { private final Roller roller; private final Timer timer = new Timer(); - private final RollerMode mode; + private final Roller.RollerMode mode; - public RunRoller(Roller roller, RollerMode mode) { + public RunRoller(Roller roller, Roller.RollerMode mode) { addRequirements(this.roller = roller); this.mode = mode; } @@ -24,8 +23,10 @@ public void initialize() { System.err.println("=============================RunRoller is Started============================="); if (mode.speed > 0) { // should not interrupt command to stop rollers - if(roller.hasGamePiece() && isIntake()) cancel(); - if(!roller.hasGamePiece() && !isIntake()) cancel(); + if (roller.hasGamePiece() && isIntake()) + cancel(); + if (!roller.hasGamePiece() && !isIntake()) + cancel(); } System.err.println("=============================RunRoller is initialized============================="); timer.reset(); @@ -33,22 +34,27 @@ public void initialize() { } @Override - public void execute() {} + public void execute() { + } @Override public void end(boolean interrupted) { // keep it running if its a cone - if (mode.obj != GameObject.CONE && !interrupted) - roller.setSpeed(0); + if (mode.obj != GameObject.CONE && !interrupted) { + mode.speed = 0; + roller.setRollerMode(mode); + } timer.stop(); - System.err.println("=============================RunRoller has ended=========================================================="); + System.err.println( + "=============================RunRoller has ended=========================================================="); } @Override public boolean isFinished() { - if(mode.speed == 0) return true; + if (mode.speed == 0) + return true; double time = timer.get(); diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/robotcode2023/commands/TeleopDrive.java index e4d16115..0c117ea0 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/commands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/commands/TeleopDrive.java @@ -8,9 +8,10 @@ package org.carlmontrobotics.robotcode2023.commands; import static org.carlmontrobotics.robotcode2023.Constants.Drivetrain.*; +import org.carlmontrobotics.robotcode2023.RobotContainer.DriverMode; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.carlmontrobotics.robotcode2023.Constants; import org.carlmontrobotics.robotcode2023.Robot; @@ -27,19 +28,19 @@ public class TeleopDrive extends CommandBase { private DoubleSupplier fwd; private DoubleSupplier str; private DoubleSupplier rcw; - private BooleanSupplier slow; + private Supplier mode; private double currentForwardVel = 0; private double currentStrafeVel = 0; /** * Creates a new TeleopDrive. */ - public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, BooleanSupplier slow) { + public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, Supplier mode) { addRequirements(this.drivetrain = drivetrain); this.fwd = fwd; this.str = str; this.rcw = rcw; - this.slow = slow; + this.mode = mode; } // Called when the command is initially scheduled. @@ -55,20 +56,30 @@ public void execute() { } public double[] getRequestedSpeeds() { - // Sets all values less than or equal to a very small value (determined by the idle joystick state) to zero. - // Used to make sure that the robot does not try to change its angle unless it is moving, + // Sets all values less than or equal to a very small value (determined by the + // idle joystick state) to zero. + // Used to make sure that the robot does not try to change its angle unless it + // is moving, double forward = fwd.getAsDouble(); double strafe = str.getAsDouble(); double rotateClockwise = rcw.getAsDouble(); - if (Math.abs(forward) <= Constants.OI.JOY_THRESH) forward = 0.0; - else forward *= maxForward; - if (Math.abs(strafe) <= Constants.OI.JOY_THRESH) strafe = 0.0; - else strafe *= maxStrafe; - if (Math.abs(rotateClockwise) <= Constants.OI.JOY_THRESH) rotateClockwise = 0.0; - else rotateClockwise *= maxRCW; - - double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed; - double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation; + if (Math.abs(forward) <= Constants.OI.JOY_THRESH) + forward = 0.0; + else + forward *= maxForward; + if (Math.abs(strafe) <= Constants.OI.JOY_THRESH) + strafe = 0.0; + else + strafe *= maxStrafe; + if (Math.abs(rotateClockwise) <= Constants.OI.JOY_THRESH) + rotateClockwise = 0.0; + else + rotateClockwise *= maxRCW; + + double driveMultiplier = ((DriverMode) mode.get()).isBaby() ? kBabyDriveSpeed + : (((DriverMode) mode.get()).isSlow() ? kSlowDriveSpeed : kNormalDriveSpeed); + double rotationMultiplier = ((DriverMode) mode.get()).isBaby() ? kBabyTurnSpeed + : (((DriverMode) mode.get()).isSlow() ? kSlowDriveRotation : kNormalDriveRotation); forward *= driveMultiplier; strafe *= driveMultiplier; @@ -78,8 +89,9 @@ public double[] getRequestedSpeeds() { double accelerationX = (forward - currentForwardVel) / robotPeriod; double accelerationY = (strafe - currentStrafeVel) / robotPeriod; double translationalAcceleration = Math.hypot(accelerationX, accelerationY); - if(translationalAcceleration > autoMaxAccelMps2) { - Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX))); + if (translationalAcceleration > autoMaxAccelMps2) { + Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, + Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX))); Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod); currentForwardVel += limitedVelocityVector.getX(); currentStrafeVel += limitedVelocityVector.getY(); @@ -90,13 +102,16 @@ public double[] getRequestedSpeeds() { // ATM, there is no rotational acceleration limit - // If the above math works, no velocity should be greater than the max velocity, so we don't need to limit it. + // If the above math works, no velocity should be greater than the max velocity, + // so we don't need to limit it. - return new double[] {currentForwardVel, currentStrafeVel, -rotateClockwise}; + return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise }; } public boolean hasDriverInput() { - return Math.abs(fwd.getAsDouble()) > Constants.OI.JOY_THRESH || Math.abs(str.getAsDouble()) > Constants.OI.JOY_THRESH || Math.abs(rcw.getAsDouble()) > Constants.OI.JOY_THRESH; + return Math.abs(fwd.getAsDouble()) > Constants.OI.JOY_THRESH + || Math.abs(str.getAsDouble()) > Constants.OI.JOY_THRESH + || Math.abs(rcw.getAsDouble()) > Constants.OI.JOY_THRESH; } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Arm.java b/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Arm.java index 32a4bab3..4e40b74a 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Arm.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Arm.java @@ -4,6 +4,7 @@ import org.carlmontrobotics.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.robotcode2023.RobotContainer; import org.carlmontrobotics.robotcode2023.Constants.GoalPos; import org.carlmontrobotics.robotcode2023.commands.ArmTeleop; @@ -11,6 +12,7 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxAbsoluteEncoder; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; @@ -30,8 +32,6 @@ // Arm angle is measured from horizontal on the intake side of the robot and bounded between -3π/2 and π/2 // Wrist angle is measured relative to the arm with 0 being parallel to the arm and bounded between -π and π (Center of Mass of Roller) public class Arm extends SubsystemBase { - // a boolean meant to tell if the arm is in a forbidden posistion AKA FORBIDDEN FLAG - private static boolean forbFlag; private final CANSparkMax armMotor = MotorControllerFactory.createSparkMax(armMotorPort, MotorConfig.NEO); private final CANSparkMax wristMotor = MotorControllerFactory.createSparkMax(wristMotorPort, MotorConfig.NEO); private final RelativeEncoder armRelEncoder = armMotor.getEncoder(); @@ -47,14 +47,25 @@ public class Arm extends SubsystemBase { private final PIDController wristPID = new PIDController(kP[WRIST], kI[WRIST], kD[WRIST]); private TrapezoidProfile armProfile = new TrapezoidProfile(armConstraints, goalState[ARM], getCurrentArmState()); - private TrapezoidProfile wristProfile = new TrapezoidProfile(wristConstraints, goalState[WRIST], getCurrentWristState()); + private TrapezoidProfile wristProfile = new TrapezoidProfile(wristConstraints, goalState[WRIST], + getCurrentWristState()); private Timer armProfileTimer = new Timer(); private Timer wristProfileTimer = new Timer(); // rad, rad/s - public static TrapezoidProfile.State[] goalState = { new TrapezoidProfile.State(-Math.PI / 2, 0), new TrapezoidProfile.State(0, 0) }; + public static TrapezoidProfile.State[] goalState = { new TrapezoidProfile.State(-Math.PI / 2, 0), + new TrapezoidProfile.State(0, 0) }; + + private double lastWristPos; + private double lastArmPos; + private double lastMeasuredArmTime; + private double lastMeasuredWristTime; + private boolean isArmEncoderConnected = false; + private boolean isWristEncoderConnected = false; public Arm() { + armMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20); + wristMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20); armMotor.setInverted(motorInverted[ARM]); wristMotor.setInverted(motorInverted[WRIST]); armMotor.setIdleMode(IdleMode.kBrake); @@ -81,84 +92,133 @@ public Arm() { setArmTarget(goalState[ARM].position, 0); setWristTarget(goalState[WRIST].position, 0); wristMotor.setSmartCurrentLimit(WRIST_CURRENT_LIMIT_AMP); + lastArmPos = getArmPos(); + lastWristPos = getWristPos(); + lastMeasuredArmTime = Timer.getFPGATimestamp(); + lastMeasuredWristTime = Timer.getFPGATimestamp(); // SmartDashboard.putNumber("Arm Max Vel", MAX_FF_VEL[ARM]); // SmartDashboard.putNumber("Wrist Max Vel", MAX_FF_VEL[WRIST]); - SmartDashboard.putNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); - SmartDashboard.putNumber("Arm Tolerance Pos", posToleranceRad[ARM]); - SmartDashboard.putNumber("Wrist Tolerance Pos", posToleranceRad[WRIST]); - SmartDashboard.putNumber("Arm Tolerance Vel", velToleranceRadPSec[ARM]); - SmartDashboard.putNumber("Wrist Tolerance Vel", velToleranceRadPSec[WRIST]); + // SmartDashboard.putNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", + // ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); + // SmartDashboard.putNumber("Arm Tolerance Pos", posToleranceRad[ARM]); + // SmartDashboard.putNumber("Wrist Tolerance Pos", posToleranceRad[WRIST]); + // SmartDashboard.putNumber("Arm Tolerance Vel", velToleranceRadPSec[ARM]); + // SmartDashboard.putNumber("Wrist Tolerance Vel", velToleranceRadPSec[WRIST]); } @Override public void periodic() { - if(DriverStation.isDisabled()) resetGoal(); + if (DriverStation.isDisabled()) + resetGoal(); - // TODO: REMOVE THIS WHEN PID CONSTANTS ARE DONE - // MAX_FF_VEL[ARM] = SmartDashboard.getNumber("Arm Max Vel", MAX_FF_VEL[ARM]); - // MAX_FF_VEL[WRIST] = SmartDashboard.getNumber("Wrist Max Vel", MAX_FF_VEL[WRIST]); - ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = SmartDashboard.getNumber("ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD", ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD); - // wristConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL[WRIST], MAX_FF_ACCEL[WRIST]); - // armConstraints = new TrapezoidProfile.Constraints(MAX_FF_VEL[ARM], MAX_FF_ACCEL[ARM]); + // TODO: is RobotContainer live or do you need supplier functions + armConstraints = new TrapezoidProfile.Constraints( + (RobotContainer.driverMode.isBaby()) ? MAX_FF_VEL_BABY[0] : MAX_FF_VEL_AUTO[0], + (RobotContainer.driverMode.isBaby()) ? MAX_FF_ACCEL_BABY[0] : MAX_FF_ACCEL[0]); + wristConstraints = new TrapezoidProfile.Constraints( + (RobotContainer.driverMode.isBaby()) ? MAX_FF_VEL_BABY[1] : MAX_FF_VEL_AUTO[1], + (RobotContainer.driverMode.isBaby()) ? MAX_FF_ACCEL_BABY[1] : MAX_FF_ACCEL[1]); armPID.setP(kP[ARM]); armPID.setI(kI[ARM]); armPID.setD(kD[ARM]); wristPID.setP(kP[WRIST]); wristPID.setI(kI[WRIST]); wristPID.setD(kD[WRIST]); - SmartDashboard.putBoolean("ArmPIDAtSetpoint", armPID.atSetpoint()); - SmartDashboard.putBoolean("ArmProfileFinished", armProfile.isFinished(armProfileTimer.get())); - SmartDashboard.putBoolean("WristPIDAtSetpoint", wristPID.atSetpoint()); - SmartDashboard.putBoolean("WristProfileFinished", wristProfile.isFinished(wristProfileTimer.get())); - posToleranceRad[ARM] = SmartDashboard.getNumber("Arm Tolerance Pos", posToleranceRad[ARM]); - posToleranceRad[WRIST] = SmartDashboard.getNumber("Wrist Tolerance Pos", posToleranceRad[WRIST]); - velToleranceRadPSec[ARM] = SmartDashboard.getNumber("Arm Tolerance Vel", velToleranceRadPSec[ARM]); - velToleranceRadPSec[WRIST] = SmartDashboard.getNumber("Wrist Tolerance Vel", velToleranceRadPSec[WRIST]); - - SmartDashboard.putNumber("MaxHoldingTorque", maxHoldingTorqueNM()); - SmartDashboard.putNumber("V_PER_NM", getV_PER_NM()); - SmartDashboard.putNumber("COMDistance", getCoM().getNorm()); - SmartDashboard.putNumber("InternalArmVelocity", armRelEncoder.getVelocity()); - SmartDashboard.putNumber("Arm Current", armMotor.getOutputCurrent()); - SmartDashboard.putNumber("Wrist Current", wristMotor.getOutputCurrent()); + // SmartDashboard.putBoolean("ArmPIDAtSetpoint", armPID.atSetpoint()); + // SmartDashboard.putBoolean("ArmProfileFinished", + // armProfile.isFinished(armProfileTimer.get())); + // SmartDashboard.putBoolean("WristPIDAtSetpoint", wristPID.atSetpoint()); + // SmartDashboard.putBoolean("WristProfileFinished", + // wristProfile.isFinished(wristProfileTimer.get())); + // posToleranceRad[ARM] = SmartDashboard.getNumber("Arm Tolerance Pos", + // posToleranceRad[ARM]); + // posToleranceRad[WRIST] = SmartDashboard.getNumber("Wrist Tolerance Pos", + // posToleranceRad[WRIST]); + // velToleranceRadPSec[ARM] = SmartDashboard.getNumber("Arm Tolerance Vel", + // velToleranceRadPSec[ARM]); + // velToleranceRadPSec[WRIST] = SmartDashboard.getNumber("Wrist Tolerance Vel", + // velToleranceRadPSec[WRIST]); + + // SmartDashboard.putNumber("MaxHoldingTorque", maxHoldingTorqueNM()); + // SmartDashboard.putNumber("V_PER_NM", getV_PER_NM()); + // SmartDashboard.putNumber("COMDistance", getCoM().getNorm()); + // SmartDashboard.putNumber("InternalArmVelocity", armRelEncoder.getVelocity()); + // SmartDashboard.putNumber("Arm Current", armMotor.getOutputCurrent()); + // SmartDashboard.putNumber("Wrist Current", wristMotor.getOutputCurrent()); SmartDashboard.putNumber("ArmPos", getArmPos()); SmartDashboard.putNumber("WristPos", getWristPos()); - driveArm(armProfile.calculate(armProfileTimer.get())); - driveWrist(wristProfile.calculate(wristProfileTimer.get())); + /* + * The code below detects whether an encoder is disconnected. When it has detected that the encoder + * is disconnected, it will stop the motors. + * The way it detects an encoder is connected is if the value the encoder returns does not change + * for DISCONNECTED_ENCODER_TIMEOUT_SECs. Even when the arm stays still, there is a lot of "noise" + * in the encoder. We can reliably say that the encoder is disconnected if it remains the exact same. + */ + double currTime = Timer.getFPGATimestamp(); + SmartDashboard.putNumber("Current Time", currTime); + SmartDashboard.putNumber("Last Arm Update (s)", lastMeasuredArmTime); + + double currentArmPos = getArmPos(); + if (currentArmPos != lastArmPos) { + lastMeasuredArmTime = currTime; + lastArmPos = currentArmPos; + } + double currentWristPos = getWristPos(); + if (currentWristPos != lastWristPos) { + lastMeasuredWristTime = currTime; + lastWristPos = currentWristPos; + } + isArmEncoderConnected = (currTime - lastMeasuredArmTime) < DISCONNECTED_ENCODER_TIMEOUT_SEC; + SmartDashboard.putBoolean("ArmEncoderConnected", isArmEncoderConnected); + + isWristEncoderConnected = (currTime - lastMeasuredWristTime) < DISCONNECTED_ENCODER_TIMEOUT_SEC; + SmartDashboard.putBoolean("WristEncoderConnected", isWristEncoderConnected); + + if (isArmEncoderConnected) + driveArm(armProfile.calculate(armProfileTimer.get())); + else { + armMotor.setVoltage(0); + SmartDashboard.putNumber("ArmTotalVolts", 0); + } + + if (isWristEncoderConnected) + driveWrist(wristProfile.calculate(wristProfileTimer.get())); + else { + wristMotor.setVoltage(0); + SmartDashboard.putNumber("WristTotalVolts", 0); + } autoCancelArmCommand(); } public void autoCancelArmCommand() { - if(!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) return; + if (!(getDefaultCommand() instanceof ArmTeleop) || DriverStation.isAutonomous()) + return; double[] requestedSpeeds = ((ArmTeleop) getDefaultCommand()).getRequestedSpeeds(); - if(requestedSpeeds[0] != 0 || requestedSpeeds[1] != 0) { + if (requestedSpeeds[0] != 0 || requestedSpeeds[1] != 0) { Command currentArmCommand = getCurrentCommand(); - if(currentArmCommand != getDefaultCommand() && currentArmCommand != null) { + if (currentArmCommand != getDefaultCommand() && currentArmCommand != null) { currentArmCommand.cancel(); } } } - //#region Drive Methods + // #region Drive Methods private void driveArm(TrapezoidProfile.State state) { double kgv = getKg(); double armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(state.velocity, 0); double armPIDVolts = armPID.calculate(getArmPos(), state.position); - if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) || - (getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) { - forbFlag = true; + if ((getArmPos() > ARM_UPPER_LIMIT_RAD && state.velocity > 0) || + (getArmPos() < ARM_LOWER_LIMIT_RAD && state.velocity < 0)) { armFeedVolts = kgv * getCoM().getAngle().getCos() + armFeed.calculate(0, 0); } - - // TODO: REMOVE WHEN DONE WITH TESTING (ANY CODE REVIEWERS, PLEASE REJECT MERGES // TO MASTER IF THIS IS STILL HERE) SmartDashboard.putNumber("ArmFeedVolts", armFeedVolts); @@ -172,12 +232,11 @@ private void driveWrist(TrapezoidProfile.State state) { double kgv = wristFeed.calculate(getWristPosRelativeToGround(), state.velocity, 0); double wristFeedVolts = wristFeed.calculate(getWristPosRelativeToGround(), state.velocity, 0); double wristPIDVolts = wristPID.calculate(getWristPos(), state.position); - if ((getWristPos() > WRIST_UPPER_LIMIT_RAD && state.velocity > 0) || - (getWristPos() < WRIST_LOWER_LIMIT_RAD && state.velocity < 0)) { - forbFlag = true; + if ((getWristPos() > WRIST_UPPER_LIMIT_RAD && state.velocity > 0) || + (getWristPos() < WRIST_LOWER_LIMIT_RAD && state.velocity < 0)) { wristFeedVolts = kgv; } - + // TODO: REMOVE WHEN DONE WITH TESTING (ANY CODE REVIEWERS, PLEASE REJECT MERGES // TO MASTER IF THIS IS STILL HERE) SmartDashboard.putNumber("WristFeedVolts", wristFeedVolts); @@ -190,14 +249,12 @@ private void driveWrist(TrapezoidProfile.State state) { public void setArmTarget(double targetPos, double targetVel) { targetPos = getArmClampedGoal(targetPos); - if(positionForbidden(targetPos, getWristPos())) - { - forbFlag = true; + if (positionForbidden(targetPos, getWristPos())) { return; - } - + } - armProfile = new TrapezoidProfile(armConstraints, new TrapezoidProfile.State(targetPos, targetVel), armProfile.calculate(armProfileTimer.get())); + armProfile = new TrapezoidProfile(armConstraints, new TrapezoidProfile.State(targetPos, targetVel), + armProfile.calculate(armProfileTimer.get())); armProfileTimer.reset(); goalState[ARM].position = targetPos; @@ -207,12 +264,11 @@ public void setArmTarget(double targetPos, double targetVel) { public void setWristTarget(double targetPos, double targetVel) { targetPos = getWristClampedGoal(targetPos); - if(wristMovementForbidden(getArmPos(), targetPos, targetPos - getWristPos())) - { - forbFlag = true; + if (wristMovementForbidden(getArmPos(), targetPos, targetPos - getWristPos())) { return; } - wristProfile = new TrapezoidProfile(wristConstraints, new TrapezoidProfile.State(targetPos, targetVel), wristProfile.calculate(wristProfileTimer.get())); + wristProfile = new TrapezoidProfile(wristConstraints, new TrapezoidProfile.State(targetPos, targetVel), + wristProfile.calculate(wristProfileTimer.get())); wristProfileTimer.reset(); goalState[WRIST].position = targetPos; @@ -228,9 +284,9 @@ public void resetGoal() { wristProfile = new TrapezoidProfile(wristConstraints, goalState[WRIST], goalState[WRIST]); } - //#endregion + // #endregion - //#region Getters + // #region Getters public double getArmPos() { return MathUtil.inputModulus(armEncoder.getPosition(), ARM_DISCONTINUITY_RAD, @@ -279,16 +335,19 @@ public boolean wristAtSetpoint() { return wristPID.atSetpoint() && wristProfile.isFinished(wristProfileTimer.get()); } - //#endregion + // #endregion - //#region Util Methods + // #region Util Methods public double getArmClampedGoal(double goal) { - return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONTINUITY_RAD, ARM_DISCONTINUITY_RAD + 2 * Math.PI), ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD); + return MathUtil.clamp(MathUtil.inputModulus(goal, ARM_DISCONTINUITY_RAD, ARM_DISCONTINUITY_RAD + 2 * Math.PI), + ARM_LOWER_LIMIT_RAD, ARM_UPPER_LIMIT_RAD); } public double getWristClampedGoal(double goal) { - return MathUtil.clamp(MathUtil.inputModulus(goal, WRIST_DISCONTINUITY_RAD, WRIST_DISCONTINUITY_RAD + 2 * Math.PI), WRIST_LOWER_LIMIT_RAD, WRIST_UPPER_LIMIT_RAD); + return MathUtil.clamp( + MathUtil.inputModulus(goal, WRIST_DISCONTINUITY_RAD, WRIST_DISCONTINUITY_RAD + 2 * Math.PI), + WRIST_LOWER_LIMIT_RAD, WRIST_UPPER_LIMIT_RAD); } public Translation2d getCoM() { @@ -315,7 +374,8 @@ public static double getV_PER_NM() { double PaRa = COM_ARM_LENGTH_METERS; double g = 9.80; - double c = (kg) / (g * Math.sqrt(Math.pow(Ma * PaRa + Mr * Ra, 2) + Math.pow(Mr * Rr, 2) + 2 * (Ma * PaRa + Mr * Ra) * (Mr * Rr) * Math.cos(phi))); + double c = (kg) / (g * Math.sqrt(Math.pow(Ma * PaRa + Mr * Ra, 2) + Math.pow(Mr * Rr, 2) + + 2 * (Ma * PaRa + Mr * Ra) * (Mr * Rr) * Math.cos(phi))); return c; } @@ -325,14 +385,15 @@ public double getKg() { public static Translation2d getWristTipPosition(double armPos, double wristPos) { Translation2d arm = new Translation2d(ARM_LENGTH_METERS, Rotation2d.fromRadians(armPos)); - Translation2d roller = new Translation2d(ROLLER_LENGTH_METERS, Rotation2d.fromRadians(armPos + wristPos + ROLLER_COM_CORRECTION_RAD)); + Translation2d roller = new Translation2d(ROLLER_LENGTH_METERS, + Rotation2d.fromRadians(armPos + wristPos + ROLLER_COM_CORRECTION_RAD)); return arm.plus(roller); } public boolean wristMovementForbidden(double armPos, double wristPos, double wristVelSign) { // If the position is not forbidden, then the movement is not forbidden - + if (true) return false; Translation2d tip = getWristTipPosition(armPos, wristPos); @@ -342,31 +403,23 @@ public boolean wristMovementForbidden(double armPos, double wristPos, double wri boolean vertical = tip.getY() > -ARM_JOINT_TOTAL_HEIGHT && tip.getY() < (-ARM_JOINT_TOTAL_HEIGHT + SAFE_HEIGHT); // If the wrist is inside the drivetrain, fold towards the - if(horizontal && vertical) { + if (horizontal && vertical) { return Math.signum(getWristPos() + ROLLER_COM_CORRECTION_RAD) != Math.signum(wristVelSign); } - - // Otherwise, fold away from vertical - double tipAngle = MathUtil.inputModulus(armPos + wristPos + ROLLER_COM_CORRECTION_RAD, -3 * Math.PI / 2, Math.PI / 2); + double tipAngle = MathUtil.inputModulus(armPos + wristPos + ROLLER_COM_CORRECTION_RAD, -3 * Math.PI / 2, + Math.PI / 2); return Math.signum(tipAngle + Math.PI / 2) != Math.signum(wristVelSign); } - - public boolean getForbFlag() - { - boolean output = forbFlag; - forbFlag = false;//default: if it wasn't set to true, it's false - return output; - } - public static boolean positionForbidden(double armPos, double wristPos) { if (true) return false; Translation2d tip = getWristTipPosition(armPos, wristPos); - boolean horizontal = tip.getX() < DT_TOTAL_WIDTH / 2 + DT_EXTENSION_FOR_ROLLER && tip.getX() > -DT_TOTAL_WIDTH / 2; + boolean horizontal = tip.getX() < DT_TOTAL_WIDTH / 2 + DT_EXTENSION_FOR_ROLLER + && tip.getX() > -DT_TOTAL_WIDTH / 2; boolean vertical = tip.getY() > -ARM_JOINT_TOTAL_HEIGHT && tip.getY() < (-ARM_JOINT_TOTAL_HEIGHT + SAFE_HEIGHT); boolean ground = tip.getY() < -ARM_JOINT_TOTAL_HEIGHT; @@ -375,71 +428,102 @@ public static boolean positionForbidden(double armPos, double wristPos) { public static boolean isWristOutsideRobot(double armPos, double wristPos) { Translation2d wristTip = Arm.getWristTipPosition(armPos, wristPos); - double driveTrainHalfLen = Units.inchesToMeters(31)/2; + double driveTrainHalfLen = Units.inchesToMeters(31) / 2; - return Math.abs(wristTip.getX())>driveTrainHalfLen; - //returns true if wristTip is outside of robot vertical bounds. + return Math.abs(wristTip.getX()) > driveTrainHalfLen; + // returns true if wristTip is outside of robot vertical bounds. } public static boolean canSafelyMoveWrist(double armPos) { return Math.abs(armPos - ARM_VERTICAL_POS_RAD) >= MIN_WRIST_FOLD_POS_RAD; } - //#endregion + // #endregion - //#region SmartDashboard Methods + // #region SmartDashboard Methods @Override public void initSendable(SendableBuilder builder) { super.initSendable(builder); - builder.addDoubleProperty("WristGoalPos", () -> goalState[WRIST].position, goal -> goalState[WRIST].position = goal); - builder.addDoubleProperty("ArmGoalPos", () -> goalState[ARM].position, goal -> goalState[ARM].position = goal); - builder.addDoubleProperty("ArmPos", () -> getArmPos(), null); - builder.addDoubleProperty("WristPos", () -> getWristPos(), null); - builder.addDoubleProperty("RobotWristPos", () -> getWristPosRelativeToGround(), null); + builder.addDoubleProperty("WristGoalPos", () -> goalState[WRIST].position, + goal -> goalState[WRIST].position = goal); + builder.addDoubleProperty("ArmGoalPos", () -> goalState[ARM].position, goal -> goalState[ARM].position = goal); + builder.addDoubleProperty("ArmPos", () -> getArmPos(), null); + builder.addDoubleProperty("WristPos", () -> getWristPos(), null); + builder.addDoubleProperty("RobotWristPos", () -> getWristPosRelativeToGround(), null); builder.addDoubleProperty("kP: Arm", () -> kP[ARM], x -> kP[ARM] = x); builder.addDoubleProperty("kP: Wis", () -> kP[WRIST], x -> kP[WRIST] = x); builder.addDoubleProperty("kI: Arm", () -> kI[ARM], x -> kI[ARM] = x); builder.addDoubleProperty("kI: Wis", () -> kI[WRIST], x -> kI[WRIST] = x); builder.addDoubleProperty("kD: Arm", () -> kD[ARM], x -> kD[ARM] = x); builder.addDoubleProperty("kD: Wis", () -> kD[WRIST], x -> kD[WRIST] = x); - //arm control - builder.addDoubleProperty("Max Manual FF Vel Arm", () -> MAX_FF_VEL_MANUAL[ARM], x -> MAX_FF_VEL_MANUAL[ARM] = x); - builder.addDoubleProperty("Max Auto FF Vel Arm", () -> MAX_FF_VEL_AUTO[ARM], x -> MAX_FF_VEL_AUTO[ARM] = x); - builder.addDoubleProperty("Max FF Acc Arm", () -> MAX_FF_ACCEL[ARM], x -> MAX_FF_ACCEL[ARM] = x); - builder.addDoubleProperty("Max Manual FF Vel Wis", () -> MAX_FF_VEL_MANUAL[WRIST], x -> MAX_FF_VEL_MANUAL[WRIST] = x); - builder.addDoubleProperty("Max Auto FF Vel Wis", () -> MAX_FF_VEL_AUTO[WRIST], x -> MAX_FF_VEL_AUTO[WRIST] = x); + // arm control + builder.addDoubleProperty("Max Manual FF Vel Arm", () -> MAX_FF_VEL_MANUAL[ARM], + x -> MAX_FF_VEL_MANUAL[ARM] = x); + builder.addDoubleProperty("Max Auto FF Vel Arm", () -> MAX_FF_VEL_AUTO[ARM], x -> MAX_FF_VEL_AUTO[ARM] = x); + builder.addDoubleProperty("Max FF Acc Arm", () -> MAX_FF_ACCEL[ARM], x -> MAX_FF_ACCEL[ARM] = x); + builder.addDoubleProperty("Max Manual FF Vel Wis", () -> MAX_FF_VEL_MANUAL[WRIST], + x -> MAX_FF_VEL_MANUAL[WRIST] = x); + builder.addDoubleProperty("Max Auto FF Vel Wis", () -> MAX_FF_VEL_AUTO[WRIST], x -> MAX_FF_VEL_AUTO[WRIST] = x); builder.addDoubleProperty("Max FF Acc Wis", () -> MAX_FF_ACCEL[WRIST], x -> MAX_FF_ACCEL[WRIST] = x); - //arm positions - builder.addDoubleProperty("Arm.Back.High.Cone", () -> GoalPos.HIGH[BACK][CONE].armPos, x -> GoalPos.HIGH[BACK][CONE].armPos = x); - builder.addDoubleProperty("Arm.Back.Mid.Cone", () -> GoalPos.MID[BACK][CONE].armPos, x -> GoalPos.MID[BACK][CONE].armPos = x); - builder.addDoubleProperty("Arm.Back.Low.Cone", () -> GoalPos.LOW[BACK][CONE].armPos, x -> GoalPos.LOW[BACK][CONE].armPos = x); - builder.addDoubleProperty("Arm.Back.Store.Cone", () -> GoalPos.STORED[BACK][CONE].armPos, x -> GoalPos.STORED[BACK][CONE].armPos = x); - builder.addDoubleProperty("Arm.Back.Shelf.Cone", () -> GoalPos.SHELF[BACK][CONE].armPos, x -> GoalPos.SHELF[BACK][CONE].armPos = x); - builder.addDoubleProperty("Arm.Back.Subst.Cone", () -> GoalPos.SUBSTATION[BACK][CONE].armPos, x -> GoalPos.SUBSTATION[BACK][CONE].armPos = x); - builder.addDoubleProperty("Arm.Back.Intak.Cone", () -> GoalPos.INTAKE[BACK][CONE].armPos, x -> GoalPos.INTAKE[BACK][CONE].armPos = x); - builder.addDoubleProperty("Arm.Back.High.Cube", () -> GoalPos.HIGH[BACK][CUBE].armPos, x -> GoalPos.HIGH[BACK][CUBE].armPos = x); - builder.addDoubleProperty("Arm.Back.Mid.Cube", () -> GoalPos.MID[BACK][CUBE].armPos, x -> GoalPos.MID[BACK][CUBE].armPos = x); - builder.addDoubleProperty("Arm.Back.Low.Cube", () -> GoalPos.LOW[BACK][CUBE].armPos, x -> GoalPos.LOW[BACK][CUBE].armPos = x); - builder.addDoubleProperty("Arm.Back.Store.Cube", () -> GoalPos.STORED[BACK][CUBE].armPos, x -> GoalPos.STORED[BACK][CUBE].armPos = x); - builder.addDoubleProperty("Arm.Back.Shelf.Cube", () -> GoalPos.SHELF[BACK][CUBE].armPos, x -> GoalPos.SHELF[BACK][CUBE].armPos = x); - builder.addDoubleProperty("Arm.Back.Subst.Cube", () -> GoalPos.SUBSTATION[BACK][CUBE].armPos, x -> GoalPos.SUBSTATION[BACK][CUBE].armPos = x); - builder.addDoubleProperty("Arm.Back.Intak.Cube", () -> GoalPos.INTAKE[BACK][CUBE].armPos, x -> GoalPos.INTAKE[BACK][CUBE].armPos = x); - //wrist positions - builder.addDoubleProperty("Wrist.Back.High.Cone", () -> GoalPos.HIGH[BACK][CONE].wristPos, x -> GoalPos.HIGH[BACK][CONE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Mid.Cone", () -> GoalPos.MID[BACK][CONE].wristPos, x -> GoalPos.MID[BACK][CONE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Low.Cone", () -> GoalPos.LOW[BACK][CONE].wristPos, x -> GoalPos.LOW[BACK][CONE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Store.Cone", () -> GoalPos.STORED[BACK][CONE].wristPos, x -> GoalPos.STORED[BACK][CONE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Shelf.Cone", () -> GoalPos.SHELF[BACK][CONE].wristPos, x -> GoalPos.SHELF[BACK][CONE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Substation.Cone", () -> GoalPos.SUBSTATION[BACK][CONE].wristPos, x -> GoalPos.SUBSTATION[BACK][CONE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Intake.Cone", () -> GoalPos.INTAKE[BACK][CONE].wristPos, x -> GoalPos.INTAKE[BACK][CONE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.High.Cube", () -> GoalPos.HIGH[BACK][CUBE].wristPos, x -> GoalPos.HIGH[BACK][CUBE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Mid.Cube", () -> GoalPos.MID[BACK][CUBE].wristPos, x -> GoalPos.MID[BACK][CUBE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Low.Cube", () -> GoalPos.LOW[BACK][CUBE].wristPos, x -> GoalPos.LOW[BACK][CUBE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Store.Cube", () -> GoalPos.STORED[BACK][CUBE].wristPos, x -> GoalPos.STORED[BACK][CUBE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Shelf.Cube", () -> GoalPos.SHELF[BACK][CUBE].wristPos, x -> GoalPos.SHELF[BACK][CUBE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Substation.Cube", () -> GoalPos.SUBSTATION[BACK][CUBE].wristPos, x -> GoalPos.SUBSTATION[BACK][CUBE].wristPos = x); - builder.addDoubleProperty("Wrist.Back.Intake.Cube", () -> GoalPos.INTAKE[BACK][CUBE].wristPos, x -> GoalPos.INTAKE[BACK][CUBE].wristPos = x); + // arm positions + builder.addDoubleProperty("Arm.Back.High.Cone", () -> GoalPos.HIGH[BACK][CONE].armPos, + x -> GoalPos.HIGH[BACK][CONE].armPos = x); + builder.addDoubleProperty("Arm.Back.Mid.Cone", () -> GoalPos.MID[BACK][CONE].armPos, + x -> GoalPos.MID[BACK][CONE].armPos = x); + builder.addDoubleProperty("Arm.Back.Low.Cone", () -> GoalPos.LOW[BACK][CONE].armPos, + x -> GoalPos.LOW[BACK][CONE].armPos = x); + builder.addDoubleProperty("Arm.Back.Store.Cone", () -> GoalPos.STORED[BACK][CONE].armPos, + x -> GoalPos.STORED[BACK][CONE].armPos = x); + builder.addDoubleProperty("Arm.Back.Shelf.Cone", () -> GoalPos.SHELF[BACK][CONE].armPos, + x -> GoalPos.SHELF[BACK][CONE].armPos = x); + builder.addDoubleProperty("Arm.Back.Subst.Cone", () -> GoalPos.SUBSTATION[BACK][CONE].armPos, + x -> GoalPos.SUBSTATION[BACK][CONE].armPos = x); + builder.addDoubleProperty("Arm.Back.Intak.Cone", () -> GoalPos.INTAKE[BACK][CONE].armPos, + x -> GoalPos.INTAKE[BACK][CONE].armPos = x); + builder.addDoubleProperty("Arm.Back.High.Cube", () -> GoalPos.HIGH[BACK][CUBE].armPos, + x -> GoalPos.HIGH[BACK][CUBE].armPos = x); + builder.addDoubleProperty("Arm.Back.Mid.Cube", () -> GoalPos.MID[BACK][CUBE].armPos, + x -> GoalPos.MID[BACK][CUBE].armPos = x); + builder.addDoubleProperty("Arm.Back.Low.Cube", () -> GoalPos.LOW[BACK][CUBE].armPos, + x -> GoalPos.LOW[BACK][CUBE].armPos = x); + builder.addDoubleProperty("Arm.Back.Store.Cube", () -> GoalPos.STORED[BACK][CUBE].armPos, + x -> GoalPos.STORED[BACK][CUBE].armPos = x); + builder.addDoubleProperty("Arm.Back.Shelf.Cube", () -> GoalPos.SHELF[BACK][CUBE].armPos, + x -> GoalPos.SHELF[BACK][CUBE].armPos = x); + builder.addDoubleProperty("Arm.Back.Subst.Cube", () -> GoalPos.SUBSTATION[BACK][CUBE].armPos, + x -> GoalPos.SUBSTATION[BACK][CUBE].armPos = x); + builder.addDoubleProperty("Arm.Back.Intak.Cube", () -> GoalPos.INTAKE[BACK][CUBE].armPos, + x -> GoalPos.INTAKE[BACK][CUBE].armPos = x); + // wrist positions + builder.addDoubleProperty("Wrist.Back.High.Cone", () -> GoalPos.HIGH[BACK][CONE].wristPos, + x -> GoalPos.HIGH[BACK][CONE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Mid.Cone", () -> GoalPos.MID[BACK][CONE].wristPos, + x -> GoalPos.MID[BACK][CONE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Low.Cone", () -> GoalPos.LOW[BACK][CONE].wristPos, + x -> GoalPos.LOW[BACK][CONE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Store.Cone", () -> GoalPos.STORED[BACK][CONE].wristPos, + x -> GoalPos.STORED[BACK][CONE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Shelf.Cone", () -> GoalPos.SHELF[BACK][CONE].wristPos, + x -> GoalPos.SHELF[BACK][CONE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Substation.Cone", () -> GoalPos.SUBSTATION[BACK][CONE].wristPos, + x -> GoalPos.SUBSTATION[BACK][CONE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Intake.Cone", () -> GoalPos.INTAKE[BACK][CONE].wristPos, + x -> GoalPos.INTAKE[BACK][CONE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.High.Cube", () -> GoalPos.HIGH[BACK][CUBE].wristPos, + x -> GoalPos.HIGH[BACK][CUBE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Mid.Cube", () -> GoalPos.MID[BACK][CUBE].wristPos, + x -> GoalPos.MID[BACK][CUBE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Low.Cube", () -> GoalPos.LOW[BACK][CUBE].wristPos, + x -> GoalPos.LOW[BACK][CUBE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Store.Cube", () -> GoalPos.STORED[BACK][CUBE].wristPos, + x -> GoalPos.STORED[BACK][CUBE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Shelf.Cube", () -> GoalPos.SHELF[BACK][CUBE].wristPos, + x -> GoalPos.SHELF[BACK][CUBE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Substation.Cube", () -> GoalPos.SUBSTATION[BACK][CUBE].wristPos, + x -> GoalPos.SUBSTATION[BACK][CUBE].wristPos = x); + builder.addDoubleProperty("Wrist.Back.Intake.Cube", () -> GoalPos.INTAKE[BACK][CUBE].wristPos, + x -> GoalPos.INTAKE[BACK][CUBE].wristPos = x); } // In the scenario that initSendable method does not work like last time @@ -480,33 +564,45 @@ public void getPositionsOnSmartDashboard() { GoalPos.MID[BACK][CUBE].armPos = SmartDashboard.getNumber("MidArmCube", GoalPos.MID[BACK][CUBE].armPos); GoalPos.MID[BACK][CUBE].wristPos = SmartDashboard.getNumber("MidWristCube", GoalPos.MID[BACK][CUBE].wristPos); GoalPos.HIGH[BACK][CUBE].armPos = SmartDashboard.getNumber("HighArmCube", GoalPos.HIGH[BACK][CUBE].armPos); - GoalPos.HIGH[BACK][CUBE].wristPos = SmartDashboard.getNumber("HighWristCube", GoalPos.HIGH[BACK][CUBE].wristPos); - GoalPos.STORED[BACK][CUBE].armPos = SmartDashboard.getNumber("StoredArmCube", GoalPos.STORED[BACK][CUBE].armPos); - GoalPos.STORED[BACK][CUBE].wristPos = SmartDashboard.getNumber("StoredWristCube", GoalPos.STORED[BACK][CUBE].wristPos); + GoalPos.HIGH[BACK][CUBE].wristPos = SmartDashboard.getNumber("HighWristCube", + GoalPos.HIGH[BACK][CUBE].wristPos); + GoalPos.STORED[BACK][CUBE].armPos = SmartDashboard.getNumber("StoredArmCube", + GoalPos.STORED[BACK][CUBE].armPos); + GoalPos.STORED[BACK][CUBE].wristPos = SmartDashboard.getNumber("StoredWristCube", + GoalPos.STORED[BACK][CUBE].wristPos); GoalPos.SHELF[BACK][CUBE].armPos = SmartDashboard.getNumber("ShelfArmCube", GoalPos.SHELF[BACK][CUBE].armPos); - GoalPos.SHELF[BACK][CUBE].wristPos = SmartDashboard.getNumber("ShelfWristCube", GoalPos.SHELF[BACK][CUBE].wristPos); + GoalPos.SHELF[BACK][CUBE].wristPos = SmartDashboard.getNumber("ShelfWristCube", + GoalPos.SHELF[BACK][CUBE].wristPos); GoalPos.SUBSTATION[BACK][CUBE].armPos = SmartDashboard.getNumber("SubstationArmCube", GoalPos.SUBSTATION[BACK][CUBE].armPos); GoalPos.SUBSTATION[BACK][CUBE].wristPos = SmartDashboard.getNumber("SubstationWristCube", GoalPos.SUBSTATION[BACK][CUBE].wristPos); - GoalPos.INTAKE[BACK][CUBE].armPos = SmartDashboard.getNumber("IntakeArmCube", GoalPos.INTAKE[BACK][CUBE].armPos); - GoalPos.INTAKE[BACK][CUBE].wristPos = SmartDashboard.getNumber("IntakeWristCube", GoalPos.INTAKE[BACK][CUBE].wristPos); + GoalPos.INTAKE[BACK][CUBE].armPos = SmartDashboard.getNumber("IntakeArmCube", + GoalPos.INTAKE[BACK][CUBE].armPos); + GoalPos.INTAKE[BACK][CUBE].wristPos = SmartDashboard.getNumber("IntakeWristCube", + GoalPos.INTAKE[BACK][CUBE].wristPos); GoalPos.LOW[BACK][CONE].armPos = SmartDashboard.getNumber("LowArmCone", GoalPos.LOW[BACK][CONE].armPos); GoalPos.LOW[BACK][CONE].wristPos = SmartDashboard.getNumber("LowWristCone", GoalPos.LOW[BACK][CONE].wristPos); GoalPos.MID[BACK][CONE].armPos = SmartDashboard.getNumber("MidArmCone", GoalPos.MID[BACK][CONE].armPos); GoalPos.MID[BACK][CONE].wristPos = SmartDashboard.getNumber("MidWristCone", GoalPos.MID[BACK][CONE].wristPos); GoalPos.HIGH[BACK][CONE].armPos = SmartDashboard.getNumber("HighArmCone", GoalPos.HIGH[BACK][CONE].armPos); - GoalPos.HIGH[BACK][CONE].wristPos = SmartDashboard.getNumber("HighWristCone", GoalPos.HIGH[BACK][CONE].wristPos); - GoalPos.STORED[BACK][CONE].armPos = SmartDashboard.getNumber("StoredArmCone", GoalPos.STORED[BACK][CONE].armPos); - GoalPos.STORED[BACK][CONE].wristPos = SmartDashboard.getNumber("StoredWristCone", GoalPos.STORED[BACK][CONE].wristPos); + GoalPos.HIGH[BACK][CONE].wristPos = SmartDashboard.getNumber("HighWristCone", + GoalPos.HIGH[BACK][CONE].wristPos); + GoalPos.STORED[BACK][CONE].armPos = SmartDashboard.getNumber("StoredArmCone", + GoalPos.STORED[BACK][CONE].armPos); + GoalPos.STORED[BACK][CONE].wristPos = SmartDashboard.getNumber("StoredWristCone", + GoalPos.STORED[BACK][CONE].wristPos); GoalPos.SHELF[BACK][CONE].armPos = SmartDashboard.getNumber("ShelfArmCone", GoalPos.SHELF[BACK][CONE].armPos); - GoalPos.SHELF[BACK][CONE].wristPos = SmartDashboard.getNumber("ShelfWristCone", GoalPos.SHELF[BACK][CONE].wristPos); + GoalPos.SHELF[BACK][CONE].wristPos = SmartDashboard.getNumber("ShelfWristCone", + GoalPos.SHELF[BACK][CONE].wristPos); GoalPos.SUBSTATION[BACK][CONE].armPos = SmartDashboard.getNumber("SubstationArmCone", GoalPos.SUBSTATION[BACK][CONE].armPos); GoalPos.SUBSTATION[BACK][CONE].wristPos = SmartDashboard.getNumber("SubstationWristCone", GoalPos.SUBSTATION[BACK][CONE].wristPos); - GoalPos.INTAKE[BACK][CONE].armPos = SmartDashboard.getNumber("IntakeArmCone", GoalPos.INTAKE[BACK][CONE].armPos); - GoalPos.INTAKE[BACK][CONE].wristPos = SmartDashboard.getNumber("IntakeWristCone", GoalPos.INTAKE[BACK][CONE].wristPos); + GoalPos.INTAKE[BACK][CONE].armPos = SmartDashboard.getNumber("IntakeArmCone", + GoalPos.INTAKE[BACK][CONE].armPos); + GoalPos.INTAKE[BACK][CONE].wristPos = SmartDashboard.getNumber("IntakeWristCone", + GoalPos.INTAKE[BACK][CONE].wristPos); } public void putArmControlOnSmartDashboard() { @@ -523,9 +619,12 @@ public void putArmControlOnSmartDashboard() { } public void getArmControlOnSmartDashboard() { - goalState[WRIST].position = Units.degreesToRadians(SmartDashboard.getNumber("WristGoalDeg", Units.radiansToDegrees(goalState[WRIST].position))); - goalState[ARM].position = Units.degreesToRadians(SmartDashboard.getNumber("ArmGoalDeg", Units.radiansToDegrees(goalState[ARM].position))); - // MAX_FF_VEL[ARM] = SmartDashboard.getNumber("Max FF Vel Arm", MAX_FF_VEL[ARM]); + goalState[WRIST].position = Units.degreesToRadians( + SmartDashboard.getNumber("WristGoalDeg", Units.radiansToDegrees(goalState[WRIST].position))); + goalState[ARM].position = Units.degreesToRadians( + SmartDashboard.getNumber("ArmGoalDeg", Units.radiansToDegrees(goalState[ARM].position))); + // MAX_FF_VEL[ARM] = SmartDashboard.getNumber("Max FF Vel Arm", + // MAX_FF_VEL[ARM]); MAX_FF_ACCEL[ARM] = SmartDashboard.getNumber("Max FF Accel Arm", MAX_FF_ACCEL[ARM]); kP[WRIST] = SmartDashboard.getNumber("kP: Wis", kP[WRIST]); kI[WRIST] = SmartDashboard.getNumber("kI: Wis", kI[WRIST]); @@ -535,6 +634,6 @@ public void getArmControlOnSmartDashboard() { kD[ARM] = SmartDashboard.getNumber("kD: Arm", kD[ARM]); } - //#endregion + // #endregion } diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java b/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java index 66512c08..e7b1992f 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java @@ -118,5 +118,31 @@ public void getRollerConstsOnSmartDashboard() { public double getPosition() { return motor.getEncoder().getPosition(); } - + + 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; + } + } }