diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp index 49c607d840..856bd38f7e 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -12,8 +13,15 @@ #include #include #include +#include +#include +#include +#include #include +#include +#include #include +#include #include #include @@ -32,7 +40,7 @@ class Robot : public frc::TimedRobot { static constexpr units::meter_t kElevatorDrumRadius = 2_in; static constexpr units::kilogram_t kCarriageMass = 4.0_kg; - static constexpr units::meter_t kMinElevatorHeight = 0_in; + static constexpr units::meter_t kMinElevatorHeight = 2_in; static constexpr units::meter_t kMaxElevatorHeight = 50_in; // distance per pulse = (distance per revolution) / (pulses per revolution) @@ -59,9 +67,21 @@ class Robot : public frc::TimedRobot { {0.01}}; frc::sim::EncoderSim m_encoderSim{m_encoder}; + // Create a Mechanism2d display of an elevator + frc::Mechanism2d m_mech2d{20, 50}; + frc::MechanismRoot2d* m_elevatorRoot = + m_mech2d.GetRoot("Elevator Root", 10, 0); + frc::MechanismLigament2d* m_elevatorMech2d = + m_elevatorRoot->Append( + "Elevator", units::inch_t(m_elevatorSim.GetPosition()).to(), + 90_deg); + public: void RobotInit() override { m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); + + // Put Mechanism 2d to SmartDashboard + frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d); } void SimulationPeriodic() override { @@ -79,13 +99,17 @@ class Robot : public frc::TimedRobot { // SimBattery estimates loaded battery voltages frc::sim::RoboRioSim::SetVInVoltage( frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()})); + + // Update the Elevator length based on the simulated elevator height + m_elevatorMech2d->SetLength( + units::inch_t(m_elevatorSim.GetPosition()).to()); } void TeleopPeriodic() override { if (m_joystick.GetTrigger()) { // Here, we run PID control like normal, with a constant setpoint of 30in. - double pidOutput = - m_controller.Calculate(m_encoder.GetDistance(), (30_in).to()); + double pidOutput = m_controller.Calculate( + m_encoder.GetDistance(), units::meter_t(30_in).to()); 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 19bc1a7eb0..b50d564536 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -653,7 +653,8 @@ "Digital", "Sensors", "Simulation", - "Physics" + "Physics", + "Mechanism2d" ], "foldername": "ElevatorSimulation", "gradlebase": "cpp", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java index 30690af9dd..3f2bae81ec 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java @@ -17,6 +17,10 @@ import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** This is a sample program to demonstrate the use of elevator simulation with existing code. */ public class Robot extends TimedRobot { @@ -30,7 +34,7 @@ public class Robot extends TimedRobot { private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0); private static final double kCarriageMass = 4.0; // kg - private static final double kMinElevatorHeight = 0.0; + private static final double kMinElevatorHeight = Units.inchesToMeters(2); private static final double kMaxElevatorHeight = Units.inchesToMeters(50); // distance per pulse = (distance per revolution) / (pulses per revolution) @@ -58,9 +62,20 @@ public class Robot extends TimedRobot { VecBuilder.fill(0.01)); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); + // Create a Mechanism2d visualization of the elevator + private final Mechanism2d m_mech2d = new Mechanism2d(20, 50); + private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0); + private final MechanismLigament2d m_elevatorMech2d = + m_mech2dRoot.append( + new MechanismLigament2d( + "Elevator", Units.metersToInches(m_elevatorSim.getPositionMeters()), 90)); + @Override public void robotInit() { m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse); + + // Publish Mechanism2d to SmartDashboard + SmartDashboard.putData("Elevator Sim", m_mech2d); } @Override @@ -77,6 +92,9 @@ public class Robot extends TimedRobot { // SimBattery estimates loaded battery voltages RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps())); + + // Update elevator visualization with simulated position + m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSim.getPositionMeters())); } @Override 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 b31e6d0c4e..ef892696d9 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 @@ -694,7 +694,8 @@ "Digital", "Sensors", "Simulation", - "Physics" + "Physics", + "Mechanism2d" ], "foldername": "elevatorsimulation", "gradlebase": "java",