diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java index 068d67be71..8a73f24210 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java @@ -32,12 +32,15 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.subsystems.GamepieceLauncher; import frc.robot.subsystems.drivetrain.SwerveDrive; public class Robot extends TimedRobot { private SwerveDrive drivetrain; private Vision vision; + private GamepieceLauncher gpLauncher; + private XboxController controller; @@ -47,10 +50,16 @@ public void robotInit() { vision = new Vision(); controller = new XboxController(0); + + gpLauncher = new GamepieceLauncher(); } @Override public void robotPeriodic() { + // Update Gamepiece Launcher Subsystem + gpLauncher.periodic(); + + // Update drivetrain subsystem drivetrain.periodic(); // Correct pose estimate with vision measurements @@ -64,7 +73,7 @@ public void robotPeriodic() { est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs); }); - // Example only! + // Test/Example only! // Apply an offset to pose estimator to test vision correction // You probably don't want this on a real robot, just delete it. if (controller.getBButtonPressed()) { @@ -99,6 +108,11 @@ public void teleopPeriodic() { // Command drivetrain motors based on target speeds drivetrain.drive(forward, strafe, turn); + + // Calculate whether the gamepiece launcher runs based on our global pose estimate. + var curPose = drivetrain.getPose(); + var shouldRun = (curPose.getX() > 5.0 && curPose.getY() < 5.0); // Close enough to blue speaker + gpLauncher.setRunning(shouldRun); } @Override @@ -113,6 +127,9 @@ public void simulationPeriodic() { debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose()); debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses()); + // Update gamepiece launcher simulation + gpLauncher.simulationPeriodic(); + // Calculate battery voltage sag due to current draw RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw())); diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java new file mode 100644 index 0000000000..d715063353 --- /dev/null +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class GamepieceLauncher { + + private final PWMSparkMax motor; + private final Encoder encoder; + + private final double LAUNCH_SPEED_RPM = 2500; + private double curDesSpd; + private double curMotorCmd = 0.0; + + public GamepieceLauncher() { + motor = new PWMSparkMax(8); + encoder = new Encoder(16, 17); + simulationInit(); + } + + public void setRunning(boolean shouldRun) { + curDesSpd = shouldRun ? LAUNCH_SPEED_RPM : 0.0; + } + + public void periodic() { + double actSpd =encoder.getRate(); + double err = curDesSpd - actSpd; + curMotorCmd = 1.0 * err; + curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0); + motor.set(curMotorCmd); + + SmartDashboard.putNumber("GPLauncher Des Spd (RPM)", curDesSpd); + SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", actSpd); + + } + + // -- SIMULATION SUPPORT + private DCMotor motorSim; + private EncoderSim encoderSim; + + private void simulationInit() { + motorSim = DCMotor.getFalcon500(1); + encoderSim = new EncoderSim(encoder); + } + + public void simulationPeriodic() { + var spd = motorSim.getSpeed(0.0, curMotorCmd*RobotController.getBatteryVoltage()); + encoderSim.setRate(Units.radiansPerSecondToRotationsPerMinute(spd)); + } + +}