diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index bb8a93de47..b97e051939 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -12,7 +12,13 @@ #include #include #include +#include +#include +#include +#include #include +#include +#include #include #include #include @@ -28,14 +34,14 @@ class Robot : public frc::TimedRobot { static constexpr int kJoystickPort = 0; // The P gain for the PID controller that drives this arm. - static constexpr double kArmKp = 5.0; + static constexpr double kArmKp = 50.0; // distance per pulse = (angle per revolution) / (pulses per revolution) // = (2 * PI rads) / (4096 pulses) static constexpr double kArmEncoderDistPerPulse = 2.0 * wpi::numbers::pi / 4096.0; - // The arm gearbox represents a gerbox containing two Vex 775pro motors. + // The arm gearbox represents a gearbox containing two Vex 775pro motors. frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2); // Standard classes for controlling our arm @@ -45,24 +51,36 @@ class Robot : public frc::TimedRobot { frc::Joystick m_joystick{kJoystickPort}; // Simulation classes help us simulate what's going on, including gravity. - // This sim represents an arm with 2 775s, a 100:1 reduction, a mass of 5kg, - // 30in overall arm length, range of motion nin [-180, 0] degrees, and noise - // with a standard deviation of 0.5 degrees. + // This sim represents an arm with 2 775s, a 600:1 reduction, a mass of 5kg, + // 30in overall arm length, range of motion in [-75, 255] degrees, and noise + // with a standard deviation of 1 encoder tick. frc::sim::SingleJointedArmSim m_armSim{ m_armGearbox, - 100.0, + 600.0, frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 5_kg), 30_in, - -180_deg, - 0_deg, + -75_deg, + 255_deg, 5_kg, true, - {(0.5_deg).to()}}; + {kArmEncoderDistPerPulse}}; frc::sim::EncoderSim m_encoderSim{m_encoder}; + // Create a Mechanism2d display of an Arm + frc::Mechanism2d m_mech2d{60, 60}; + frc::MechanismRoot2d* m_armBase = m_mech2d.GetRoot("ArmBase", 30, 30); + frc::MechanismLigament2d* m_armTower = + m_armBase->Append( + "Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue}); + frc::MechanismLigament2d* m_arm = m_armBase->Append( + "Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow}); + public: void RobotInit() override { m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); + + // Put Mechanism 2d to SmartDashboard + frc::SmartDashboard::PutData("Arm Sim", &m_mech2d); } void SimulationPeriodic() override { @@ -80,13 +98,17 @@ class Robot : public frc::TimedRobot { // SimBattery estimates loaded battery voltages frc::sim::RoboRioSim::SetVInVoltage( frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()})); + + // Update the Mechanism Arm angle based on the simulated arm angle + m_arm->SetAngle(m_armSim.GetAngle()); } 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()); + // Here, we run PID control like normal, with a constant setpoint of 75 + // degrees. + double pidOutput = m_controller.Calculate( + m_encoder.GetDistance(), (units::radian_t(75_deg)).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 e0f1022dc0..f598e1e74d 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -701,7 +701,8 @@ "Digital", "Sensors", "Simulation", - "Physics" + "Physics", + "Mechanism2d" ], "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 56c72565c2..a7e4d7d87a 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 @@ -17,8 +17,14 @@ import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +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; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; -/** This is a sample program to demonstrate the use of elevator simulation with existing code. */ +/** This is a sample program to demonstrate the use of arm simulation with existing code. */ public class Robot extends TimedRobot { private static final int kMotorPort = 0; private static final int kEncoderAChannel = 0; @@ -26,49 +32,67 @@ public class Robot extends TimedRobot { private static final int kJoystickPort = 0; // The P gain for the PID controller that drives this arm. - private static final double kArmKp = 5.0; + private static final double kArmKp = 50.0; // distance per pulse = (angle per revolution) / (pulses per revolution) // = (2 * PI rads) / (4096 pulses) private static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096; - // The arm gearbox represents a gerbox containing two Vex 775pro motors. + // The arm gearbox represents a gearbox containing two Vex 775pro motors. private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2); - // Standard classes for controlling our elevator + // Standard classes for controlling our arm private final PIDController m_controller = new PIDController(kArmKp, 0, 0); private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort); private final Joystick m_joystick = new Joystick(kJoystickPort); // Simulation classes help us simulate what's going on, including gravity. - private static final double m_armReduction = 100; + private static final double m_armReduction = 600; private static final double m_armMass = 5.0; // Kilograms private static final double m_armLength = Units.inchesToMeters(30); - // This arm sim represents an arm that can travel from -180 degrees (rotated straight backwards) - // to 0 degrees (rotated straight forwards). + // This arm sim represents an arm that can travel from -75 degrees (rotated down front) + // to 255 degrees (rotated down in the back). private final SingleJointedArmSim m_armSim = new SingleJointedArmSim( m_armGearbox, m_armReduction, SingleJointedArmSim.estimateMOI(m_armLength, m_armMass), m_armLength, - Units.degreesToRadians(-180), - Units.degreesToRadians(0), + Units.degreesToRadians(-75), + Units.degreesToRadians(255), m_armMass, true, - VecBuilder.fill(Units.degreesToRadians(0.5)) // Add noise with a std-dev of 0.5 degrees + VecBuilder.fill(kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick ); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); + // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm. + private final Mechanism2d m_mech2d = new Mechanism2d(60, 60); + private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30); + private final MechanismLigament2d m_armTower = + m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90)); + private final MechanismLigament2d m_arm = + m_armPivot.append( + new MechanismLigament2d( + "Arm", + 30, + Units.radiansToDegrees(m_armSim.getAngleRads()), + 6, + new Color8Bit(Color.kYellow))); + @Override public void robotInit() { m_encoder.setDistancePerPulse(kArmEncoderDistPerPulse); + + // Put Mechanism 2d to SmartDashboard + SmartDashboard.putData("Arm Sim", m_mech2d); + m_armTower.setColor(new Color8Bit(Color.kBlue)); } @Override public void simulationPeriodic() { - // In this method, we update our simulation of what our elevator is doing + // In this method, we update our simulation of what our arm is doing // First, we set our "inputs" (voltages) m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage()); @@ -80,13 +104,16 @@ public class Robot extends TimedRobot { // SimBattery estimates loaded battery voltages RoboRioSim.setVInVoltage( BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps())); + + // Update the Mechanism Arm angle based on the simulated arm angle + m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads())); } @Override public void teleopPeriodic() { if (m_joystick.getTrigger()) { - // Here, we run PID control like normal, with a constant setpoint of 30in. - var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(0)); + // 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)); 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 7f9a365acd..8e05a1570e 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 @@ -710,7 +710,8 @@ "Digital", "Sensors", "Simulation", - "Physics" + "Physics", + "Mechanism2d" ], "foldername": "armsimulation", "gradlebase": "java",