diff --git a/.gitignore b/.gitignore index cf497fe..d243bc4 100644 --- a/.gitignore +++ b/.gitignore @@ -179,3 +179,5 @@ ctre_sim/ # gversion build constants file src/main/java/frc/robot/BuildConstants.java + +ttb-maven diff --git a/AdvantageScope.json b/AdvantageScope.json index 238eb49..5955c4a 100644 --- a/AdvantageScope.json +++ b/AdvantageScope.json @@ -21,11 +21,17 @@ "/RealOutputs/Auto/DriveTrajectory/1-2-b", "/RealOutputs/Auto/DriveTrajectory/TargetPose/rotation", "/RealOutputs/Auto/DriveTrajectory/CurrentPose", - "/RealOutputs/Auto/DriveTrajectory/CurrentPose/rotation" + "/RealOutputs/Auto/DriveTrajectory/CurrentPose/rotation", + "/RealOutputs/Elevator/Position", + "/RealOutputs/Coral/RPM/Left", + "/RealOutputs/Coral/RPM/Right", + "/RealOutputs/Elevator/Output", + "/RealOutputs/Algae", + "/RealOutputs/Algae/Wrist" ] }, "tabs": { - "selected": 2, + "selected": 4, "tabs": [ { "type": 0, @@ -78,7 +84,7 @@ "cameraPosition": [ 8.846666903479488, 5.635495269604634, - -2.048242193592743 + -2.0482421935927437 ], "cameraTarget": [ 4.77960094630555, @@ -101,12 +107,12 @@ "fields": [ { "key": "/RealOutputs/Auto/DriveTrajectory/TargetPose/rotation/value", - "color": "#e5b31b", + "color": "#2b66a2", "show": true }, { "key": "/RealOutputs/Auto/DriveTrajectory/CurrentPose/rotation/value", - "color": "#af2437", + "color": "#e5b31b", "show": true } ] @@ -115,21 +121,208 @@ "fields": [ { "key": "/RealOutputs/Auto/DriveTrajectory/shouldReplan", + "color": "#af2437", + "show": true + } + ] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "PathPlanning" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Elevator/Position/Current", "color": "#2b66a2", "show": true + }, + { + "key": "/RealOutputs/Elevator/Position/Target", + "color": "#e5b31b", + "show": true } ] }, + "discrete": { + "fields": [] + }, "right": { "lockedRange": null, "unitConversion": { "type": null, "factor": 1 }, + "fields": [ + { + "key": "/RealOutputs/Elevator/Current/Left", + "color": "#af2437", + "show": true + }, + { + "key": "/RealOutputs/Elevator/Current/Right", + "color": "#80588e", + "show": true + } + ] + } + }, + "title": "Elevator" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Algae/Wrist/Frequency", + "color": "#e5b31b", + "show": false + }, + { + "key": "/RealOutputs/Algae/Wrist/Output", + "color": "#af2437", + "show": true + }, + { + "key": "/RealOutputs/Algae/Wrist/Voltage", + "color": "#e48b32", + "show": true + } + ] + }, + "discrete": { "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Algae/Wrist/Position", + "color": "#2b66a2", + "show": true + }, + { + "key": "/RealOutputs/Algae/Wrist/Target", + "color": "#80588e", + "show": true + } + ] + } + }, + "title": "Wrist" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Coral/RPM/target", + "color": "#af2437", + "show": true + }, + { + "key": "/RealOutputs/Coral/RPM/Left/Position", + "color": "#2b66a2", + "show": true + }, + { + "key": "/RealOutputs/Coral/RPM/Right/Position", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Coral/Laser/distance", + "color": "#e48b32", + "show": true + } + ] + } + }, + "title": "Coral" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Algae/Intake/Output", + "color": "#e5b31b", + "show": true + } + ] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [ + { + "key": "/RealOutputs/Algae/Intake/Current", + "color": "#2b66a2", + "show": true + } + ] } }, - "title": "Line Graph" + "title": "Intake" }, { "type": 3, diff --git a/simgui.json b/simgui.json index a0e69b1..5aa4636 100644 --- a/simgui.json +++ b/simgui.json @@ -28,5 +28,8 @@ "open": true } } + }, + "NetworkTables Info": { + "visible": true } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 32416e7..8bcaa80 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,6 +10,91 @@ public static class Robot { public static final double k_length = 28; // Inches } + public static class Elevator { + public static final int kElevatorLeftMotorId = 9; + public static final int kElevatorRightMotorId = 10; + + public static final double kP = 0.15; + public static final double kI = 0; + public static final double kD = 0.0; + public static final double kIZone = 5.0; + public static final double kG = 0.5; + + public static final double kMaxVelocity = 65; + public static final double kMaxAcceleration = 200; + + public static final int kMaxCurrent = 40; + public static final double kMaxPowerUp = 0.1; + public static final double kMaxPowerDown = 0.1; + + public static final double kStowHeight = 0.0; + public static final double kL2Height = 9.0; + public static final double kL3Height = 25.14; + public static final double kL4Height = 52.0; + public static final double kMaxHeight = 56.2; + public static final double kGroundAlgaeHeight = 0.0; + public static final double kScoreAlgaeHeight = 0.0; + public static final double kLowAlgaeHeight = 24.8; + public static final double kHighAlgaeHeight = 42.5; + } + + public static class Coral { + public static final int kLeftMotorId = 11; + public static final int kRightMotorId = 12; + + public static final int kLaserId = 0; + public static final int kColorId = 16; + + public static final double kMaxCurrent = 20; + + public static final double kP = 0.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kIZone = 0; + + public static final double kIntakeSpeed = 0.3; + public static final double kReverseSpeed = -0.3; + public static final double kL1Speed = 0.4; + public static final double kL24Speed = 0.4; + public static final double kIndexSpeed = 0.1; + public static final double kSpeedDifference = kL1Speed * 0.5; + } + + public static class Algae { + // WRIST + public static final int kWristMotorId = 13; + public static final int kIntakeMotorId = 14; + + public static final int kWristEncoderId = 9; + + public static final int kMaxWristCurrent = 10; + + public static final double kWristP = 0.01; + public static final double kWristI = 0.0; + public static final double kWristD = 0.0; + + public static final double kWristKS = 0.0; + public static final double kWristKG = 0.0; + public static final double kWristKV = 0.100; + public static final double kWristKA = 0.0; + + public static final double kWristOffset = 141.0; + + public static final double kWristMaxVelocity = 690.0; + public static final double kWristMaxAcceleration = 1380.0; + + public static final double kStowAngle = 233.0; + public static final double kDeAlgaeAngle = 215.0; + public static final double kGroundIntakeAngle = 162.0; + + // INTAKE + public static final int kMaxIntakeCurrent = 20; + + public static final double kIntakeSpeed = 0.6; + public static final double kEjectSpeed = -0.3; + public static final double kGroundIntakeSpeed = -0.3; + } + public static class Intake { // Motors public static final int kIntakeMotorId = 9; @@ -69,12 +154,12 @@ public static class Intake { // Drivetrain public static class Drive { - public static final double kP = 0.00085;//3.6954;//0.085; - public static final double kI = 0.0;//0.0; - public static final double kD = 0.0;//0.0; + public static final double kP = 0.0; // 0.00085; + public static final double kI = 0.0; + public static final double kD = 0.0; - public static final double kS = 0.1695;//0.01; - public static final double kV = 2.8559;//2.6; + public static final double kS = 0.1695;// 0.01; + public static final double kV = 2.8559;// 2.6; public static final double kA = 0.4864; public static final int kFLMotorId = 8; @@ -93,7 +178,7 @@ public static class Field { } public static class LEDs { - public static final int k_PWMId = 0; + public static final int k_PWMId = 9; public static final int k_totalLength = 300; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5ac4fab..c888356 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.autonomous.AutoRunner; @@ -24,8 +25,11 @@ import frc.robot.controls.controllers.DriverController; import frc.robot.controls.controllers.OperatorController; import frc.robot.simulation.Field; +import frc.robot.subsystems.Algae; import frc.robot.subsystems.Compressor; +import frc.robot.subsystems.Coral; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Subsystem; import frc.robot.subsystems.leds.LEDs; @@ -52,8 +56,11 @@ public class Robot extends LoggedRobot { // private final Intake m_intake = Intake.getInstance(); private final Compressor m_compressor = Compressor.getInstance(); private final Drivetrain m_drive = Drivetrain.getInstance(); + private final Coral m_coral = Coral.getInstance(); + private final Algae m_algae = Algae.getInstance(); // private final Shooter m_shooter = Shooter.getInstance(); - // private final Climber m_climber = Climber.getInstance(); + private final Elevator m_elevator = Elevator.getInstance(); + public final LEDs m_leds = LEDs.getInstance(); // Auto stuff @@ -71,11 +78,12 @@ public void robotInit() { setupLogging(); // Add all subsystems to the list - // m_allSubsystems.add(m_intake); - m_allSubsystems.add(m_compressor); + // m_allSubsystems.add(m_compressor); m_allSubsystems.add(m_drive); - // m_allSubsystems.add(m_shooter); - // m_allSubsystems.add(m_climber); + m_allSubsystems.add(m_coral); + m_allSubsystems.add(m_algae); + m_allSubsystems.add(m_elevator); + m_allSubsystems.add(m_leds); // Set up the Field2d object for simulation @@ -92,7 +100,7 @@ public void robotPeriodic() { updateSim(); // Used by sysid - if (this.isTestEnabled()){ + if (this.isTestEnabled()) { CommandScheduler.getInstance().run(); } } @@ -130,6 +138,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + m_leds.breathe(); } double speed = 0; @@ -152,50 +161,117 @@ public void teleopPeriodic() { m_drive.drive(xSpeed, rot); - // // Shooter variable speed - // if (m_driverController.getWantsMoreSpeed() || m_operatorController.getWantsMoreSpeed()) { - // m_leds.setColor(Color.kPink); - // speed = 3000; - // } else if (m_driverController.getWantsLessSpeed() || m_operatorController.getWantsLessSpeed()) { - // m_leds.setColor(Color.kOrange); - // speed = 430; - // } else if (m_driverController.getWantsShooterStop() || m_operatorController.getWantsShooterStop()) { - // m_leds.defaultLEDS(); - // speed = 0; + // FINAL CONTROLS + if (m_driverController.getWantsStow()) { + m_elevator.goToElevatorStow(); + // m_algae.stow(); + } else if (m_driverController.getWantsL2()) { + m_elevator.goToElevatorL2(); + m_algae.stow(); + } else if (m_driverController.getWantsL3()) { + m_elevator.goToElevatorL3(); + m_algae.stow(); + } else if (m_driverController.getWantsL4()) { + m_elevator.goToElevatorL4(); + m_algae.stow(); + } else if (m_driverController.getWantsA1()) { + m_elevator.goToAlgaeLow(); + m_algae.grabAlgae(); + } else if (m_driverController.getWantsA2()) { + m_elevator.goToAlgaeHigh(); + m_algae.grabAlgae(); + } else if (m_driverController.getWantsStopAlgae()) { + m_algae.stopAlgae(); + } else if (m_driverController.getWantsEjectAlgae()) { + m_algae.score(); + } else if (m_driverController.getWantsGroundAlgae()) { + m_algae.groundIntake(); + } + + if (m_driverController.getWantsScoreCoral()) { + if (m_elevator.getState() == Elevator.ElevatorState.STOW) { + m_coral.scoreL1(); + } else { + m_coral.scoreL24(); + } + } else if (m_driverController.getWantsIntakeCoral()) { + m_coral.intake(); + m_elevator.goToElevatorStow(); + } + + // ALGAE + // if (m_driverController.getWantsAlgaeStow()) { + // m_algae.stow(); + // } else if (m_driverController.getWantsAlgaeGrab()) { + // m_algae.grabAlgae(); + // } else if (m_driverController.getWantsAlgaeScore()) { + // m_algae.score(); + // } else if (m_driverController.getWantsAlgaeGroundIntake()) { + // m_algae.groundIntake(); + // } + + // ELEVATOR + // m_elevator.setElevatorPower(m_operatorController.getElevatorAxis()); + // if (m_operatorController.getWantsElevatorStow()) { + // m_elevator.goToElevatorStow(); + // } else if (m_operatorController.getWantsElevatorL2()) { + // m_elevator.goToElevatorL2(); + // } else if (m_operatorController.getWantsElevatorL3()) { + // m_elevator.goToElevatorL3(); + // } else if (m_operatorController.getWantsElevatorL4()) { + // m_elevator.goToElevatorL4(); // } - // speed = MathUtil.clamp(speed, -6000, 10000); - // m_shooter.setSpeed(speed); + + // CORAL + // if (m_operatorController.getWantsCoralIntake()) { + // m_coral.intake(); + // } else if (m_operatorController.getWantsCoralReverse()) { + // m_coral.reverse(); + // } else if (m_operatorController.getWantsCoralIndex()) { + // m_coral.index(); + // } else if (m_operatorController.getWantsCoralL1()) { + // m_coral.scoreL1(); + // } else if (m_operatorController.getWantsCoralL24()) { + // m_coral.scoreL24(); + // } else { + // m_coral.stopCoral(); + // } + + if (m_operatorController.getWantsElevatorReset() || m_driverController.getWantsElevatorReset()) { + RobotTelemetry.print("Resetting elevator"); + m_elevator.reset(); + } // // Intake // if (m_driverController.getWantsFullIntake()) { - // m_intake.goToGround(); + // m_intake.goToGround(); // } else if (m_driverController.getWantsIntake()) { - // if (m_intake.getIntakeHasNote()) { - // m_intake.pulse(); - // } else { - // m_intake.intake(); - // } + // if (m_intake.getIntakeHasNote()) { + // m_intake.pulse(); + // } else { + // m_intake.intake(); + // } // } else if (m_driverController.getWantsEject()) { - // m_intake.eject(); + // m_intake.eject(); // } else if (m_driverController.getWantsSource()) { - // m_intake.goToSource(); + // m_intake.goToSource(); // } else if (m_driverController.getWantsStow()) { - // m_intake.goToStow(); + // m_intake.goToStow(); // } else if (m_intake.getIntakeState() != IntakeState.INTAKE) { - // m_intake.stopIntake(); + // m_intake.stopIntake(); // } // // Climber // if (m_operatorController.getWantsClimberClimb()) { - // m_climber.climb(); + // m_climber.climb(); // } else if (m_operatorController.getWantsClimberRelease()) { - // m_climber.release(); + // m_climber.release(); // } else if (m_operatorController.getWantsClimberTiltLeft()) { - // m_climber.tiltLeft(); + // m_climber.tiltLeft(); // } else if (m_operatorController.getWantsClimberTiltRight()) { - // m_climber.tiltRight(); + // m_climber.tiltRight(); // } else { - // m_climber.stopClimber(); + // m_climber.stopClimber(); // } // if (m_operatorController.getWantsBrakeMode()) { @@ -207,7 +283,9 @@ public void teleopPeriodic() { @Override public void disabledInit() { - m_leds.rainbow(); + // m_leds.rainbow(); + m_leds.setColor(Color.kRed); + speed = 0; m_allSubsystems.forEach(subsystem -> subsystem.stop()); diff --git a/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java b/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java index 3a8782e..fc7f923 100644 --- a/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java +++ b/src/main/java/frc/robot/autonomous/tasks/DriveTrajectoryTask.java @@ -43,14 +43,14 @@ public DriveTrajectoryTask(String pathName) { filePath = Filesystem.getDeployDirectory().toPath().resolve("paths/" + pathName); } else { RobotTelemetry.print("Running in simulation!"); - filePath = Filesystem.getLaunchDirectory().toPath().resolve("PathWeaver/output/" + pathName); + filePath = Filesystem.getDeployDirectory().toPath().resolve("pathplanner/paths/" + pathName); } RobotTelemetry.print("Loading path from:\n" + filePath.toString()); m_autoPath = PathPlannerPath.fromPathFile(pathName); // RobotTelemetry.print(m_autoPath.numPoints()); - if (DriverStation.getAlliance().get() == Alliance.Red) { + if (DriverStation.getAlliance().orElse(Alliance.Red) == Alliance.Red) { RobotTelemetry.print("Translating path for Red Alliance!"); m_autoPath = m_autoPath.flipPath(); } diff --git a/src/main/java/frc/robot/controls/controllers/DriverController.java b/src/main/java/frc/robot/controls/controllers/DriverController.java index cc10f25..eed120e 100644 --- a/src/main/java/frc/robot/controls/controllers/DriverController.java +++ b/src/main/java/frc/robot/controls/controllers/DriverController.java @@ -24,69 +24,76 @@ public double getTurnAxis() { return -this.getFilteredAxis(4); } - public double getShooterAxis() { - return (this.getFilteredAxis(3) - 0.5) * 2; + public boolean getWantsSpeedMode() { + return this.getFilteredAxis(2) > k_triggerActivationThreshold; } - public boolean getWantsFullIntake() { - return this.getRawButton(1); - } + // public boolean getWantsSlowMode() { + // return this.getFilteredAxis(3) > k_triggerActivationThreshold; + // } - public boolean getWantsIntake() { - return this.getRawButton(6); + public boolean getWantsStow() { + return this.getRawButton(3); } - public boolean getWantsEject() { - return this.getRawButton(2); + public boolean getWantsL2() { + return this.getRawButton(1); } - public boolean getWantsSource() { - return this.getRawButton(3); + public boolean getWantsL3() { + return this.getRawButton(2); } - public boolean getWantsStow() { + public boolean getWantsL4() { return this.getRawButton(4); } - // public boolean getWantsFire() { - // return this.getHatUpPressed(); - // } + public boolean getWantsScoreCoral() { + return this.getFilteredAxis(3) > k_triggerActivationThreshold; + } - public boolean getWantsMoreSpeed() { - return this.getHatUp(); + public boolean getWantsIntakeCoral() { + return this.getFilteredAxis(2) > k_triggerActivationThreshold; } - public boolean getWantsLessSpeed() { + public boolean getWantsA1() { return this.getHatDown(); } - public boolean getWantsShooterStop() { - return this.getRawButton(5); + public boolean getWantsA2() { + return this.getHatUp(); } - public boolean getWantsSpeedMode() { - return this.getFilteredAxis(2) > k_triggerActivationThreshold; + public boolean getWantsStopAlgae() { + return this.getHatRight(); } - public boolean getWantsSlowMode() { - return this.getFilteredAxis(3) > k_triggerActivationThreshold; + public boolean getWantsElevatorReset() { + return this.getRawButton(7); } - // Buttons - public boolean getWantsSomethingToggle() { - return this.getRawButtonPressed(1); + public boolean getWantsEjectAlgae() { + return this.getRawButton(6); } - public boolean getWantsSomething2Toggle() { - return this.getRawButtonPressed(3); + public boolean getWantsGroundAlgae() { + return this.getRawButton(5); } - public boolean getWantsResetGyro() { - return this.getRawButton(4); - } + // public boolean getWantsAlgaeStow() { + // return this.getRawButton(1); + // } + + // public boolean getWantsAlgaeGrab() { + // return this.getRawButton(3); + // } + + // public boolean getWantsAlgaeScore() { + // return this.getRawButton(2); + // } - // public boolean getWantsBrake() { - // return this.getRawButton(5); + // public boolean getWantsAlgaeGroundIntake() { + // return this.getRawButton(4); // } public void outputTelemetry() { diff --git a/src/main/java/frc/robot/controls/controllers/OperatorController.java b/src/main/java/frc/robot/controls/controllers/OperatorController.java index 8d46404..c800d21 100644 --- a/src/main/java/frc/robot/controls/controllers/OperatorController.java +++ b/src/main/java/frc/robot/controls/controllers/OperatorController.java @@ -13,50 +13,53 @@ public OperatorController(int port, boolean useDeadband, boolean useSquaredInput // Axis private final double k_triggerActivationThreshold = 0.5; - public boolean getWantsTriggerSomething1() { - return this.getFilteredAxis(3) > k_triggerActivationThreshold; + public double getElevatorAxis() { + return -this.getFilteredAxis(1); } - public boolean getWantsTriggerSomething2() { - return this.getFilteredAxis(2) > k_triggerActivationThreshold; + // public boolean getWantsTriggerSomething2() { + // return this.getFilteredAxis(2) > k_triggerActivationThreshold; + // } + + // CORAL + public boolean getWantsCoralIntake() { + return this.getRawButton(4); } - // Buttons - public boolean getWantsBrakeMode() { + public boolean getWantsCoralReverse() { return this.getRawButton(1); } - public boolean getWantsCoastMode() { - return this.getRawButton(2); + public boolean getWantsCoralIndex() { + return this.getRawButton(8); } - public boolean getWantsMoreSpeed() { - return this.getRawButtonPressed(6); + public boolean getWantsCoralL1() { + return this.getRawButton(3); } - public boolean getWantsLessSpeed() { - return this.getRawButtonPressed(5); + public boolean getWantsCoralL24() { + return this.getRawButton(2); } - public boolean getWantsShooterStop() { - return this.getRawButtonPressed(4); + // ELEVATOR + public boolean getWantsElevatorReset() { + return this.getRawButton(7); } - // D pad - - public boolean getWantsClimberRelease() { + public boolean getWantsElevatorStow() { return this.getHatDown(); } - public boolean getWantsClimberTiltRight() { - return this.getHatRight(); + public boolean getWantsElevatorL2() { + return this.getHatLeft(); } - public boolean getWantsClimberClimb() { - return this.getHatUp(); + public boolean getWantsElevatorL3() { + return this.getHatRight(); } - public boolean getWantsClimberTiltLeft() { - return this.getHatLeft(); + public boolean getWantsElevatorL4() { + return this.getHatUp(); } } diff --git a/src/main/java/frc/robot/subsystems/Algae.java b/src/main/java/frc/robot/subsystems/Algae.java new file mode 100644 index 0000000..64d659d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Algae.java @@ -0,0 +1,201 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.simulation.SimulatableCANSparkMax; +import frc.robot.wrappers.REVThroughBoreEncoder; + +public class Algae extends Subsystem { + + /*-------------------------------- Private instance variables ---------------------------------*/ + private static Algae mInstance; + private PeriodicIO mPeriodicIO; + + public static Algae getInstance() { + if (mInstance == null) { + mInstance = new Algae(); + } + return mInstance; + } + + public enum IntakeState { + NONE, + STOW, + DEALGAE, + GROUND + } + + private SimulatableCANSparkMax mWristMotor; + private final ProfiledPIDController mWristPIDController; + private final ArmFeedforward mWristFeedForward; + + private SimulatableCANSparkMax mIntakeMotor; + + private final REVThroughBoreEncoder mWristAbsEncoder = new REVThroughBoreEncoder(Constants.Algae.kWristEncoderId); + + private Algae() { + super("Algae"); + + mPeriodicIO = new PeriodicIO(); + + // WRIST + mWristMotor = new SimulatableCANSparkMax(Constants.Algae.kWristMotorId, MotorType.kBrushless); + mWristMotor.restoreFactoryDefaults(); + mWristMotor.setIdleMode(IdleMode.kCoast); + mWristMotor.setSmartCurrentLimit(Constants.Algae.kMaxWristCurrent); + + // Wrist PID + mWristPIDController = new ProfiledPIDController( + Constants.Algae.kWristP, + Constants.Algae.kWristI, + Constants.Algae.kWristD, + new TrapezoidProfile.Constraints( + Constants.Algae.kWristMaxVelocity, + Constants.Algae.kWristMaxAcceleration)); + + // Wrist Feedforward + mWristFeedForward = new ArmFeedforward( + Constants.Algae.kWristKS, + Constants.Algae.kWristKG, + Constants.Algae.kWristKV, + Constants.Algae.kWristKA); + + // INTAKE + mIntakeMotor = new SimulatableCANSparkMax(Constants.Algae.kIntakeMotorId, MotorType.kBrushless); + mIntakeMotor.restoreFactoryDefaults(); + mIntakeMotor.setIdleMode(IdleMode.kBrake); + mIntakeMotor.setSmartCurrentLimit(Constants.Algae.kMaxIntakeCurrent); + + mIntakeMotor.setInverted(true); + + mWristMotor.setInverted(true); + + mWristMotor.burnFlash(); + mIntakeMotor.burnFlash(); + } + + private static class PeriodicIO { + double wrist_target_angle = 0.0; + double wrist_voltage = 0.0; + + double intake_power = 0.0; + + IntakeState state = IntakeState.STOW; + } + + /*-------------------------------- Generic Subsystem Functions --------------------------------*/ + + @Override + public void periodic() { + double pidCalc = mWristPIDController.calculate(getWristAngle(), mPeriodicIO.wrist_target_angle); + double ffCalc = mWristFeedForward.calculate(Math.toRadians(getWristReferenceToHorizontal()), + Math.toRadians(mWristPIDController.getSetpoint().velocity)); + + mPeriodicIO.wrist_voltage = pidCalc + ffCalc; + } + + @Override + public void writePeriodicOutputs() { + mWristMotor.set(mPeriodicIO.wrist_voltage); + + mIntakeMotor.set(mPeriodicIO.intake_power); + } + + @Override + public void stop() { + mPeriodicIO.wrist_voltage = 0.0; + mPeriodicIO.wrist_target_angle = Constants.Algae.kStowAngle; + + mWristMotor.set(0.0); + mIntakeMotor.set(0.0); + } + + @Override + public void outputTelemetry() { + putNumber("Wrist/Position", getWristAngle()); + putNumber("Wrist/Target", mPeriodicIO.wrist_target_angle); + putNumber("Wrist/Current", mWristMotor.getOutputCurrent()); + putNumber("Wrist/Output", mWristMotor.getAppliedOutput()); + putNumber("Wrist/Voltage", mPeriodicIO.wrist_voltage); + putNumber("Wrist/Frequency", mWristAbsEncoder.getFrequency()); + + putNumber("Intake/Current", mIntakeMotor.getOutputCurrent()); + putNumber("Intake/Output", mIntakeMotor.getAppliedOutput()); + putNumber("Intake/Power", mPeriodicIO.intake_power); + } + + @Override + public void reset() { + } + + /*---------------------------------- Custom Public Functions ----------------------------------*/ + + public void stow() { + mPeriodicIO.wrist_target_angle = Constants.Algae.kStowAngle; + + mPeriodicIO.state = IntakeState.STOW; + // mPeriodicIO.intake_power = 0.0; + } + + public void grabAlgae() { + mPeriodicIO.wrist_target_angle = Constants.Algae.kDeAlgaeAngle; + mPeriodicIO.intake_power = Constants.Algae.kIntakeSpeed; + + mPeriodicIO.state = IntakeState.DEALGAE; + } + + public void score() { + if (mPeriodicIO.state == IntakeState.GROUND) { + mPeriodicIO.intake_power = -Constants.Algae.kEjectSpeed; + } else { + mPeriodicIO.intake_power = Constants.Algae.kEjectSpeed; + } + } + + public void groundIntake() { + mPeriodicIO.wrist_target_angle = Constants.Algae.kGroundIntakeAngle; + mPeriodicIO.intake_power = Constants.Algae.kGroundIntakeSpeed; + + mPeriodicIO.state = IntakeState.GROUND; + } + + public void stopAlgae() { + mPeriodicIO.intake_power = 0.0; + mPeriodicIO.wrist_target_angle = Constants.Algae.kStowAngle; + } + + /*---------------------------------- Custom Private Functions ---------------------------------*/ + + public double getWristAngle() { + return Units.rotationsToDegrees(mWristAbsEncoder.getAbsolutePosition()); + } + + public double getWristReferenceToHorizontal() { + return getWristAngle() - Constants.Algae.kWristOffset; + } + + public IntakeState getState() { + return mPeriodicIO.state; + } + + // public double getSpeedFromState(IntakeState state) { + // switch (state) { + // case NONE: + // return 0.0; + // case INTAKE: + // return RobotConstants.config.Intake.k_intakeSpeed; + // case INDEX: + // return RobotConstants.config.Intake.k_ejectSpeed; + // case READY: + // return RobotConstants.config.Intake.k_feedShooterSpeed; + // default: + // return 0.0; + // } + // } +} diff --git a/src/main/java/frc/robot/subsystems/Coral.java b/src/main/java/frc/robot/subsystems/Coral.java new file mode 100644 index 0000000..70163af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Coral.java @@ -0,0 +1,257 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkPIDController; +import com.thethriftybot.ThriftyNova.PIDConfig; + +import au.grapplerobotics.ConfigurationFailedException; +import au.grapplerobotics.LaserCan; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.Constants; +import frc.robot.simulation.SimulatableCANSparkMax; +import frc.robot.subsystems.leds.LEDs; + +public class Coral extends Subsystem { + + /*-------------------------------- Private instance variables ---------------------------------*/ + private static Coral mInstance; + private PeriodicIO mPeriodicIO; + public final LEDs m_leds = LEDs.getInstance(); + + public static Coral getInstance() { + if (mInstance == null) { + mInstance = new Coral(); + } + return mInstance; + } + + public enum IntakeState { + NONE, + INTAKE, + REVERSE, + INDEX, + READY, + SCORE + } + + // private ThriftyNova mLeftMotor; + // private ThriftyNova mRightMotor; + private SimulatableCANSparkMax mLeftMotor; + private SparkPIDController mLeftPIDController; + private SimulatableCANSparkMax mRightMotor; + private SparkPIDController mRightPIDController; + + private PIDConfig mPIDConfig; + + private LaserCan mLaserCAN; + + private Coral() { + super("Coral"); + + mPeriodicIO = new PeriodicIO(); + + // mLeftMotor = new ThriftyNova(Constants.Coral.kLeftMotorId); + // mRightMotor = new ThriftyNova(Constants.Coral.kRightMotorId); + mLeftMotor = new SimulatableCANSparkMax(Constants.Coral.kLeftMotorId, MotorType.kBrushless); + mRightMotor = new SimulatableCANSparkMax(Constants.Coral.kRightMotorId, MotorType.kBrushless); + + // mLeftMotor.factoryReset(); + // mRightMotor.factoryReset(); + + // mLeftMotor.setBrakeMode(true); + // mRightMotor.setBrakeMode(true); + mLeftMotor.setIdleMode(IdleMode.kBrake); + mRightMotor.setIdleMode(IdleMode.kBrake); + + // mLeftPIDController = mLeftMotor.getPIDController(); + // mLeftPIDController.setP(Constants.Coral.kP); + // mLeftPIDController.setI(Constants.Coral.kI); + // mLeftPIDController.setD(Constants.Coral.kD); + // mLeftPIDController.setIZone(Constants.Coral.kIZone); + + // mRightPIDController = mRightMotor.getPIDController(); + // mRightPIDController.setP(Constants.Coral.kP); + // mRightPIDController.setI(Constants.Coral.kI); + // mRightPIDController.setD(Constants.Coral.kD); + // mRightPIDController.setIZone(Constants.Coral.kIZone); + + // mLeftMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); + // mRightMotor.setMaxCurrent(CurrentType.STATOR, Constants.Coral.kMaxCurrent); + + // mLeftMotor.setMaxCurrent(CurrentType.STATOR, 200.0); + // mRightMotor.setMaxCurrent(CurrentType.STATOR, 200.0); + + // mLeftMotor.setMaxCurrent(CurrentType.SUPPLY, 200.0); + // mRightMotor.setMaxCurrent(CurrentType.SUPPLY, 200.0); + + // mLeftMotor.setRampUp(0.5); + // mRightMotor.setRampUp(0.5); + + // mLeftMotor.setMaxOutput(1.0); + // mRightMotor.setMaxOutput(1.0); + + // mPIDConfig = mRightMotor.pid0; + // mPIDConfig.setP(Constants.Coral.kP); + // mPIDConfig.setI(Constants.Coral.kI); + // mPIDConfig.setD(Constants.Coral.kD); + // mPIDConfig.setIZone(Constants.Coral.kIZone); + + // TODO: Make sure we're inverting the correct motor + // mLeftMotor.setInverted(true); + // mLeftMotor.setInversion(false); + // mRightMotor.setInversion(false); + + mLaserCAN = new LaserCan(Constants.Coral.kLaserId); + try { + mLaserCAN.setRangingMode(LaserCan.RangingMode.SHORT); + mLaserCAN.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + mLaserCAN.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); + } catch (ConfigurationFailedException e) { + System.out.println("Configuration failed! " + e); + } + } + + private static class PeriodicIO { + double rpm = 0.0; + double speed_diff = 0.0; + + int index_debounce = 0; + + LaserCan.Measurement measurement; + + IntakeState state = IntakeState.NONE; + } + + /*-------------------------------- Generic Subsystem Functions --------------------------------*/ + + @Override + public void periodic() { + mPeriodicIO.measurement = mLaserCAN.getMeasurement(); + + checkAutoTasks(); + } + + @Override + public void writePeriodicOutputs() { + // mLeftMotor.setVelocity(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + // mRightMotor.setVelocity(mPeriodicIO.rpm); + + // mLeftMotor.setPercent(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + // mRightMotor.setPercent(-mPeriodicIO.rpm); + + mLeftMotor.set(mPeriodicIO.rpm - mPeriodicIO.speed_diff); + mRightMotor.set(-mPeriodicIO.rpm); + } + + @Override + public void stop() { + mPeriodicIO.rpm = 0.0; + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.state = IntakeState.NONE; + } + + @Override + public void outputTelemetry() { + putNumber("RPM/target", mPeriodicIO.rpm); + + // putNumber("RPM/Left/Position", mLeftMotor.getPosition()); + // putNumber("RPM/Right/Position", mRightMotor.getPosition()); + + // putNumber("RPM/Left/Velocity", mLeftMotor.getVelocity()); + // putNumber("RPM/Right/Velocity", mRightMotor.getVelocity()); + + LaserCan.Measurement measurement = mPeriodicIO.measurement; + if (measurement != null) { + putNumber("Laser/distance", measurement.distance_mm); + putNumber("Laser/ambient", measurement.ambient); + putNumber("Laser/budget_ms", measurement.budget_ms); + putNumber("Laser/status", measurement.status); + + putBoolean("Laser/hasCoral", isHoldingCoralViaLaserCAN()); + } + } + + @Override + public void reset() { + stopCoral(); + } + + /*---------------------------------- Custom Public Functions ----------------------------------*/ + + public boolean isHoldingCoralViaLaserCAN() { + return mPeriodicIO.measurement.distance_mm < 75.0; + } + + public void setSpeed(double rpm) { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = rpm; + } + + public void intake() { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = Constants.Coral.kIntakeSpeed; + mPeriodicIO.state = IntakeState.INTAKE; + + m_leds.setColor(Color.kYellow); + } + + public void reverse() { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = Constants.Coral.kReverseSpeed; + mPeriodicIO.state = IntakeState.REVERSE; + } + + public void index() { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = Constants.Coral.kIndexSpeed; + mPeriodicIO.state = IntakeState.INDEX; + + m_leds.setColor(Color.kBlue); + } + + public void scoreL1() { + mPeriodicIO.speed_diff = Constants.Coral.kSpeedDifference; + mPeriodicIO.rpm = Constants.Coral.kL1Speed; + mPeriodicIO.state = IntakeState.SCORE; + } + + public void scoreL24() { + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.rpm = Constants.Coral.kL24Speed; + mPeriodicIO.state = IntakeState.SCORE; + } + + public void stopCoral() { + mPeriodicIO.rpm = 0.0; + mPeriodicIO.speed_diff = 0.0; + mPeriodicIO.state = IntakeState.NONE; + } + + /*---------------------------------- Custom Private Functions ---------------------------------*/ + + private void checkAutoTasks() { + switch (mPeriodicIO.state) { + case INTAKE: + if (isHoldingCoralViaLaserCAN()) { + mPeriodicIO.index_debounce++; + + if (mPeriodicIO.index_debounce > 10) { + mPeriodicIO.index_debounce = 0; + index(); + } + } + break; + case INDEX: + if (!isHoldingCoralViaLaserCAN()) { + stopCoral(); + + mPeriodicIO.state = IntakeState.READY; + m_leds.setColor(Color.kBlue); + } + break; + default: + break; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 6a81d25..2faf97f 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -54,6 +54,8 @@ public class Drivetrain extends Subsystem { private static final double kSlowModeRotScale = 0.1; private static final double kSpeedModeScale = 2.0; + private static final double kTippyModeScale = 0.7; + private static final double kTrackWidth = Units.inchesToMeters(20.75); private static final double kWheelRadius = Units.inchesToMeters(3.0); private static final double kGearRatio = 10.71; @@ -77,6 +79,7 @@ public class Drivetrain extends Subsystem { Constants.Drive.kD); private final AHRS mGyro = new AHRS(); + private final Elevator mElevator = Elevator.getInstance(); private final DifferentialDriveKinematics mKinematics = new DifferentialDriveKinematics(kTrackWidth); @@ -170,18 +173,20 @@ public Drivetrain() { // TODO: get rid of this? // Configure AutoBuilder last // AutoBuilder.configureRamsete( - // this::getPose, // Robot pose supplier - // this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - // this::getCurrentSpeeds, // Current ChassisSpeeds supplier - // this::drive, // Method that will drive the robot given ChassisSpeeds - // new ReplanningConfig(), // Default path replanning config. See the API for the options here - // new BooleanSupplier() { - // @Override - // public boolean getAsBoolean() { - // return true; - // } - // }, // determines if paths should be flipped to the other side of the field - // this // Reference to this subsystem to set requirements + // this::getPose, // Robot pose supplier + // this::resetOdometry, // Method to reset odometry (will be called if your auto + // has a starting pose) + // this::getCurrentSpeeds, // Current ChassisSpeeds supplier + // this::drive, // Method that will drive the robot given ChassisSpeeds + // new ReplanningConfig(), // Default path replanning config. See the API for + // the options here + // new BooleanSupplier() { + // @Override + // public boolean getAsBoolean() { + // return true; + // } + // }, // determines if paths should be flipped to the other side of the field + // this // Reference to this subsystem to set requirements // ); mSysIdRoutine = new SysIdRoutine( @@ -268,7 +273,15 @@ public void drive(double xSpeed, double rot) { mPeriodicIO.diffWheelSpeeds = mKinematics .toWheelSpeeds(new ChassisSpeeds(xSpeed * kSpeedModeScale, 0, rot * kSlowModeRotScale)); } else { - mPeriodicIO.diffWheelSpeeds = mKinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)); + Elevator.ElevatorState state = mElevator.getState(); + + boolean highSetPoint = state == Elevator.ElevatorState.L4 || + state == Elevator.ElevatorState.L3 || + state == Elevator.ElevatorState.A2; + + double scale = (highSetPoint ? kTippyModeScale : 1.0); + + mPeriodicIO.diffWheelSpeeds = mKinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed * scale, 0, rot)); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..06de782 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,236 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkPIDController.ArbFFUnits; + +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.simulation.SimulatableCANSparkMax; + +public class Elevator extends Subsystem { + + /*-------------------------------- Private instance variables ---------------------------------*/ + private static Elevator mInstance; + private PeriodicIO mPeriodicIO; + + // private static final double kPivotCLRampRate = 0.5; + // private static final double kCLRampRate = 0.5; + + public static Elevator getInstance() { + if (mInstance == null) { + mInstance = new Elevator(); + } + return mInstance; + } + + private SimulatableCANSparkMax mLeftMotor; + private RelativeEncoder mLeftEncoder; + private SparkPIDController mLeftPIDController; + + private SimulatableCANSparkMax mRightMotor; + private RelativeEncoder mRightEncoder; + private SparkPIDController mRightPIDController; + + private TrapezoidProfile mProfile; + private TrapezoidProfile.State mCurState = new TrapezoidProfile.State(); + private TrapezoidProfile.State mGoalState = new TrapezoidProfile.State(); + private double prevUpdateTime = Timer.getFPGATimestamp(); + + // private RelativeEncoder mLeftEncoder; + // private SparkMaxLimitSwitch mLowerLimit; + // private SparkMaxLimitSwitch mUpperLimit; + + // private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(1000); + + private void setUpElevatorMotor(SimulatableCANSparkMax motor, SparkPIDController pidController) { + motor.restoreFactoryDefaults(); + motor.setIdleMode(IdleMode.kBrake); + motor.setSmartCurrentLimit(Constants.Elevator.kMaxCurrent); + // mLeftEncoder = motor.getEncoder(); + + pidController.setP(Constants.Elevator.kP); + pidController.setI(Constants.Elevator.kI); + pidController.setD(Constants.Elevator.kD); + pidController.setIZone(Constants.Elevator.kIZone); + // pidController.setIMaxAccum(0.001, 0) + + // mLeftPIDController.setOutputRange(Constants.Elevator.kMaxPowerUp, + // Constants.Elevator.kMaxPowerDown); + + // motor.setClosedLoopRampRate(kExtensionCLRampRate); + + // mLowerLimit = mLeftMotor.getReverseLimitSwitch(Type.kNormallyOpen); + // mUpperLimit = mLeftMotor.getForwardLimitSwitch(Type.kNormallyOpen); + } + + private Elevator() { + super("Elevator"); + + mPeriodicIO = new PeriodicIO(); + + // LEFT ELEVATOR MOTOR + mLeftMotor = new SimulatableCANSparkMax(Constants.Elevator.kElevatorLeftMotorId, MotorType.kBrushless); + mLeftEncoder = mLeftMotor.getEncoder(); + mLeftPIDController = mLeftMotor.getPIDController(); + setUpElevatorMotor(mLeftMotor, mLeftPIDController); + + // RIGHT ELEVATOR MOTOR + mRightMotor = new SimulatableCANSparkMax(Constants.Elevator.kElevatorRightMotorId, MotorType.kBrushless); + mRightEncoder = mRightMotor.getEncoder(); + mRightPIDController = mRightMotor.getPIDController(); + setUpElevatorMotor(mRightMotor, mRightPIDController); + + // mRightMotor.setInverted(true); + mRightMotor.follow(mLeftMotor, true); + + mLeftMotor.burnFlash(); + mRightMotor.burnFlash(); + + mProfile = new TrapezoidProfile( + new TrapezoidProfile.Constraints(Constants.Elevator.kMaxVelocity, Constants.Elevator.kMaxAcceleration)); + } + + public enum ElevatorState { + NONE, + STOW, + L2, + L3, + L4, + A1, + A2 + } + + private static class PeriodicIO { + double elevator_target = 0.0; + double elevator_power = 0.0; + + boolean is_elevator_pos_control = false; + + ElevatorState state = ElevatorState.STOW; + } + + /*-------------------------------- Generic Subsystem Functions --------------------------------*/ + + @Override + public void periodic() { + // TODO: Use this pattern to only drive slowly when we're really high up + // if(mPivotEncoder.getPosition() > Constants.kPivotScoreCount) { + // mPeriodicIO.is_pivot_low = true; + // } else { + // mPeriodicIO.is_pivot_low = false; + // } + } + + @Override + public void writePeriodicOutputs() { + double curTime = Timer.getFPGATimestamp(); + double dt = curTime - prevUpdateTime; + prevUpdateTime = curTime; + if (mPeriodicIO.is_elevator_pos_control) { + // Update goal + mGoalState.position = mPeriodicIO.elevator_target; + + // Calculate new state + prevUpdateTime = curTime; + mCurState = mProfile.calculate(dt, mCurState, mGoalState); + + // Set PID controller to new state + mLeftPIDController.setReference( + mCurState.position, + CANSparkMax.ControlType.kPosition, + 0, + Constants.Elevator.kG, + ArbFFUnits.kVoltage); + } else { + mCurState.position = mLeftEncoder.getPosition(); + mCurState.velocity = 0; + mLeftMotor.set(mPeriodicIO.elevator_power); + } + } + + @Override + public void stop() { + mPeriodicIO.is_elevator_pos_control = false; + mPeriodicIO.elevator_power = 0.0; + + mLeftMotor.set(0.0); + } + + @Override + public void outputTelemetry() { + putNumber("Position/Current", mLeftEncoder.getPosition()); + putNumber("Position/Target", mPeriodicIO.elevator_target); + putNumber("Velocity/Current", mLeftEncoder.getVelocity()); + + putNumber("Position/Setpoint", mCurState.position); + putNumber("Velocity/Setpoint", mCurState.velocity); + + putNumber("Current/Left", mLeftMotor.getOutputCurrent()); + putNumber("Current/Right", mRightMotor.getOutputCurrent()); + + putNumber("Output/Left", mLeftMotor.getAppliedOutput()); + putNumber("Output/Right", mRightMotor.getAppliedOutput()); + + putNumber("State", mPeriodicIO.state); + } + + @Override + public void reset() { + mLeftEncoder.setPosition(0.0); + } + + /*---------------------------------- Custom Public Functions ----------------------------------*/ + + public ElevatorState getState() { + return mPeriodicIO.state; + } + + public void setElevatorPower(double power) { + putNumber("setElevatorPower", power); + mPeriodicIO.is_elevator_pos_control = false; + mPeriodicIO.elevator_power = power; + } + + public void goToElevatorStow() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kStowHeight; + mPeriodicIO.state = ElevatorState.STOW; + } + + public void goToElevatorL2() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kL2Height; + mPeriodicIO.state = ElevatorState.L2; + } + + public void goToElevatorL3() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kL3Height; + mPeriodicIO.state = ElevatorState.L3; + } + + public void goToElevatorL4() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kL4Height; + mPeriodicIO.state = ElevatorState.L4; + } + + public void goToAlgaeLow() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kLowAlgaeHeight; + mPeriodicIO.state = ElevatorState.A1; + } + + public void goToAlgaeHigh() { + mPeriodicIO.is_elevator_pos_control = true; + mPeriodicIO.elevator_target = Constants.Elevator.kHighAlgaeHeight; + mPeriodicIO.state = ElevatorState.A2; + } + + /*---------------------------------- Custom Private Functions ---------------------------------*/ +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 2a61d6f..59452b5 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -65,7 +65,6 @@ private Shooter() { mLeftShooterMotor.setInverted(true); mRightShooterMotor.setInverted(false); - } private static class PeriodicIO { diff --git a/src/main/java/frc/robot/subsystems/Subsystem.java b/src/main/java/frc/robot/subsystems/Subsystem.java index 49c9879..18ea09a 100644 --- a/src/main/java/frc/robot/subsystems/Subsystem.java +++ b/src/main/java/frc/robot/subsystems/Subsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -67,30 +69,34 @@ public void writeToLog() { public String baseSmartDashboardKey = "UnknownSubsystem/"; public void putNumber(String key, double value) { - SmartDashboard.putNumber(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putBoolean(String key, boolean value) { - SmartDashboard.putBoolean(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putString(String key, String value) { - SmartDashboard.putString(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putNumberArray(String key, double[] value) { - SmartDashboard.putNumberArray(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putBooleanArray(String key, boolean[] value) { - SmartDashboard.putBooleanArray(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putStringArray(String key, String[] value) { - SmartDashboard.putStringArray(baseSmartDashboardKey + "/" + key, value); + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); } public void putData(String key, Sendable value) { SmartDashboard.putData(baseSmartDashboardKey + "/" + key, value); } + + public void putNumber(String key, Elevator.ElevatorState value) { + Logger.recordOutput(baseSmartDashboardKey + "/" + key, value); + } } diff --git a/src/main/java/frc/robot/wrappers/REVThroughBoreEncoder.java b/src/main/java/frc/robot/wrappers/REVThroughBoreEncoder.java new file mode 100644 index 0000000..58db56b --- /dev/null +++ b/src/main/java/frc/robot/wrappers/REVThroughBoreEncoder.java @@ -0,0 +1,14 @@ +package frc.robot.wrappers; + +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +public class REVThroughBoreEncoder extends DutyCycleEncoder { + public REVThroughBoreEncoder(int channel) { + super(channel); + + // Based on logging: + // - Initializing: 764 ± 5 + // - Ready: 973 ± 5 + setConnectedFrequencyThreshold(900); + } +} diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json new file mode 100644 index 0000000..9da07c4 --- /dev/null +++ b/vendordeps/ThriftyLib.json @@ -0,0 +1,20 @@ +{ + "fileName": "ThriftyLib.json", + "name": "ThriftyLib", + "version": "2024.0.0", + "frcYear": "2024", + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "mavenUrls": [ + "file:./ttb-maven" + ], + "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", + "javaDependencies": [ + { + "groupId": "com.thethriftybot.frc", + "artifactId": "ThriftyLib-java", + "version": "2024.0.0" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/vendordeps/libgrapplefrc2024.json b/vendordeps/libgrapplefrc2024.json new file mode 100644 index 0000000..5532a13 --- /dev/null +++ b/vendordeps/libgrapplefrc2024.json @@ -0,0 +1,72 @@ +{ + "fileName": "libgrapplefrc2024.json", + "name": "libgrapplefrc", + "version": "2024.3.1", + "frcYear": "2024", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ "https://storage.googleapis.com/grapple-frc-maven" ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2024.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2024.3.1" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2024.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2024.3.1", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2024.3.1", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +}