-
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.
- Loading branch information
1 parent
f9d168d
commit a6f1333
Showing
11 changed files
with
688 additions
and
247 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
55 changes: 55 additions & 0 deletions
55
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/subsystems/Elevator.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,55 @@ | ||
| // 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/Elevator.h" | ||
|
|
||
| #include <frc/RobotController.h> | ||
| #include <frc/StateSpaceUtil.h> | ||
| #include <frc/smartdashboard/SmartDashboard.h> | ||
|
|
||
| Elevator::Elevator() { | ||
| m_encoder.SetDistancePerPulse(Constants::kArmEncoderDistPerPulse); | ||
|
|
||
| // Put Mechanism 2d to SmartDashboard | ||
| // To view the Elevator visualization, select Network Tables -> SmartDashboard | ||
| // -> Elevator Sim | ||
| frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d); | ||
| } | ||
|
|
||
| void Elevator::SimulationPeriodic() { | ||
| // In this method, we update our simulation of what our elevator is doing | ||
| // First, we set our "inputs" (voltages) | ||
| m_elevatorSim.SetInput(frc::Vectord<1>{ | ||
| m_motorSim.GetSpeed() * frc::RobotController::GetInputVoltage()}); | ||
|
|
||
| // Next, we update it. The standard loop time is 20ms. | ||
| m_elevatorSim.Update(20_ms); | ||
|
|
||
| // Finally, we set our simulated encoder's readings and simulated battery | ||
| // voltage | ||
| m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value()); | ||
| // SimBattery estimates loaded battery voltages | ||
| frc::sim::RoboRioSim::SetVInVoltage( | ||
| frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()})); | ||
| } | ||
|
|
||
| void Elevator::UpdateTelemetry() { | ||
| // Update the Elevator length based on the simulated elevator height | ||
| m_elevatorMech2d->SetLength(m_encoder.GetDistance()); | ||
| } | ||
|
|
||
| void Elevator::ReachGoal(units::meter_t goal) { | ||
| m_controller.SetGoal(goal); | ||
| // With the setpoint value we run PID control like normal | ||
| double pidOutput = | ||
| m_controller.Calculate(units::meter_t{m_encoder.GetDistance()}); | ||
| units::volt_t feedforwardOutput = | ||
| m_feedforward.Calculate(m_controller.GetSetpoint().velocity); | ||
| m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput); | ||
| } | ||
|
|
||
| void Elevator::Stop() { | ||
| m_controller.SetGoal(0.0_m); | ||
| m_motor.Set(0.0); | ||
| } |
55 changes: 55 additions & 0 deletions
55
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/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,55 @@ | ||
| // 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/acceleration.h> | ||
| #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. | ||
| */ | ||
|
|
||
| namespace Constants { | ||
|
|
||
| static constexpr int kMotorPort = 0; | ||
| static constexpr int kEncoderAChannel = 0; | ||
| static constexpr int kEncoderBChannel = 1; | ||
| static constexpr int kJoystickPort = 0; | ||
|
|
||
| static constexpr double kElevatorKp = 5.0; | ||
| static constexpr double kElevatorKi = 0.0; | ||
| static constexpr double kElevatorKd = 0.0; | ||
|
|
||
| static constexpr units::volt_t kElevatorkS = 0.0_V; | ||
| static constexpr units::volt_t kElevatorkG = 0.0_V; | ||
| static constexpr auto kElevatorkV = 0.762_V / 1_mps; | ||
| static constexpr auto kElevatorkA = 0.762_V / 1_mps_sq; | ||
|
|
||
| static constexpr double kElevatorGearing = 10.0; | ||
| static constexpr units::meter_t kElevatorDrumRadius = 2_in; | ||
| static constexpr units::kilogram_t kCarriageMass = 4.0_kg; | ||
|
|
||
| static constexpr units::meter_t kSetpoint = 75_cm; | ||
| static constexpr units::meter_t kMinElevatorHeight = 0_cm; | ||
| static constexpr units::meter_t kMaxElevatorHeight = 1.25_m; | ||
|
|
||
| // distance per pulse = (distance per revolution) / (pulses per revolution) | ||
| // = (Pi * D) / ppr | ||
| static constexpr double kArmEncoderDistPerPulse = | ||
| 2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0; | ||
|
|
||
| } // namespace Constants |
26 changes: 26 additions & 0 deletions
26
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/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/Elevator.h" | ||
|
|
||
| /** | ||
| * This is a sample program to demonstrate the use of elevator simulation. | ||
| */ | ||
| class Robot : public frc::TimedRobot { | ||
| public: | ||
| void RobotInit() override {} | ||
| void RobotPeriodic() override; | ||
| void SimulationPeriodic() override; | ||
| void TeleopPeriodic() override; | ||
| void DisabledInit() override; | ||
|
|
||
| private: | ||
| frc::Joystick m_joystick{Constants::kJoystickPort}; | ||
| Elevator m_elevator; | ||
| }; |
69 changes: 69 additions & 0 deletions
69
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.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,69 @@ | ||
| // 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/ElevatorFeedforward.h> | ||
| #include <frc/controller/PIDController.h> | ||
| #include <frc/controller/ProfiledPIDController.h> | ||
| #include <frc/motorcontrol/PWMSparkMax.h> | ||
| #include <frc/simulation/BatterySim.h> | ||
| #include <frc/simulation/ElevatorSim.h> | ||
| #include <frc/simulation/EncoderSim.h> | ||
| #include <frc/simulation/PWMSim.h> | ||
| #include <frc/simulation/RoboRioSim.h> | ||
| #include <frc/smartdashboard/Mechanism2d.h> | ||
| #include <frc/smartdashboard/MechanismLigament2d.h> | ||
| #include <frc/smartdashboard/MechanismRoot2d.h> | ||
| #include <units/length.h> | ||
|
|
||
| #include "Constants.h" | ||
|
|
||
| class Elevator { | ||
| public: | ||
| Elevator(); | ||
| void SimulationPeriodic(); | ||
| void UpdateTelemetry(); | ||
| void ReachGoal(units::meter_t goal); | ||
| void Stop(); | ||
|
|
||
| private: | ||
| // This gearbox represents a gearbox containing 4 Vex 775pro motors. | ||
| frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4); | ||
|
|
||
| // Standard classes for controlling our elevator | ||
| frc::TrapezoidProfile<units::meters>::Constraints m_constraints{2.45_mps, | ||
| 2.45_mps_sq}; | ||
| frc::ProfiledPIDController<units::meters> m_controller{ | ||
| Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd, | ||
| m_constraints}; | ||
|
|
||
| frc::ElevatorFeedforward m_feedforward{ | ||
| Constants::kElevatorkS, Constants::kElevatorkG, Constants::kElevatorkV, | ||
| Constants::kElevatorkA}; | ||
| frc::Encoder m_encoder{Constants::kEncoderAChannel, | ||
| Constants::kEncoderBChannel}; | ||
| frc::PWMSparkMax m_motor{Constants::kMotorPort}; | ||
| frc::sim::PWMSim m_motorSim{m_motor}; | ||
|
|
||
| // Simulation classes help us simulate what's going on, including gravity. | ||
| frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox, | ||
| Constants::kElevatorGearing, | ||
| Constants::kCarriageMass, | ||
| Constants::kElevatorDrumRadius, | ||
| Constants::kMinElevatorHeight, | ||
| Constants::kMaxElevatorHeight, | ||
| true, | ||
| {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<frc::MechanismLigament2d>( | ||
| "Elevator", m_elevatorSim.GetPosition().value(), 90_deg); | ||
| }; |
Oops, something went wrong.