diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 8457fc6..04a91cd 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -31,26 +31,27 @@ private static class PORT { /** * Bidings on driver controls */ - private static final XboxController xboxController = new XboxController(PORT.DRIVE_CONTROLLER_PORT); + private static final XboxController driveController = new XboxController(PORT.DRIVE_CONTROLLER_PORT); public static final XboxController operatorController = new XboxController(PORT.OPERATOR_CONTROLLER_PORT); // Changed to public - + private static class DRIVER_MAP { - private static final int ABUTTON = XboxController.Button.kA.value; - private static final int BBUTTON = XboxController.Button.kB.value; + private static final int ABUTTON = XboxController.Button.kA.value; + private static final int BBUTTON = XboxController.Button.kB.value; } // drive sticks - public static DoubleSupplier xboxLeftStickXSupplier = () -> { return processDriveInput(-xboxController.getLeftX()); }; - public static DoubleSupplier xboxLeftStickYSupplier = () -> { return processDriveInput(-xboxController.getLeftY()); }; - public static DoubleSupplier xboxRightStickXSupplier = () -> { return processRotationInput(-xboxController.getRightX()); }; - public static DoubleSupplier xboxRightStickYSupplier = () -> { return processRotationInput(xboxController.getRightY()); }; + public static DoubleSupplier xboxLeftStickXSupplier = () -> { return processDriveInput(-driveController.getLeftX()); }; + public static DoubleSupplier xboxLeftStickYSupplier = () -> { return processDriveInput(-driveController.getLeftY()); }; + public static DoubleSupplier xboxRightStickXSupplier = () -> { return processRotationInput(-driveController.getRightX()); }; + public static DoubleSupplier xboxRightStickYSupplier = () -> { return processRotationInput(driveController.getRightY()); }; // drive buttons - public static final JoystickButton driveControllerA = new JoystickButton(xboxController, DRIVER_MAP.ABUTTON); - public static final JoystickButton driveControllerB = new JoystickButton(xboxController, DRIVER_MAP.BBUTTON); - public static final Trigger driveRightTrigger = new Trigger(() -> - xboxController.getRightTriggerAxis() > TRIGGER_THRESHOLD); - + public static final JoystickButton driveControllerA = new JoystickButton(driveController, DRIVER_MAP.ABUTTON); + public static final JoystickButton driveControllerB = new JoystickButton(driveController, DRIVER_MAP.BBUTTON); + public static DoubleSupplier driveControllerRightTriggerSupplier = () -> { return driveController.getRightTriggerAxis();}; + + + private static double processDriveInput(double raw) { if (Math.abs(raw) < 0.1) { raw = 0 ; @@ -89,9 +90,12 @@ private static class operatorBindings { public static final JoystickButton operatorControllerBack = new JoystickButton(operatorController, operatorBindings.BACK_BUTTON); // Trigger controls for coral claw + public static DoubleSupplier operatorRightTriggerSupplier= () -> { return operatorController.getRightTriggerAxis();}; public static final Trigger operatorRightTrigger = new Trigger(() -> operatorController.getRightTriggerAxis() > TRIGGER_THRESHOLD); + public static DoubleSupplier operatorLeftTriggerSupplier= () -> { return operatorController.getLeftTriggerAxis();}; + public static final Trigger operatorLeftTrigger = new Trigger(() -> operatorController.getLeftTriggerAxis() > TRIGGER_THRESHOLD); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6702f02..ffc3456 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,7 +44,7 @@ public class RobotContainer { public RobotContainer() { // Initialize OI with a reference to this RobotContainer instance configureBindings(); - drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier)); + drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.xboxLeftStickXSupplier, OI.xboxLeftStickYSupplier, OI.xboxRightStickXSupplier, OI.driveControllerRightTriggerSupplier)); RobotMap.compressor.enableAnalog(70, 120); // set the default drive command to use the left and right stick values from the Xbox controller // The left stick controls the forward and backward movement of the robot, while the right stick @@ -62,53 +62,39 @@ private void configureBindings() { drivetrain.resetPosition(); })); - OI.driveRightTrigger.onTrue(new InstantCommand(() -> drivetrain.setSlow(true))); - OI.driveRightTrigger.onFalse(new InstantCommand(() -> drivetrain.setSlow(false))); - // Operator controller bindings // Elevator level controls - // OperatorBindings.operatorControllerA.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.LOW))); - // OperatorBindings.operatorControllerB.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.HIGH))); - // OperatorBindings.operatorControllerX.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.MED))); - // OperatorBindings.operatorControllerY.onTrue(new InstantCommand(() -> elevator.moveToLevel(ElevatorLevel.EXTRA_HIGH))); - // OPERATOR B : INTAKE OI.operatorControllerB.onTrue(new IntakeCommandGroup(elevator, wrist, coralClaw)); // OPERATOR RB : TROUGH(L1) - OI.operatorControllerRightBumper.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 1)); + OI.operatorControllerRightBumper.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.LOW)); // OPERATOR A : L2 (lowest pole) - OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 2)); + OI.operatorControllerA.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.MED)); // OPERATOR X : L3 (2nd pole) - OI.operatorControllerX.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 3)); + OI.operatorControllerX.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.HIGH)); // OPERATOR Y : L4 (last pole) - OI.operatorControllerY.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, 4)); + OI.operatorControllerY.onTrue(new ScoreCommandGroup(elevator, wrist, coralClaw, ElevatorLevel.EXTRA_HIGH)); - // OPERATOR LB: WRIST TOGGLE + // OPERATOR LB : WRIST TOGGLE OI.operatorControllerLeftBumper.onTrue(new InstantCommand(() -> wrist.toggle())); - + + // OPERATOR START : TRIM UP OI.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.trimTarget(0.0175))); + // OPERATOR BACK : TRIM DOWN OI.operatorControllerBack.onTrue(new InstantCommand(() -> elevator.trimTarget(-0.0175))); - - - - // Coral Claw controls with triggers - // OI.operatorRightTrigger.onTrue(new InstantCommand(() -> coralClaw.intake())); - // OI.operatorRightTrigger.onFalse(new InstantCommand(() -> coralClaw.stop())); - OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, 1)); - OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, 0)); - // OI.operatorLeftTrigger.onTrue(new InstantCommand(() -> coralClaw.setOutput(coralClaw.OUTTAKE_SPEED))); - // OI.operatorLeftTrigger.onFalse(new InstantCommand(() -> coralClaw.stop())); - - // Reset encoder with Start button - // OI.operatorControllerStart.onTrue(new InstantCommand(() -> elevator.resetEncoder())); - + + // OPERATOR RT : INTAKE + OI.operatorRightTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorRightTriggerSupplier, 1)); + // OPERATOR LT : OUTTAKE + OI.operatorLeftTrigger.whileTrue(new ClawCommand(coralClaw, OI.operatorLeftTriggerSupplier, 0)); + // Manual control with right stick for testing in simulation - OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> - elevator.setSpeed(OI.processElevatorInput(OI.operatorController.getRightY())), elevator)); + // OI.operatorControllerRightBumper.whileTrue(new InstantCommand(() -> + // elevator.setSpeed(OI.processElevatorInput(OI.operatorController.getRightY())), elevator)); } public Command getAutonomousCommand() { - return new ScoreTest(drivetrain, gyro, camera); + return new ScoreTest(drivetrain, gyro, camera, elevator, wrist, coralClaw); // return new DriveDistance(drivetrain, gyro, 7.0, 0.0, 0.0).andThen(new DriveDistance(drivetrain, gyro, 0.0, 3.0, 0.0)); // return new DriveWithHeadingCommand(drivetrain, gyro, OI.xboxLeftStickXSupplier, OI.xboxRightStickYSupplier, new Rotation2d(0.0)); // return new DriveMeters(drivetrain, 0.0, 0.0, 0.0); diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index f7be539..3395849 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -8,19 +8,21 @@ import frc.robot.subsystems.Drivetrain; public class DriveCommand extends Command { - - Drivetrain drivetrain; - DoubleSupplier xSupplier; - DoubleSupplier ySupplier; - DoubleSupplier omegaSupplier; - - public DriveCommand(Drivetrain drivetrain, DoubleSupplier xSupplier, DoubleSupplier ySupplier, - DoubleSupplier omegaSupplier) { - addRequirements(drivetrain); + + Drivetrain drivetrain; + DoubleSupplier xSupplier; + DoubleSupplier ySupplier; + DoubleSupplier omegaSupplier; + DoubleSupplier triggerSupplier; + /** drive robot according to the x and y of the drive controller, drive at half speed by default, increase by half the value of the drive trigger (0-1)**/ + public DriveCommand(Drivetrain drivetrain, DoubleSupplier xSupplier, DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier, DoubleSupplier triggerSupplier) { + addRequirements(drivetrain); this.drivetrain = drivetrain; this.xSupplier = xSupplier; this.ySupplier = ySupplier; this.omegaSupplier = omegaSupplier; + this.triggerSupplier = triggerSupplier; } @Override @@ -29,9 +31,8 @@ public void initialize() { @Override public void execute() { - drivetrain.drive(ySupplier.getAsDouble(), xSupplier.getAsDouble(), omegaSupplier.getAsDouble()); - // opposite suppliers are passed into command, appear correct in - // DriveCommand.java + double scaleFactor = triggerSupplier.getAsDouble() * 0.5 + 0.5; + drivetrain.drive(ySupplier.getAsDouble() * scaleFactor, xSupplier.getAsDouble() * scaleFactor, omegaSupplier.getAsDouble()); } @Override diff --git a/src/main/java/frc/robot/commands/ScoreCommandGroup.java b/src/main/java/frc/robot/commands/ScoreCommandGroup.java index bd368ea..3db5edf 100644 --- a/src/main/java/frc/robot/commands/ScoreCommandGroup.java +++ b/src/main/java/frc/robot/commands/ScoreCommandGroup.java @@ -1,4 +1,3 @@ - package frc.robot.commands; import frc.robot.subsystems.Elevator; @@ -11,31 +10,15 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; public class ScoreCommandGroup extends ParallelCommandGroup { - public ScoreCommandGroup(Elevator elevator, Wrist wrist, CoralClaw claw, int level) { - switch (level) { - case 1: - addCommands( new InstantCommand(() -> elevator.setTarget(ElevatorLevel.LOW)), new InstantCommand(() -> wrist.extend())); - break; - case 2: - addCommands( new InstantCommand(() -> elevator.setTarget(ElevatorLevel.MED)), new InstantCommand(() -> wrist.extend())); - break; - case 3: - addCommands( new InstantCommand(() -> elevator.setTarget(ElevatorLevel.HIGH)), new InstantCommand(() -> wrist.extend())); - break; - case 4: - addCommands( new InstantCommand(() -> elevator.setTarget(ElevatorLevel.EXTRA_HIGH)), new InstantCommand(() -> wrist.extend())); - break; - default: - System.out.println("SCORECOMMANDGROUP INVALID TARGET: " + level); - break; + public ScoreCommandGroup(Elevator elevator, Wrist wrist, CoralClaw claw, ElevatorLevel level) { + if(level == ElevatorLevel.LOW) { + addCommands(new InstantCommand(() -> claw.setMotorsOn(false, true))); + } else { + addCommands(new InstantCommand(() -> claw.setMotorsOn(true, true))); } - - - - - // addCommands( - // new InstantCommand(() -> elevator.setTarget(ElevatorLevel.STATION)), - // new InstantCommand(() -> wrist.retract()) - // ); + addCommands( + new InstantCommand(() -> elevator.setTarget(level)), + new InstantCommand(() -> wrist.extend()) + ); } } diff --git a/src/main/java/frc/robot/commands/auto/DriveDistance.java b/src/main/java/frc/robot/commands/auto/DriveDistance.java index d347bd6..316b47c 100644 --- a/src/main/java/frc/robot/commands/auto/DriveDistance.java +++ b/src/main/java/frc/robot/commands/auto/DriveDistance.java @@ -21,21 +21,23 @@ public class DriveDistance extends Command { double angleDisplacement; double xyThreshold = 0.6; - double xyMaxSpeed = 0.65; + double xyMaxSpeed; double rotateMaxSpeed = 1.5; - public DriveDistance(Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) { + public DriveDistance(double xyMaxSpeed, Drivetrain drivetrain, Gyro gyro, double xDistance, double yDistance, double theta) { this.drivetrain = drivetrain; this.gyro = gyro; this.xDistance = xDistance; this.yDistance = yDistance; this.theta = theta; + this.xyMaxSpeed = xyMaxSpeed; addRequirements(drivetrain); } @Override public void initialize() { + System.out.println("drivedistance initialized"); Pose2d start = drivetrain.getPose2d(); this.target = new Pose2d((start.getX() + xDistance), (start.getY() + yDistance), new Rotation2d(theta)); } @@ -84,7 +86,7 @@ public void execute() { @Override public void end(boolean interrupted) { - drivetrain.driveRobotRelative(0.0, 0.0, 0.0); + drivetrain.drive(0.0, 0.0, 0.0); } @Override diff --git a/src/main/java/frc/robot/commands/auto/ScoreTest.java b/src/main/java/frc/robot/commands/auto/ScoreTest.java index dd61bad..4b429b6 100644 --- a/src/main/java/frc/robot/commands/auto/ScoreTest.java +++ b/src/main/java/frc/robot/commands/auto/ScoreTest.java @@ -2,9 +2,17 @@ import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Gyro; import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Wrist; +import frc.robot.subsystems.CoralClaw; import frc.robot.commands.auto.DriveDistance; +import frc.robot.commands.IntakeCommandGroup; +import frc.robot.commands.ScoreCommandGroup; +import frc.robot.ElevatorLevel; + import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; /** * The ScoreTest command is a SequentialCommandGroup that performs a series of driving maneuvers @@ -37,14 +45,52 @@ public class ScoreTest extends SequentialCommandGroup { * @param gyro The gyro sensor used for orientation and heading. * @param camera The camera used for vision processing (currently commented out). */ - public ScoreTest(Drivetrain drivetrain, Gyro gyro, Camera camera) { - // ID7 from outer + public ScoreTest(Drivetrain drivetrain, Gyro gyro, Camera camera, Elevator elevator, Wrist wrist, CoralClaw claw) { addCommands( new InstantCommand(() -> drivetrain.resetPosition()), - new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall - new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7 - new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command - new DriveTowardsThing(drivetrain, camera) + new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 1.0472), + new DriveDistance(0.25, drivetrain, gyro, 0.0, 2.1, 1.0472), + new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), + new WaitCommand(0.5), + new InstantCommand(() -> claw.setOutput(-0.4)), + new WaitCommand(1.0), + new InstantCommand(() -> claw.setOutput(0.0)) ); + + // SIDE 2 + // addCommands( + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(0.25, drivetrain, gyro, 2.5, 0.0, 5.23599), + // new DriveDistance(0.25, drivetrain, gyro, 0.0, -2.1, 5.23599), + // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), + // new WaitCommand(500), + // new InstantCommand(() -> claw.setOutput(-0.4)), + // new WaitCommand(1000.0), + // new InstantCommand(() -> claw.setOutput(0.0)) + // ); + + + // new InstantCommand(() -> drivetrain.driveRobotRelative(0.25, 0.0, 0.0)) + + // SIDE 1 L1 + // addCommands( + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(0.5, drivetrain, gyro, 2.73, 0.0, 0.0), // drive to wall + // new ScoreCommandGroup(elevator, wrist, claw, ElevatorLevel.LOW), + // new WaitCommand(500.0), + // new InstantCommand(() -> claw.setOutput(-0.4)), + // new WaitCommand(1000.0), + // new InstantCommand(() -> claw.setOutput(0.0)) + // ); + + + // ID7 from outer + // addCommands( + // new InstantCommand(() -> drivetrain.resetPosition()), + // new DriveDistance(drivetrain, gyro, 7.3, 0.0, 3.14), // drive to wall + // new DriveDistance(drivetrain, gyro, 0.0, -2.7, 3.14), // strafe left to ID7 + // new DriveDistance(drivetrain, gyro, 0.0, 0.0, 3.14), // correct any drift from the previous command + // new DriveTowardsThing(drivetrain, camera) + // ); } } diff --git a/src/main/java/frc/robot/commands/instant/ClawCommand.java b/src/main/java/frc/robot/commands/instant/ClawCommand.java index 663489f..b3be853 100644 --- a/src/main/java/frc/robot/commands/instant/ClawCommand.java +++ b/src/main/java/frc/robot/commands/instant/ClawCommand.java @@ -3,17 +3,20 @@ import frc.robot.subsystems.CoralClaw; import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.DoubleSupplier; public class ClawCommand extends Command { CoralClaw claw; double speed; int outin; + DoubleSupplier triggerSupplier; - public ClawCommand(CoralClaw claw, int outin) { + public ClawCommand(CoralClaw claw, DoubleSupplier triggerSupplier, int outin) { addRequirements(claw); this.claw = claw; this.speed = claw.INTAKE_SPEED; this.outin = outin; + this.triggerSupplier = triggerSupplier; } @Override @@ -22,13 +25,9 @@ public ClawCommand(CoralClaw claw, int outin) { @Override public void execute() { if(outin == 0) { // OUT - System.out.println("CLAWCOMMAND OUTTAKE"); - claw.outtake(); + claw.setOutput(-(triggerSupplier.getAsDouble() * 0.5 + 0.2)); } else { // INTAKE - System.out.println("CLAWCOMMAND INTAKE"); - claw.intake(); - // if(!claw.isCoralDetected()) { - // } + claw.setOutput(triggerSupplier.getAsDouble() * 0.5 + 0.2); } diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java index e3e328e..e6d291b 100644 --- a/src/main/java/frc/robot/subsystems/Camera.java +++ b/src/main/java/frc/robot/subsystems/Camera.java @@ -4,6 +4,14 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import java.io.IOException; +import java.util.Optional; public class Camera extends SubsystemBase { @@ -18,9 +26,17 @@ public class Camera extends SubsystemBase { private long tagId; private double distanceCm; private double angleOffset; + private AprilTagFieldLayout fieldLayout; + private Pose2d estimatedPose; public Camera() { table = NetworkTableInstance.getDefault().getTable("datatable"); + try { + fieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025ReefscapeAndyMark.m_resourceFile); + } catch (IOException e) { + // Handle exception + System.err.println("Failed to load AprilTag field layout: " + e.getMessage()); + } } @Override @@ -43,6 +59,42 @@ public void periodic() { SmartDashboard.putNumber("AprilTag ID", tagId); SmartDashboard.putNumber("Distance (cm)", distanceCm); SmartDashboard.putNumber("Angle Offset", angleOffset); + + if (lock && tagId > 0) { + Optional tagPose = fieldLayout.getTagPose((int)tagId); + if (tagPose.isPresent()) { + // Calculate robot position using AprilTag data + double focalLength = 320.0; // Example value, needs calibration + double tagSize = 0.166; // Tag size in meters + + // Calculate distance using the data from the Raspberry Pi + double distance = distanceCm / 100.0; // Convert cm to meters + + // Calculate angle based on the angleOffset from the Pi + double angleRad = Math.toRadians(angleOffset); + + // Create the transform + Transform3d cameraToTag = new Transform3d( + distance, 0, 0, + new Rotation3d(0, 0, angleRad) + ); + + // Camera position on robot - adjust for your robot + Transform3d robotToCamera = new Transform3d( + 0.25, 0, 0.5, + new Rotation3d() + ); + + // Calculate robot pose + Pose3d cameraPose = tagPose.get().transformBy(cameraToTag.inverse()); + Pose3d robotPose = cameraPose.transformBy(robotToCamera.inverse()); + estimatedPose = robotPose.toPose2d(); + + SmartDashboard.putNumber("Pose X", estimatedPose.getX()); + SmartDashboard.putNumber("Pose Y", estimatedPose.getY()); + SmartDashboard.putNumber("Pose Angle", estimatedPose.getRotation().getDegrees()); + } + } } public long getX() { @@ -76,4 +128,8 @@ public double getDistanceCm() { public double getAngleOffset() { return angleOffset; } + + public Pose2d getEstimatedPose() { + return estimatedPose; + } } diff --git a/src/main/java/frc/robot/subsystems/CoralClaw.java b/src/main/java/frc/robot/subsystems/CoralClaw.java index 3faf09f..a850410 100644 --- a/src/main/java/frc/robot/subsystems/CoralClaw.java +++ b/src/main/java/frc/robot/subsystems/CoralClaw.java @@ -84,7 +84,8 @@ public class CoralClaw extends SubsystemBase { // Track previous coral state to detect transitions private boolean previousCoralDetected = false; - + + private boolean[] motorsOn = {true, true}; /** * Creates a new CoralClaw subsystem. * @@ -101,8 +102,8 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput li // Configure the secondary motor to follow the primary motor with inversion SparkMaxConfig config = new SparkMaxConfig(); primaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - config.follow(primaryMotor, true); // true makes it follow with inversion secondaryMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // config.follow(primaryMotor, true); // true makes it follow with inversion // Initialize simulation-specific components if in simulation isSimulation = RobotBase.isSimulation(); @@ -118,8 +119,8 @@ public CoralClaw(SparkMax primaryMotor, SparkMax secondaryMotor, DigitalInput li * Will run until a coral is detected or manually stopped */ public void intake() { - System.out.println("CLAW INTAKE"); - primaryMotor.set(INTAKE_SPEED); + if(motorsOn[0]) { primaryMotor.set(INTAKE_SPEED); } + if(motorsOn[1]) { secondaryMotor.set(INTAKE_SPEED); } // Secondary motor automatically follows with inversion // autoIntakeMode = true; SmartDashboard.putString("Coral Claw State", "Intaking (Auto)"); @@ -129,18 +130,26 @@ public void intake() { * Spins the wheels to outtake/eject a coral game piece */ public void outtake() { - System.out.println("CLAW OUTTAKE"); - primaryMotor.set(OUTTAKE_SPEED); + if(motorsOn[0]) { primaryMotor.set(OUTTAKE_SPEED); } + if(motorsOn[1]) { secondaryMotor.set(OUTTAKE_SPEED); } + // Secondary motor automatically follows with inversion // autoIntakeMode = false; SmartDashboard.putString("Coral Claw State", "Outtaking"); } + + public void setMotorsOn(boolean left, boolean right) { + motorsOn[0] = left; + motorsOn[1] = right; + } + /** * Stops the intake/outtake wheels */ public void stop() { primaryMotor.set(0); + secondaryMotor.set(0); // Secondary motor automatically follows with inversion // autoIntakeMode = false; SmartDashboard.putString("Coral Claw State", "Stopped"); @@ -192,9 +201,9 @@ public void setSimulatedCoralDetected(boolean detected) { * set claw output to @param out **/ public void setOutput(double out) { - System.out.println("claw set output; " + out); - primaryMotor.set(INTAKE_SPEED); - + if(motorsOn[0]) { primaryMotor.set(out); } + if(motorsOn[1]) { secondaryMotor.set(out); } + primaryMotor.set(out); } @Override diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 2606437..727d383 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -25,8 +25,7 @@ public class Drivetrain extends SubsystemBase { public static final double DRIVE_RATIO = 1 / 8.14; // 1:8.14 is the ratio of the drive motor from the motor to the wheel output shaft public static final double STEER_RATIO = 7 / 150.0; // 7:150 is the ratio of the steering motor from the motor to the wheel output shaft - private static int slowAmount = 1; // amount to divide drive amounts by (1= no chage, 2=half speed, 3=third speed etc..) - + private static SwerveDriveKinematics kinematics; private static SwerveDriveOdometry odometry; @@ -76,8 +75,8 @@ public void drive(double x, double y, double omega) { // (radians per second) ChassisSpeeds chassisSpeed = ChassisSpeeds .fromFieldRelativeSpeeds( - (FREE_SPEED * x) / slowAmount, - (FREE_SPEED * y) / slowAmount, + (FREE_SPEED * x), + (FREE_SPEED * y), omega, gyro.getRotation2d()); SwerveModuleState[] states = kinematics.toSwerveModuleStates(chassisSpeed); @@ -126,16 +125,6 @@ public void driveRobotRelative(double x, double y, double omega) { rightBackSwerve.setState(states[3]); } - public void setSlow(boolean on) { - if(on) { - slowAmount = 3; - } else { - slowAmount = 1; - } - - - - } public Pose2d getPose2d() { return odometry.getPoseMeters(); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 6d8d5ae..52e28ea 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -153,19 +153,19 @@ public void setTarget(ElevatorLevel level) { // Tune these values to match the actual robot requirmenets to seed the coral switch (level) { case STATION: - groundHeight = Units.inchesToMeters(28.0); // all these measurements are from measuring the field components + groundHeight = Units.inchesToMeters(27.5); // all these measurements are from measuring the field components break; case LOW: groundHeight = Units.inchesToMeters(18.0); break; case MED: - groundHeight = Units.inchesToMeters(36.5); + groundHeight = Units.inchesToMeters(29.75); break; case HIGH: - groundHeight = Units.inchesToMeters(45.5); + groundHeight = Units.inchesToMeters(42.25); break; case EXTRA_HIGH: - groundHeight = Units.inchesToMeters(67.0); + groundHeight = Units.inchesToMeters(63.75); break; default: groundHeight = 0.0;