diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index b8b8368274..49c3efc830 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -33,8 +34,13 @@ class Robot : public frc::TimedRobot { static constexpr int kEncoderBChannel = 1; static constexpr int kJoystickPort = 0; + static constexpr std::string_view kArmPositionKey = "ArmPosition"; + static constexpr std::string_view kArmPKey = "ArmP"; + // The P gain for the PID controller that drives this arm. - static constexpr double kArmKp = 50.0; + double kArmKp = 50.0; + + units::degree_t armPosition = 75.0_deg; // distance per pulse = (angle per revolution) / (pulses per revolution) // = (2 * PI rads) / (4096 pulses) @@ -81,6 +87,15 @@ class Robot : public frc::TimedRobot { // Put Mechanism 2d to SmartDashboard frc::SmartDashboard::PutData("Arm Sim", &m_mech2d); + + // Set the Arm position setpoint and P constant to Preferences if the keys + // don't already exist + if (!frc::Preferences::ContainsKey(kArmPositionKey)) { + frc::Preferences::SetDouble(kArmPositionKey, armPosition.value()); + } + if (!frc::Preferences::ContainsKey(kArmPKey)) { + frc::Preferences::SetDouble(kArmPKey, armPosition.value()); + } } void SimulationPeriodic() override { @@ -103,12 +118,22 @@ class Robot : public frc::TimedRobot { m_arm->SetAngle(m_armSim.GetAngle()); } + void TeleopInit() override { + // Read Preferences for Arm setpoint and kP on entering Teleop + armPosition = units::degree_t( + frc::Preferences::GetDouble(kArmPositionKey, armPosition.value())); + if (kArmKp != frc::Preferences::GetDouble(kArmPKey, kArmKp)) { + kArmKp = frc::Preferences::GetDouble(kArmPKey, kArmKp); + m_controller.SetP(kArmKp); + } + } + void TeleopPeriodic() override { if (m_joystick.GetTrigger()) { - // Here, we run PID control like normal, with a constant setpoint of 75 - // degrees. + // Here, we run PID control like normal, with a setpoint read from + // preferences in degrees. double pidOutput = m_controller.Calculate( - m_encoder.GetDistance(), (units::radian_t(75_deg)).value()); + m_encoder.GetDistance(), (units::radian_t(armPosition).value())); m_motor.SetVoltage(units::volt_t(pidOutput)); } else { // Otherwise, we disable the motor. diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 65ed41a9b7..9eb5b6b1be 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -713,7 +713,8 @@ "Sensors", "Simulation", "Physics", - "Mechanism2d" + "Mechanism2d", + "Preferences" ], "foldername": "ArmSimulation", "gradlebase": "cpp", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java index a7e4d7d87a..c84d9228bc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java @@ -10,6 +10,7 @@ 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.Joystick; +import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; @@ -31,8 +32,13 @@ public class Robot extends TimedRobot { private static final int kEncoderBChannel = 1; private static final int kJoystickPort = 0; + public static final String kArmPositionKey = "ArmPosition"; + public static final String kArmPKey = "ArmP"; + // The P gain for the PID controller that drives this arm. - private static final double kArmKp = 50.0; + private static double kArmKp = 50.0; + + private static double armPositionDeg = 75.0; // distance per pulse = (angle per revolution) / (pulses per revolution) // = (2 * PI rads) / (4096 pulses) @@ -88,6 +94,14 @@ public class Robot extends TimedRobot { // Put Mechanism 2d to SmartDashboard SmartDashboard.putData("Arm Sim", m_mech2d); m_armTower.setColor(new Color8Bit(Color.kBlue)); + + // Set the Arm position setpoint and P constant to Preferences if the keys don't already exist + if (!Preferences.containsKey(kArmPositionKey)) { + Preferences.setDouble(kArmPositionKey, armPositionDeg); + } + if (!Preferences.containsKey(kArmPKey)) { + Preferences.setDouble(kArmPKey, kArmKp); + } } @Override @@ -109,11 +123,22 @@ public class Robot extends TimedRobot { m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads())); } + @Override + public void teleopInit() { + // Read Preferences for Arm setpoint and kP on entering Teleop + armPositionDeg = Preferences.getDouble(kArmPositionKey, armPositionDeg); + if (kArmKp != Preferences.getDouble(kArmPKey, kArmKp)) { + kArmKp = Preferences.getDouble(kArmPKey, kArmKp); + m_controller.setP(kArmKp); + } + } + @Override public void teleopPeriodic() { if (m_joystick.getTrigger()) { // Here, we run PID control like normal, with a constant setpoint of 75 degrees. - var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(75)); + var pidOutput = + m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(armPositionDeg)); m_motor.setVoltage(pidOutput); } else { // Otherwise, we disable the motor. diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 5bc729ef84..ddf5d5ebec 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -701,7 +701,8 @@ "Sensors", "Simulation", "Physics", - "Mechanism2d" + "Mechanism2d", + "Preferences" ], "foldername": "armsimulation", "gradlebase": "java",