diff --git a/photonlib-java-examples/poseest/simgui.json b/photonlib-java-examples/poseest/simgui.json index 2f0d45d903..b8d18f09c6 100644 --- a/photonlib-java-examples/poseest/simgui.json +++ b/photonlib-java-examples/poseest/simgui.json @@ -117,5 +117,43 @@ }, "NetworkTables Info": { "visible": true + }, + "NetworkTables View": { + "visible": false + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "backgroundColor": [ + 0.0, + 0.0, + 0.0, + 0.8500000238418579 + ], + "height": 332, + "series": [ + { + "color": [ + 0.2980392277240753, + 0.44705885648727417, + 0.6901960968971252, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)" + }, + { + "color": [ + 0.8666667342185974, + 0.5176470875740051, + 0.32156863808631897, + 1.0 + ], + "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)" + } + ] + } + ] + } } } 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 8a73f24210..437d8aa72c 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 @@ -111,7 +111,7 @@ public void teleopPeriodic() { // 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 + var shouldRun = (curPose.getY() > 2.0 && curPose.getX() < 4.0); // Close enough to blue speaker gpLauncher.setRunning(shouldRun); } 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 index d715063353..0be40ebfec 100644 --- 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 @@ -3,16 +3,14 @@ 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.simulation.FlywheelSim; 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; @@ -20,7 +18,6 @@ public class GamepieceLauncher { public GamepieceLauncher() { motor = new PWMSparkMax(8); - encoder = new Encoder(16, 17); simulationInit(); } @@ -29,29 +26,30 @@ public void setRunning(boolean shouldRun) { } public void periodic() { - double actSpd =encoder.getRate(); - double err = curDesSpd - actSpd; - curMotorCmd = 1.0 * err; + double maxRPM = Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeedRadPerSec); + curMotorCmd = curDesSpd / maxRPM; 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 FlywheelSim launcherSim; private void simulationInit() { motorSim = DCMotor.getFalcon500(1); - encoderSim = new EncoderSim(encoder); + launcherSim = new FlywheelSim(motorSim, 1.0, 0.002); } public void simulationPeriodic() { - var spd = motorSim.getSpeed(0.0, curMotorCmd*RobotController.getBatteryVoltage()); - encoderSim.setRate(Units.radiansPerSecondToRotationsPerMinute(spd)); + launcherSim.setInputVoltage(curMotorCmd*RobotController.getBatteryVoltage()); + launcherSim.update(0.02); + var spd = launcherSim.getAngularVelocityRPM(); + SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", spd); + } }