diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 44a0606..feda997 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -36,12 +36,13 @@ */ public final class Constants { public static final double g = 9.81; // meters per second squared + public static final class Led { public static final Color8Bit DEFAULT_COLOR_BLUE = new Color8Bit(0, 0, 200); public static final Color8Bit DETECT_NOTE_YELLOW = new Color8Bit(255, 255, 0); public static final Color8Bit HOLDING_GREEN = new Color8Bit(0, 250, 0); - public static final Color8Bit RED_NEO_550_MOTOR = new Color8Bit(255,0,0); + public static final Color8Bit RED_NEO_550_MOTOR = new Color8Bit(255, 0, 0); public static final int ledPort = 0; // TODO: figure out how to get port of LED, it could be 0 or } @@ -55,17 +56,19 @@ public static final class Effectorc { // 0.0001184 /* - * public static final double kP = 0.0001067; public static final double kI = 0; public - * static final double kD = 0; public static final double kS = 0; public static final double + * public static final double kP = 0.0001067; public static final double kI = 0; + * public + * static final double kD = 0; public static final double kS = 0; public static + * final double * kV = 0.11124; public static final double kA = 0.039757; */ - public static final double[] kP = {0, 0.0001067}; + public static final double[] kP = { 0, 0.0001067 }; public static final double[] kI = { /* /Intake/ */0, /* /Outake/ */0 }; public static final double[] kD = { /* /Intake/ */0, /* /Outake/ */0 }; public static final double[] kS = { /* /Intake/ */0.22, /* /Outake/ */0.29753 * 2 }; - public static final double[] kV = {0.122, 0.11124}; - public static final double[] kA = {0, 0.039757}; + public static final double[] kV = { 0.122, 0.11124 }; + public static final double[] kA = { 0, 0.039757 }; public static final int INTAKE_PORT = 30; // port public static final int OUTTAKE_PORT = 31; // port public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port correspnds with intake @@ -85,7 +88,7 @@ public static final class Effectorc { public static final double PODIUM_RPM = 4000; public static final double SPEAKER_RPM = 2100; // WTF FAB ISSUE - //public static final double SAFE_RPM = 6000;// WTF FAB ISSUE + // public static final double SAFE_RPM = 6000;// WTF FAB ISSUE public static final double EJECT_RPM_INTAKE = -2550; public static final double EJECT_RPM_OUTAKE = -2550; @@ -141,7 +144,8 @@ public static final class Armc { public static final double SUBWOOFER_ANGLE_RAD = -0.22;// touching the base of the speaker public static final double SAFE_ZONE_ANGLE_RAD = Units.degreesToRadians(36);// touching the white line - //public static final double PODIUM_ANGLE_RAD = Units.degreesToRadians(40);// touching the safe pad on the stage + // public static final double PODIUM_ANGLE_RAD = Units.degreesToRadians(40);// + // touching the safe pad on the stage // 0.4 rad for shooting at podium @@ -163,15 +167,14 @@ public static final class Armc { public static final double MAX_FF_VEL_RAD_P_S_BABY = 0; public static final double MAX_FF_ACCEL_RAD_P_S_BABY = 0; - //TODO: determine these values^ - + // TODO: determine these values^ public static final double SOFT_LIMIT_LOCATION_IN_RADIANS = 0; - public static final double CLIMB_POS = 1.701; //RADIANS + public static final double CLIMB_POS = 1.701; // RADIANS public static final double MIN_VOLTAGE = -0.5; // -((kS + kG + 1)/12); public static final double MAX_VOLTAGE = 0.5; // (kS + kG + 1)/12; - public static final double MIN_VOLTAGE_BABY = MIN_VOLTAGE/12 *0.7; - public static final double MAX_VOLTAGE_BABY = MAX_VOLTAGE/12*0.7; + public static final double MIN_VOLTAGE_BABY = MIN_VOLTAGE / 12 * 0.7; + public static final double MAX_VOLTAGE_BABY = MAX_VOLTAGE / 12 * 0.7; public static final double CLIMB_FINISH_POS = -0.38; // if needed public static final double COM_ARM_LENGTH_METERS = 0.381; @@ -184,9 +187,9 @@ public static final class Armc { // Boundaries public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 1.8345; // placeholder - public static final double POS_TOLERANCE_RAD = - Units.degreesToRadians(5); // placeholder //Whether or not this is the actual - // account + public static final double POS_TOLERANCE_RAD = Units.degreesToRadians(5); // placeholder //Whether or not this + // is the actual + // account // idk TODO: test on actual encoder without a conversion // factor public static final double VEL_TOLERANCE_RAD_P_SEC = (POS_TOLERANCE_RAD / 0.02); // 20ms per robot loop @@ -199,7 +202,6 @@ public static final class Armc { public static final double DISCONNECTED_ENCODER_TIMEOUT_SEC = 0.5; public static final double SMART_CURRENT_LIMIT_TIMEOUT = 0.8; - public static final int NEO_BUILTIN_ENCODER_CPR = 42; /* * counts per rotation - @@ -253,34 +255,44 @@ public static final class Drivetrainc { 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[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } + public static final double[] turnZeroDeg = RobotBase.isSimulation() + ? new double[] { -90.0, -90.0, -90.0, -90.0 } : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } : new double[] { -48.6914, 63.3691, 94.1309, -6.7676 });/* real values here */ // 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 = { 51.078, 60.885, 60.946, 60.986 }; // {0.00374, 0.00374, 0.00374, - // 0.00374}; + public static final double[] turnkP = CONFIG.isSwimShady() + ? new double[] { 0.00374 * 350, 0.00374 * 350, 0.00374 * 350, + 0.00374 * 350 } + : new double[] { 51.078, 60.885, 60.946, 60.986 }; public static final double[] turnkI = { 0, 0, 0, 0 }; - public static final double[] turnkD = { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d + public static final double[] turnkD = CONFIG.isSwimShady() ? new double[] { 0, 0, 0, 0 } + : new double[] { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262 }; + public static final double[] turnkS = CONFIG.isSwimShady() ? new double[] { 0.2, 0.2, 0.2, 0.2 } + : new double[] { 0.13027, 0.17026, 0.2, 0.23262 }; // 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 = { 2.6532, 2.7597, 2.7445, 2.7698 }; - public static final double[] turnkV = { 0.00463, 0.00463, 0.00463, 0.00463 }; // swimshady - public static final double[] turnkA = { 0.000115, 0.000115, 0.000115, 0.000115 }; // swimshady + public static final double[] turnkV = CONFIG.isSwimShady() + ? new double[] { 0.00463, 0.00463, 0.00463, 0.00463 } + : new double[] { 2.6532, 2.7597, 2.7445, 2.7698 }; + public static final double[] turnkA = CONFIG.isSwimShady() + ? new double[] { 0.000115, 0.000115, 0.000115, 0.000115 } + : new double[] { 0.17924, 0.17924, 0.17924, 0.17924 }; // 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 // Order of modules: (FL, FR, BL, BR) - public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } + public static final double[] drivekP = CONFIG.isSwimShady() + ? new double[] { 2.8 / 6, 2.8 / 6, 2.8 / 6, 2.8 / 6 } : new double[] { 1.75, 1.75, 1.75, .75 }; // {1.82/100, 1.815/100, 2.015/100, - // 1.915/100}; + // 1.915/100}; public static final double[] drivekI = { 0, 0, 0, 0 }; public static final double[] drivekD = { 0, 0, 0, 0 }; public static final boolean[] driveInversion = (CONFIG.isSwimShady() @@ -288,13 +300,25 @@ public static final class Drivetrainc { : new boolean[] { true, false, true, false }); public static final boolean[] turnInversion = { true, true, true, true }; // kS - public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; - public static final double[] kBackwardVolts = kForwardVolts; - - public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kBackwardVels = kForwardVels; - public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; - public static final double[] kBackwardAccels = kForwardAccels; + public static final double[] kForwardVolts = CONFIG.isSwimShady() ? new double[] { 0.129, 0.108, 0.14, 0.125 } + : new double[] { 0.26744, 0.31897, 0.27967, 0.2461 }; + public static final double[] kBackwardVolts = CONFIG.isSwimShady() ? new double[] { 0.115, 0.169, 0.13, 0.148 } + : kForwardVolts; + + public static final double[] kForwardVels = CONFIG.isSwimShady() + ? new double[] { 2.910 / 1.1, 2.970 / 1.1, 2.890 / 1.1, + 2.930 / 1.1 } + : new double[] { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kBackwardVels = CONFIG.isSwimShady() + ? new double[] { 2.890 / 1.1, 2.800 / 1.1, 2.850 / 1.1, + 2.820 / 1.1 } + : kForwardVels; + public static final double[] kForwardAccels = CONFIG.isSwimShady() ? new double[] { 0.145, 0.149, 0.192, 0.198 } + : new double[] { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, + 1.1003 / 2 }; + public static final double[] kBackwardAccels = CONFIG.isSwimShady() + ? new double[] { 0.192, 0.187, 0.264, 0.176 } + : kForwardAccels; public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 @@ -311,7 +335,7 @@ public static final class Drivetrainc { : new double[] { 2, 0.0, 0.0 }; public static final double[] yPIDController = xPIDController; public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } - : new double[] {0.05, 0.0, 0.00}; + : new double[] { 0.05, 0.0, 0.00 }; public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, @@ -336,8 +360,8 @@ public static final class Drivetrainc { public static final int turnBackRightPort = 14; // correct public static final int canCoderPortFL = CONFIG.isSwimShady() ? 1 : 0; - public static final int canCoderPortFR = CONFIG.isSwimShady() ? 3 : 3; - public static final int canCoderPortBL = CONFIG.isSwimShady() ? 2 : 2; + public static final int canCoderPortFR = CONFIG.isSwimShady() ? 2 : 3; + public static final int canCoderPortBL = CONFIG.isSwimShady() ? 3 : 2; public static final int canCoderPortBR = CONFIG.isSwimShady() ? 4 : 1; // #endregion @@ -349,7 +373,8 @@ public static final class Drivetrainc { public static double kSlowDriveSpeed = 0.4; // Percent Multiplier public static double kSlowDriveRotation = 0.250; // Percent Multiplier - //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use + // baby speed values, i just guessed the percent multiplier. TODO: find actual + // ones we wana use public static double kBabyDriveSpeed = 0.3; public static double kBabyDriveRotation = 0.2; public static double kAlignMultiplier = 1D / 3D; @@ -411,8 +436,7 @@ public static final class Limelightc { public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446; public static final double MOUNT_ANGLE_DEG_INTAKE = -29; public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); - public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = - Units.inchesToMeters(13); + public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(13); public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused public static final double MIN_MOVEMENT_METERSPSEC = 1.5; @@ -421,6 +445,7 @@ public static final class Limelightc { public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911); public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75); public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused + public static final class Apriltag { public static final int RED_SPEAKER_CENTER_TAG_ID = 4; public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; @@ -463,19 +488,16 @@ public static final class Manipulator { public static final Axis AMP_AX = Axis.kLeftTrigger; public static final int SPEAKER_POS = Button.kX.value; public static final int ARM_TO_AMP_BUTTON = Button.kY.value; - public static final int PASS_TO_OUTTAKE_STICK = - Button.kLeftStick.value; - public static final int PASS_TO_INTAKE_STICK = - Button.kRightStick.value; + public static final int PASS_TO_OUTTAKE_STICK = Button.kLeftStick.value; + public static final int PASS_TO_INTAKE_STICK = Button.kRightStick.value; public static final int UP_D_PAD = 0; public static final int DOWN_D_PAD = 180; public static final int LEFT_D_PAD = 270; - + public static final int A_BUTTON = Button.kA.value; public static final int RAMP_OUTTAKE = Button.kB.value; } - public static final double JOY_THRESH = 0.01; public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;// woah, this is high. diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 84026de..46c7317 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -71,14 +71,14 @@ public class RobotContainer { // 1. using GenericHID allows us to use different kinds of controllers // 2. Use absolute paths from constants to reduce confusion public final GenericHID driverController = new GenericHID(Driver.port); - public final GenericHID manipulatorController = new GenericHID(Manipulator.port); - private final IntakeShooter intakeShooter = new IntakeShooter(); + // public final GenericHID manipulatorController = new GenericHID(Manipulator.port); + // private final IntakeShooter intakeShooter = new IntakeShooter(); // ignore warning, LED must be initialized - private final Led led = new Led(intakeShooter); - private final Arm arm = new Arm(); + // private final Led led = new Led(intakeShooter); + // private final Arm arm = new Arm(); private final Drivetrain drivetrain = new Drivetrain(); - private final Limelight limelight = new Limelight(drivetrain); + // private final Limelight limelight = new Limelight(drivetrain); /* These are assumed to be equal to the AUTO ames in pathplanner */ /* These must be equal to the pathPlanner path names from the GUI! */ @@ -153,17 +153,17 @@ private void setDefaultCommands() { () -> ProcessedAxisValue(driverController, Axis.kRightX), () -> driverController.getRawButton(Driver.slowDriveButton))); // TODO: Are we going to use default command for intakeshooter? - intakeShooter.setDefaultCommand(new TeleopEffector(intakeShooter, - () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY), - manipulatorController, driverController)); + // intakeShooter.setDefaultCommand(new TeleopEffector(intakeShooter, + // () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY), + // manipulatorController, driverController)); // TODO // intakeShooter.setDefaultCommand(new RampMaxRPMDriving(intakeShooter)); - arm.setDefaultCommand( - Config.CONFIG.useSmartDashboardControl ? new TestArmToPos(arm) - : new TeleopArm(arm, - () -> ProcessedAxisValue(manipulatorController, - Axis.kLeftY))); + // arm.setDefaultCommand( + // Config.CONFIG.useSmartDashboardControl ? new TestArmToPos(arm) + // : new TeleopArm(arm, + // () -> ProcessedAxisValue(manipulatorController, + // Axis.kLeftY))); } @@ -174,18 +174,18 @@ private void setBindingsDriver() { // .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), // new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight))); - new POVButton(driverController, 0) - .whileTrue(new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 1))); + // new POVButton(driverController, 0) + // .whileTrue(new ParallelCommandGroup(new Intake(intakeShooter), + // new AutoMATICALLYGetNote(drivetrain, limelight, + // intakeShooter, 1))); axisTrigger(driverController, Axis.kLeftTrigger) // .onTrue(new AlignToApriltag(drivetrain, limelight)); .onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false))) .onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true))); - axisTrigger(driverController, Manipulator.SHOOTER_BUTTON) - .whileTrue(new AlignToApriltag(drivetrain, limelight, 2.0)); + // axisTrigger(driverController, Manipulator.SHOOTER_BUTTON) + // .whileTrue(new AlignToApriltag(drivetrain, limelight, 2.0)); new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue( new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); new JoystickButton(driverController, Driver.rotateFieldRelative90Deg) @@ -200,8 +200,8 @@ private void setBindingsDriver() { } private void setBindingsManipulator() { - new JoystickButton(manipulatorController, EJECT_BUTTON) - .onTrue(new Eject(intakeShooter)); + // new JoystickButton(manipulatorController, EJECT_BUTTON) + // .onTrue(new Eject(intakeShooter)); // new JoystickButton(manipulatorController, A_BUTTON) // .onTrue(new RampMaxRPMDriving(intakeShooter)); @@ -209,10 +209,10 @@ private void setBindingsManipulator() { // new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), // new PassToOuttake(intakeShooter))); - axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( - new ConditionalCommand(new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), - new PassToOuttake(intakeShooter)), new InstantCommand(() -> { - }), () -> LimelightHelpers.getTV(SHOOTER_LL_NAME))); + // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( + // new ConditionalCommand(new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), + // new PassToOuttake(intakeShooter)), new InstantCommand(() -> { + // }), () -> LimelightHelpers.getTV(SHOOTER_LL_NAME))); // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) // .whileTrue(new PassToOuttake(intakeShooter)); @@ -220,10 +220,10 @@ private void setBindingsManipulator() { // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) // .whileTrue(new AimArmSpeaker(arm, limelight)); - new JoystickButton(manipulatorController, RAMP_OUTTAKE) - .whileTrue(new RampMaxRPM(intakeShooter)); - new JoystickButton(manipulatorController, OPPOSITE_EJECT) - .whileTrue(new EjectOuttakeSide(intakeShooter)); + // new JoystickButton(manipulatorController, RAMP_OUTTAKE) + // .whileTrue(new RampMaxRPM(intakeShooter)); + // new JoystickButton(manipulatorController, OPPOSITE_EJECT) + // .whileTrue(new EjectOuttakeSide(intakeShooter)); /* * axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) @@ -235,23 +235,23 @@ private void setBindingsManipulator() { // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) // .onTrue(new PassToOuttake(intakeShooter)); - axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON) - .whileTrue(new Intake(intakeShooter)); - new JoystickButton(manipulatorController, ARM_TO_AMP_BUTTON) - .onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR)); - new JoystickButton(manipulatorController, A_BUTTON) - .onTrue(new ArmToPos(arm, GROUND_INTAKE_POS)); - new JoystickButton(manipulatorController, PASS_TO_OUTTAKE_STICK) - .onTrue(new PassToOuttake(intakeShooter)); - new JoystickButton(manipulatorController, PASS_TO_INTAKE_STICK) - .onTrue(new PassToIntake(intakeShooter)); - new JoystickButton(manipulatorController, SPEAKER_POS) - .onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD)); - new POVButton(manipulatorController, UP_D_PAD) - .onTrue(new ArmToPos(arm, CLIMB_POS)); - new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm)); - new POVButton(manipulatorController, LEFT_D_PAD) - .onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD)); + // axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON) + // .whileTrue(new Intake(intakeShooter)); + // new JoystickButton(manipulatorController, ARM_TO_AMP_BUTTON) + // .onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR)); + // new JoystickButton(manipulatorController, A_BUTTON) + // .onTrue(new ArmToPos(arm, GROUND_INTAKE_POS)); + // new JoystickButton(manipulatorController, PASS_TO_OUTTAKE_STICK) + // .onTrue(new PassToOuttake(intakeShooter)); + // new JoystickButton(manipulatorController, PASS_TO_INTAKE_STICK) + // .onTrue(new PassToIntake(intakeShooter)); + // new JoystickButton(manipulatorController, SPEAKER_POS) + // .onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD)); + // new POVButton(manipulatorController, UP_D_PAD) + // .onTrue(new ArmToPos(arm, CLIMB_POS)); + // new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm)); + // new POVButton(manipulatorController, LEFT_D_PAD) + // .onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD)); } @@ -329,80 +329,80 @@ private Trigger axisTrigger(GenericHID controller, Axis axis) { private void registerAutoCommands() { //// AUTO-USABLE COMMANDS - NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); - NamedCommands.registerCommand("Eject", new Eject(intakeShooter)); + // NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); + // NamedCommands.registerCommand("Eject", new Eject(intakeShooter)); // NamedCommands.registerCommand("ArmToSpeaker", new MoveToPos(arm, // Armc.SPEAKER_ANGLE_RAD, 0)); - NamedCommands.registerCommand("ArmToAmp", - new ArmToPos(arm, Armc.AMP_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToSubwoofer", - new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToPodium", - new ArmToPos(arm, Armc.PODIUM_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToGround", - new ArmToPos(arm, GROUND_INTAKE_POS)); + // NamedCommands.registerCommand("ArmToAmp", + // new ArmToPos(arm, Armc.AMP_ANGLE_RAD)); + // NamedCommands.registerCommand("ArmToSubwoofer", + // new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD)); + // NamedCommands.registerCommand("ArmToPodium", + // new ArmToPos(arm, Armc.PODIUM_ANGLE_RAD)); + // NamedCommands.registerCommand("ArmToGround", + // new ArmToPos(arm, GROUND_INTAKE_POS)); - NamedCommands.registerCommand("RampRPMAuton", - new RampRPMAuton(intakeShooter)); + // NamedCommands.registerCommand("RampRPMAuton", + // new RampRPMAuton(intakeShooter)); - NamedCommands.registerCommand("SwitchRPMShoot", - new Outtake(intakeShooter, arm)); + // NamedCommands.registerCommand("SwitchRPMShoot", + // new Outtake(intakeShooter, arm)); // NamedCommands.registerCommand("AutonRuinerShoot", new // AutonRuinerShoot(intakeShooter)); // NamedCommands.registerCommand("IntakeAutonRuiner", new // IntakeAutonRuiner(intakeShooter)); - NamedCommands.registerCommand("AutonRuinerShootAndIntake", - new AutonRuinerShootAndIntake(intakeShooter, arm)); - - NamedCommands.registerCommand("PassToOuttake", - new PassToOuttake(intakeShooter)); - NamedCommands.registerCommand("AimArmSpeakerMT2", - new AimArmSpeaker(arm, limelight)); - NamedCommands.registerCommand("AlignToAprilTagMegaTag2", - new AlignToApriltag(drivetrain, limelight, 0.0)); - NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( - new ParallelDeadlineGroup( - new WaitCommand(3.0), - new SequentialCommandGroup( - // TODO: Use Align To Drivetrain - // new AlignDrivetrain(drivetrain), - new ParallelCommandGroup( - new AlignToApriltag(drivetrain, limelight, 0.0), - new AimArmSpeaker(arm, limelight), - new RampRPMAuton(intakeShooter)), - new PassToOuttake(intakeShooter), - new ArmToPos(arm, GROUND_INTAKE_POS))))); - NamedCommands.registerCommand("ShootSubwoofer", - new SequentialCommandGroup(new ParallelCommandGroup( - new ArmToPos(arm, - Armc.SUBWOOFER_ANGLE_RAD), - new RampRPMAuton(intakeShooter)), - new PassToOuttake(intakeShooter), - new ArmToPos(arm, GROUND_INTAKE_POS))); - NamedCommands.registerCommand("Limelight Intake CCW", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 1))); - NamedCommands.registerCommand("Limelight Intake CW", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, -1))); - - NamedCommands.registerCommand("Limelight Intake Straight", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 0))); - - NamedCommands.registerCommand("StopIntake", - new InstantCommand(intakeShooter::stopIntake)); - NamedCommands.registerCommand("StopOutake", - new InstantCommand(intakeShooter::stopOuttake)); - NamedCommands.registerCommand("StopBoth", - new ParallelCommandGroup(new InstantCommand(intakeShooter::stopIntake), - new InstantCommand(intakeShooter::stopOuttake))); + // NamedCommands.registerCommand("AutonRuinerShootAndIntake", + // new AutonRuinerShootAndIntake(intakeShooter, arm)); + + // NamedCommands.registerCommand("PassToOuttake", + // new PassToOuttake(intakeShooter)); + // NamedCommands.registerCommand("AimArmSpeakerMT2", + // new AimArmSpeaker(arm, limelight)); + // NamedCommands.registerCommand("AlignToAprilTagMegaTag2", + // new AlignToApriltag(drivetrain, limelight, 0.0)); + // NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( + // new ParallelDeadlineGroup( + // new WaitCommand(3.0), + // new SequentialCommandGroup( + // // TODO: Use Align To Drivetrain + // // new AlignDrivetrain(drivetrain), + // new ParallelCommandGroup( + // new AlignToApriltag(drivetrain, limelight, 0.0), + // new AimArmSpeaker(arm, limelight), + // new RampRPMAuton(intakeShooter)), + // new PassToOuttake(intakeShooter), + // new ArmToPos(arm, GROUND_INTAKE_POS))))); + // NamedCommands.registerCommand("ShootSubwoofer", + // new SequentialCommandGroup(new ParallelCommandGroup( + // new ArmToPos(arm, + // Armc.SUBWOOFER_ANGLE_RAD), + // new RampRPMAuton(intakeShooter)), + // new PassToOuttake(intakeShooter), + // new ArmToPos(arm, GROUND_INTAKE_POS))); + // NamedCommands.registerCommand("Limelight Intake CCW", + // new ParallelCommandGroup(new Intake(intakeShooter), + // new AutoMATICALLYGetNote(drivetrain, limelight, + // intakeShooter, 1))); + // NamedCommands.registerCommand("Limelight Intake CW", + // new ParallelCommandGroup(new Intake(intakeShooter), + // new AutoMATICALLYGetNote(drivetrain, limelight, + // intakeShooter, -1))); + + // NamedCommands.registerCommand("Limelight Intake Straight", + // new ParallelCommandGroup(new Intake(intakeShooter), + // new AutoMATICALLYGetNote(drivetrain, limelight, + // intakeShooter, 0))); + + // NamedCommands.registerCommand("StopIntake", + // new InstantCommand(intakeShooter::stopIntake)); + // NamedCommands.registerCommand("StopOutake", + // new InstantCommand(intakeShooter::stopOuttake)); + // NamedCommands.registerCommand("StopBoth", + // new ParallelCommandGroup(new InstantCommand(intakeShooter::stopIntake), + // new InstantCommand(intakeShooter::stopOuttake))); } private void setupAutos() { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 2a75050..9a71568 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -110,7 +110,8 @@ public class Drivetrain extends SubsystemBase { private Timer simTimer = new Timer(); private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; - + private Timer timer = new Timer(); + private double lastMeasuredTime = 0; public Drivetrain() { // SmartDashboard.putNumber("Pose Estimator set x (m)", lastSetX); // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); @@ -119,7 +120,7 @@ public Drivetrain() { // SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); // SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); - + SmartDashboard.putNumber("biggoal", 0); // Calibrate Gyro { @@ -139,6 +140,7 @@ public Drivetrain() { // this.resetFieldOrientation(); System.out.println("NavX-MXP firmware version: " + gyro.getFirmwareVersion()); System.out.println("Magnetometer is calibrated: " + gyro.isMagnetometerCalibrated()); + timer.start(); } // Setup Kinematics @@ -307,12 +309,15 @@ public void periodic() { // moduleFR.periodic(); // moduleBL.periodic(); // moduleBR.periodic(); - // double goal = SmartDashboard.getNumber("bigoal", 0); + double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { module.periodic(); - // module.move(0, goal); + // module.move(5, goal); } - + double deltaTime = lastMeasuredTime - timer.get(); + SmartDashboard.putNumber("Time to update", deltaTime); + lastMeasuredTime = timer.get(); + // keepRotateMotorsAtDegrees((int) goal); // field.setRobotPose(odometry.getPoseMeters()); field.setRobotPose(poseEstimator.getEstimatedPosition());