diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index bcf1ea9..3bc197d 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -254,7 +254,8 @@ public static final class Drivetrainc { // front-left, front-right, back-left, back-right. // Determine correct turn PID constants public static final double[] turnkP = CONFIG.isSwimShady() - ? new double[] {0.00374, 0.00374, 0.00374, 0.00374} + ? 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 = @@ -280,7 +281,8 @@ public static final class Drivetrainc { // 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}; public static final double[] drivekI = { 0, 0, 0, 0 }; @@ -343,19 +345,27 @@ public static final class Drivetrainc { // #region Ports - public static final int driveFrontLeftPort = CONFIG.isSwimShady() ? 18 : 11; // - public static final int driveFrontRightPort = CONFIG.isSwimShady() ? 10 : 19; // - public static final int driveBackLeftPort = CONFIG.isSwimShady() ? 19 : 14; // - public static final int driveBackRightPort = CONFIG.isSwimShady() ? 1 : 17; // correct - - public static final int turnFrontLeftPort = CONFIG.isSwimShady() ? 17 : 12; // - public static final int turnFrontRightPort = CONFIG.isSwimShady() ? 9 : 20; // 20 - public static final int turnBackLeftPort = CONFIG.isSwimShady() ? 20 : 15; // - public static final int turnBackRightPort = CONFIG.isSwimShady() ? 2 : 16; // correct + public static final int driveFrontLeftPort = + CONFIG.isSwimShady() ? 1 : 11; // + public static final int driveFrontRightPort = + CONFIG.isSwimShady() ? 2 : 19; // + public static final int driveBackLeftPort = + CONFIG.isSwimShady() ? 3 : 14; // + public static final int driveBackRightPort = + CONFIG.isSwimShady() ? 4 : 17; // correct + + public static final int turnFrontLeftPort = + CONFIG.isSwimShady() ? 11 : 12; // + public static final int turnFrontRightPort = + CONFIG.isSwimShady() ? 12 : 20; // 20 + public static final int turnBackLeftPort = + CONFIG.isSwimShady() ? 13 : 15; // + public static final int turnBackRightPort = + CONFIG.isSwimShady() ? 14 : 16; // 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 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());