Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[examples] ArmSimulation, ElevatorSimulation: Extract mechanism to class (#5052)

Browse Source
This commit is contained in:
Starlight220
2023-02-12 16:50:57 +02:00
committed by GitHub
parent 5483464158
commit 43975ac7cc
24 changed files with 1430 additions and 508 deletions
Download Patch File Download Diff File Expand all files Collapse all files

158
wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
View File

@@ -2,150 +2,30 @@
// 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 <numbers>
#include "Robot.h"
#include <frc/Encoder.h>
#include <frc/Joystick.h>
#include <frc/Preferences.h>
#include <frc/RobotController.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <frc/simulation/BatterySim.h>
#include <frc/simulation/EncoderSim.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 <frc/smartdashboard/SmartDashboard.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/util/Color.h>
#include <frc/util/Color8Bit.h>
#include <units/angle.h>
#include <units/moment_of_inertia.h>
void Robot::SimulationPeriodic() {
m_arm.SimulationPeriodic();
}
/**
* This is a sample program to demonstrate how to use a state-space controller
* to control an arm.
*/
class Robot : public frc::TimedRobot {
static constexpr int kMotorPort = 0;
static constexpr int kEncoderAChannel = 0;
static constexpr int kEncoderBChannel = 1;
static constexpr int kJoystickPort = 0;
void Robot::TeleopInit() {
m_arm.LoadPreferences();
}
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.
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)
static constexpr double kArmEncoderDistPerPulse =
2.0 * std::numbers::pi / 4096.0;
// 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{kArmKp, 0, 0};
frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
frc::PWMSparkMax m_motor{kMotorPort};
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 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,
200.0,
frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 8_kg),
30_in,
-75_deg,
255_deg,
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});
public:
void RobotInit() override {
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
if (!frc::Preferences::ContainsKey(kArmPositionKey)) {
frc::Preferences::SetDouble(kArmPositionKey, armPosition.value());
}
if (!frc::Preferences::ContainsKey(kArmPKey)) {
frc::Preferences::SetDouble(kArmPKey, kArmKp);
}
void Robot::TeleopPeriodic() {
if (m_joystick.GetTrigger()) {
// Here, we run PID control like normal.
m_arm.ReachSetpoint();
} else {
// Otherwise, we disable the motor.
m_arm.Stop();
}
}
void SimulationPeriodic() override {
// 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 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 setpoint read from
// preferences in degrees.
double pidOutput = m_controller.Calculate(
m_encoder.GetDistance(), (units::radian_t{armPosition}.value()));
m_motor.SetVoltage(units::volt_t{pidOutput});
} else {
// Otherwise, we disable the motor.
m_motor.Set(0.0);
}
}
void DisabledInit() override {
// This just makes sure that our simulation code knows that the motor's off.
m_motor.Set(0.0);
}
};
void Robot::DisabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_arm.Stop();
}
#ifndef RUNNING_FRC_TESTS
int main() {

64
wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp Normal file
View File

@@ -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
wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h Normal file
View File

@@ -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
wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Robot.h Normal file
View File

@@ -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
wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h Normal file
View File

@@ -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});
};