-
Notifications
You must be signed in to change notification settings - Fork 612
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[examples] ArmSimulation, ElevatorSimulation: Extract mechanism to cl…
…ass (#5052)
- Loading branch information
1 parent
5483464
commit 43975ac
Showing
24 changed files
with
1,430 additions
and
508 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
64 changes: 64 additions & 0 deletions
64
wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,64 @@ | ||
| // Copyright (c) FIRST and other WPILib contributors. | ||
| // Open Source Software; you can modify and/or share it under the terms of | ||
| // the WPILib BSD license file in the root directory of this project. | ||
|
|
||
| #include "subsystems/Arm.h" | ||
|
|
||
| #include <frc/Preferences.h> | ||
| #include <frc/RobotController.h> | ||
| #include <frc/StateSpaceUtil.h> | ||
| #include <frc/smartdashboard/SmartDashboard.h> | ||
|
|
||
| Arm::Arm() { | ||
| m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); | ||
|
|
||
| // 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 | ||
| frc::Preferences::InitDouble(kArmPositionKey, m_armSetpoint.value()); | ||
| frc::Preferences::InitDouble(kArmPKey, m_armKp); | ||
| } | ||
|
|
||
| void Arm::SimulationPeriodic() { | ||
| // In this method, we update our simulation of what our arm is doing | ||
| // First, we set our "inputs" (voltages) | ||
| m_armSim.SetInput( | ||
| frc::Vectord<1>{m_motor.Get() * frc::RobotController::GetInputVoltage()}); | ||
|
|
||
| // Next, we update it. The standard loop time is 20ms. | ||
| m_armSim.Update(20_ms); | ||
|
|
||
| // Finally, we set our simulated encoder's readings and simulated battery | ||
| // voltage | ||
| m_encoderSim.SetDistance(m_armSim.GetAngle().value()); | ||
| // 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 Arm::LoadPreferences() { | ||
| // Read Preferences for Arm setpoint and kP on entering Teleop | ||
| m_armSetpoint = units::degree_t{ | ||
| frc::Preferences::GetDouble(kArmPositionKey, m_armSetpoint.value())}; | ||
| if (m_armKp != frc::Preferences::GetDouble(kArmPKey, m_armKp)) { | ||
| m_armKp = frc::Preferences::GetDouble(kArmPKey, m_armKp); | ||
| m_controller.SetP(m_armKp); | ||
| } | ||
| } | ||
|
|
||
| void Arm::ReachSetpoint() { | ||
| // 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{m_armSetpoint}.value())); | ||
| m_motor.SetVoltage(units::volt_t{pidOutput}); | ||
| } | ||
|
|
||
| void Arm::Stop() { | ||
| m_motor.Set(0.0); | ||
| } |
46 changes: 46 additions & 0 deletions
46
wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,46 @@ | ||
| // Copyright (c) FIRST and other WPILib contributors. | ||
| // Open Source Software; you can modify and/or share it under the terms of | ||
| // the WPILib BSD license file in the root directory of this project. | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <numbers> | ||
|
|
||
| #include <units/angle.h> | ||
| #include <units/length.h> | ||
| #include <units/mass.h> | ||
| #include <units/time.h> | ||
| #include <units/velocity.h> | ||
| #include <units/voltage.h> | ||
|
|
||
| /** | ||
| * The Constants header provides a convenient place for teams to hold robot-wide | ||
| * numerical or bool constants. This should not be used for any other purpose. | ||
| * | ||
| * It is generally a good idea to place constants into subsystem- or | ||
| * command-specific namespaces within this header, which can then be used where | ||
| * they are needed. | ||
| */ | ||
|
|
||
| static constexpr int kMotorPort = 0; | ||
| static constexpr int kEncoderAChannel = 0; | ||
| static constexpr int kEncoderBChannel = 1; | ||
| static constexpr int kJoystickPort = 0; | ||
|
|
||
| static constexpr std::string_view kArmPositionKey = "ArmPosition"; | ||
| static constexpr std::string_view kArmPKey = "ArmP"; | ||
|
|
||
| static constexpr double kDefaultArmKp = 50.0; | ||
| static constexpr units::degree_t kDefaultArmSetpoint = 75.0_deg; | ||
|
|
||
| static constexpr units::radian_t kMinAngle = -75.0_deg; | ||
| static constexpr units::radian_t kMaxAngle = 255.0_deg; | ||
|
|
||
| static constexpr double kArmReduction = 200.0; | ||
| static constexpr units::kilogram_t kArmMass = 8.0_kg; | ||
| static constexpr units::meter_t kArmLength = 30.0_in; | ||
|
|
||
| // distance per pulse = (angle per revolution) / (pulses per revolution) | ||
| // = (2 * PI rads) / (4096 pulses) | ||
| static constexpr double kArmEncoderDistPerPulse = | ||
| 2.0 * std::numbers::pi / 4096.0; |
26 changes: 26 additions & 0 deletions
26
wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Robot.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,26 @@ | ||
| // Copyright (c) FIRST and other WPILib contributors. | ||
| // Open Source Software; you can modify and/or share it under the terms of | ||
| // the WPILib BSD license file in the root directory of this project. | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <frc/Joystick.h> | ||
| #include <frc/TimedRobot.h> | ||
|
|
||
| #include "subsystems/Arm.h" | ||
|
|
||
| /** | ||
| * This is a sample program to demonstrate the use of arm simulation. | ||
| */ | ||
| class Robot : public frc::TimedRobot { | ||
| public: | ||
| void RobotInit() override {} | ||
| void SimulationPeriodic() override; | ||
| void TeleopInit() override; | ||
| void TeleopPeriodic() override; | ||
| void DisabledInit() override; | ||
|
|
||
| private: | ||
| frc::Joystick m_joystick{kJoystickPort}; | ||
| Arm m_arm; | ||
| }; |
67 changes: 67 additions & 0 deletions
67
wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,67 @@ | ||
| // Copyright (c) FIRST and other WPILib contributors. | ||
| // Open Source Software; you can modify and/or share it under the terms of | ||
| // the WPILib BSD license file in the root directory of this project. | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <frc/Encoder.h> | ||
| #include <frc/controller/ArmFeedforward.h> | ||
| #include <frc/controller/PIDController.h> | ||
| #include <frc/motorcontrol/PWMSparkMax.h> | ||
| #include <frc/simulation/BatterySim.h> | ||
| #include <frc/simulation/EncoderSim.h> | ||
| #include <frc/simulation/PWMSim.h> | ||
| #include <frc/simulation/RoboRioSim.h> | ||
| #include <frc/simulation/SingleJointedArmSim.h> | ||
| #include <frc/smartdashboard/Mechanism2d.h> | ||
| #include <frc/smartdashboard/MechanismLigament2d.h> | ||
| #include <frc/smartdashboard/MechanismRoot2d.h> | ||
| #include <units/length.h> | ||
|
|
||
| #include "Constants.h" | ||
|
|
||
| class Arm { | ||
| public: | ||
| Arm(); | ||
| void SimulationPeriodic(); | ||
| void LoadPreferences(); | ||
| void ReachSetpoint(); | ||
| void Stop(); | ||
|
|
||
| private: | ||
| // The P gain for the PID controller that drives this arm. | ||
| double m_armKp = kDefaultArmKp; | ||
| units::degree_t m_armSetpoint = kDefaultArmSetpoint; | ||
|
|
||
| // 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 | ||
| frc2::PIDController m_controller{m_armKp, 0, 0}; | ||
| frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; | ||
| frc::PWMSparkMax m_motor{kMotorPort}; | ||
|
|
||
| // Simulation classes help us simulate what's going on, including gravity. | ||
| // 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, | ||
| kArmReduction, | ||
| frc::sim::SingleJointedArmSim::EstimateMOI(kArmLength, kArmMass), | ||
| kArmLength, | ||
| kMinAngle, | ||
| kMaxAngle, | ||
| true, | ||
| {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<frc::MechanismLigament2d>( | ||
| "Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue}); | ||
| frc::MechanismLigament2d* m_arm = m_armBase->Append<frc::MechanismLigament2d>( | ||
| "Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow}); | ||
| }; |
Oops, something went wrong.