Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31:43 +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});
};

163
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
View File

@@ -2,152 +2,35 @@
// 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/RobotController.h>
#include <frc/StateSpaceUtil.h>
#include <frc/TimedRobot.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/RoboRioSim.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/length.h>
#include <units/moment_of_inertia.h>
#include "Constants.h"
/**
* This is a sample program to demonstrate the use of elevator simulation.
*/
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::RobotPeriodic() {
// Update the telemetry, including mechanism visualization, regardless of
// mode.
m_elevator.UpdateTelemetry();
}
static constexpr double kElevatorKp = 5.0;
static constexpr double kElevatorKi = 0.0;
static constexpr double kElevatorKd = 0.0;
void Robot::SimulationPeriodic() {
// Update the simulation model.
m_elevator.SimulationPeriodic();
}
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 = 30_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)
// = (Pi * D) / ppr
static constexpr double kArmEncoderDistPerPulse =
2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0;
// 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{
kElevatorKp, kElevatorKi, kElevatorKd, m_constraints};
frc::ElevatorFeedforward m_feedforward{kElevatorkS, kElevatorkG, kElevatorkV,
kElevatorkA};
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.
frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
kElevatorGearing,
kCarriageMass,
kElevatorDrumRadius,
kMinElevatorHeight,
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", units::inch_t{m_elevatorSim.GetPosition()}.value(),
90_deg);
public:
void RobotInit() override {
m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
// Put Mechanism 2d to SmartDashboard
// To view the Elevator Sim in the simulator, select Network Tables ->
// SmartDashboard -> Elevator Sim
frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
void Robot::TeleopPeriodic() {
if (m_joystick.GetTrigger()) {
// Here, we set the constant setpoint of 0.75 meters.
m_elevator.ReachGoal(Constants::kSetpoint);
} else {
// Otherwise, we update the setpoint to 0.
m_elevator.ReachGoal(0.0_m);
}
}
void SimulationPeriodic() override {
// 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_motor.Get() * 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()}));
// Update the Elevator length based on the simulated elevator height
m_elevatorMech2d->SetLength(
units::inch_t{m_elevatorSim.GetPosition()}.value());
}
void TeleopPeriodic() override {
if (m_joystick.GetTrigger()) {
// Here, we set the constant setpoint of 30in.
m_controller.SetGoal(kSetpoint);
} else {
// Otherwise, we update the setpoint to 0.
m_controller.SetGoal(0.0_m);
}
// 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);
}
// To view the Elevator in the simulator, select Network Tables ->
// SmartDashboard -> Elevator Sim
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_elevator.Stop();
}
#ifndef RUNNING_FRC_TESTS
int main() {

55
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/subsystems/Elevator.cpp Normal file
View File

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

@@ -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.762_V;
static constexpr auto kElevatorkV = 0.762_V / 1_mps;
static constexpr auto kElevatorkA = 0.0_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
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/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/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
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h Normal file
View File

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

2
wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h
View File

@@ -25,7 +25,7 @@ class Robot : public frc::TimedRobot {
static constexpr int kPotChannel = 1;
static constexpr int kMotorChannel = 7;
static constexpr int kJoystickChannel = 0;
static constexpr int kJoystickChannel = 3;
// The elevator can move 1.5 meters from top to bottom
static constexpr units::meter_t kFullHeight = 1.5_m;

152
wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp Normal file
View File

@@ -0,0 +1,152 @@
// 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 <gtest/gtest.h>
#include <string>
#include <thread>
#include <frc/Preferences.h>
#include <frc/simulation/DriverStationSim.h>
#include <frc/simulation/JoystickSim.h>
#include <frc/simulation/PWMSim.h>
#include <frc/simulation/SimHooks.h>
#include <hal/simulation/MockHooks.h>
#include <units/length.h>
#include <units/time.h>
#include "Constants.h"
#include "Robot.h"
class ArmSimulationTest : public testing::TestWithParam<units::degree_t> {
Robot m_robot;
std::optional<std::thread> m_thread;
protected:
frc::sim::PWMSim m_motorSim{kMotorPort};
frc::sim::EncoderSim m_encoderSim =
frc::sim::EncoderSim::CreateForChannel(kEncoderAChannel);
frc::sim::JoystickSim m_joystickSim{kJoystickPort};
public:
void SetUp() override {
frc::sim::PauseTiming();
m_thread = std::thread([&] { m_robot.StartCompetition(); });
frc::sim::StepTiming(0.0_ms); // Wait for Notifiers
}
void TearDown() override {
m_robot.EndCompetition();
m_thread->join();
m_encoderSim.ResetData();
m_motorSim.ResetData();
frc::sim::DriverStationSim::ResetData();
frc::Preferences::RemoveAll();
}
};
TEST_P(ArmSimulationTest, Teleop) {
EXPECT_TRUE(frc::Preferences::ContainsKey(kArmPositionKey));
EXPECT_TRUE(frc::Preferences::ContainsKey(kArmPKey));
EXPECT_DOUBLE_EQ(kDefaultArmSetpoint.value(),
frc::Preferences::GetDouble(kArmPositionKey, NAN));
frc::Preferences::SetDouble(kArmPositionKey, GetParam().value());
units::degree_t setpoint = GetParam();
// teleop init
{
frc::sim::DriverStationSim::SetAutonomous(false);
frc::sim::DriverStationSim::SetEnabled(true);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_TRUE(m_motorSim.GetInitialized());
EXPECT_TRUE(m_encoderSim.GetInitialized());
}
{
frc::sim::StepTiming(3_s);
// Ensure elevator is still at 0.
EXPECT_NEAR(kMinAngle.value(), m_encoderSim.GetDistance(), 1.5);
}
{
// Press button to reach setpoint
m_joystickSim.SetTrigger(true);
m_joystickSim.NotifyNewData();
frc::sim::StepTiming(1.5_s);
EXPECT_NEAR(setpoint.value(),
units::radian_t(m_encoderSim.GetDistance())
.convert<units::degree>()
.value(),
1.5);
// see setpoint is held.
frc::sim::StepTiming(0.5_s);
EXPECT_NEAR(setpoint.value(),
units::radian_t(m_encoderSim.GetDistance())
.convert<units::degree>()
.value(),
1.5);
}
{
// Unpress the button to go back down
m_joystickSim.SetTrigger(false);
m_joystickSim.NotifyNewData();
frc::sim::StepTiming(3_s);
EXPECT_NEAR(kMinAngle.value(), m_encoderSim.GetDistance(), 1.5);
}
{
// Press button to go back up
m_joystickSim.SetTrigger(true);
m_joystickSim.NotifyNewData();
// advance 75 timesteps
frc::sim::StepTiming(1.5_s);
EXPECT_NEAR(setpoint.value(),
units::radian_t(m_encoderSim.GetDistance())
.convert<units::degree>()
.value(),
1.5);
// advance 25 timesteps to see setpoint is held.
frc::sim::StepTiming(0.5_s);
EXPECT_NEAR(setpoint.value(),
units::radian_t(m_encoderSim.GetDistance())
.convert<units::degree>()
.value(),
1.5);
}
{
// Disable
frc::sim::DriverStationSim::SetAutonomous(false);
frc::sim::DriverStationSim::SetEnabled(false);
frc::sim::DriverStationSim::NotifyNewData();
frc::sim::StepTiming(3_s);
ASSERT_NEAR(0.0, m_motorSim.GetSpeed(), 0.05);
EXPECT_NEAR(kMinAngle.value(), m_encoderSim.GetDistance(), 1.5);
}
}
INSTANTIATE_TEST_SUITE_P(
ArmSimulationTests, ArmSimulationTest,
testing::Values(kDefaultArmSetpoint, 25.0_deg, 50.0_deg),
[](const testing::TestParamInfo<units::degree_t>& info) {
return testing::PrintToString(info.param.value())
.append(std::string(info.param.abbreviation()));
});

17
wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/main.cpp Normal file
View File

@@ -0,0 +1,17 @@
// 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 <hal/HALBase.h>
#include "gtest/gtest.h"
/**
* Runs all unit tests.
*/
int main(int argc, char** argv) {
HAL_Initialize(500, 0);
::testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}

126
wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp Normal file
View File

@@ -0,0 +1,126 @@
// 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 <gtest/gtest.h>
#include <string>
#include <thread>
#include <frc/simulation/DriverStationSim.h>
#include <frc/simulation/JoystickSim.h>
#include <frc/simulation/PWMSim.h>
#include <frc/simulation/SimHooks.h>
#include <hal/simulation/MockHooks.h>
#include <units/length.h>
#include <units/mass.h>
#include <units/time.h>
#include "Constants.h"
#include "Robot.h"
using namespace Constants;
class ElevatorSimulationTest : public testing::Test {
Robot m_robot;
std::optional<std::thread> m_thread;
protected:
frc::sim::PWMSim m_motorSim{Constants::kMotorPort};
frc::sim::EncoderSim m_encoderSim =
frc::sim::EncoderSim::CreateForChannel(Constants::kEncoderAChannel);
frc::sim::JoystickSim m_joystickSim{Constants::kJoystickPort};
public:
void SetUp() override {
frc::sim::PauseTiming();
m_thread = std::thread([&] { m_robot.StartCompetition(); });
frc::sim::StepTiming(0.0_ms); // Wait for Notifiers
}
void TearDown() override {
m_robot.EndCompetition();
m_thread->join();
m_encoderSim.ResetData();
m_motorSim.ResetData();
frc::sim::DriverStationSim::ResetData();
}
};
TEST_F(ElevatorSimulationTest, Teleop) {
// teleop init
{
frc::sim::DriverStationSim::SetAutonomous(false);
frc::sim::DriverStationSim::SetEnabled(true);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_TRUE(m_motorSim.GetInitialized());
EXPECT_TRUE(m_encoderSim.GetInitialized());
}
{
// advance 50 timesteps
frc::sim::StepTiming(1_s);
// Ensure elevator is still at 0.
EXPECT_NEAR(0.0, m_encoderSim.GetDistance(), 0.05);
}
{
// Press button to reach setpoint
m_joystickSim.SetTrigger(true);
m_joystickSim.NotifyNewData();
// advance 75 timesteps
frc::sim::StepTiming(1.5_s);
EXPECT_NEAR(kSetpoint.value(), m_encoderSim.GetDistance(), 0.05);
// advance 25 timesteps to see setpoint is held.
frc::sim::StepTiming(0.5_s);
EXPECT_NEAR(kSetpoint.value(), m_encoderSim.GetDistance(), 0.05);
}
{
// Unpress the button to go back down
m_joystickSim.SetTrigger(false);
m_joystickSim.NotifyNewData();
// advance 75 timesteps
frc::sim::StepTiming(1.5_s);
EXPECT_NEAR(0.0, m_encoderSim.GetDistance(), 0.05);
}
{
// Press button to go back up
m_joystickSim.SetTrigger(true);
m_joystickSim.NotifyNewData();
// advance 75 timesteps
frc::sim::StepTiming(1.5_s);
EXPECT_NEAR(kSetpoint.value(), m_encoderSim.GetDistance(), 0.05);
// advance 25 timesteps to see setpoint is held.
frc::sim::StepTiming(0.5_s);
EXPECT_NEAR(kSetpoint.value(), m_encoderSim.GetDistance(), 0.05);
}
{
// Disable
frc::sim::DriverStationSim::SetAutonomous(false);
frc::sim::DriverStationSim::SetEnabled(false);
frc::sim::DriverStationSim::NotifyNewData();
// advance 75 timesteps
frc::sim::StepTiming(1.5_s);
ASSERT_NEAR(0.0, m_motorSim.GetSpeed(), 0.05);
ASSERT_NEAR(0.0, m_encoderSim.GetDistance(), 0.05);
}
}

17
wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/main.cpp Normal file
View File

@@ -0,0 +1,17 @@
// 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 <hal/HALBase.h>
#include "gtest/gtest.h"
/**
* Runs all unit tests.
*/
int main(int argc, char** argv) {
HAL_Initialize(500, 0);
::testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}

31
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Constants.java Normal file
View File

@@ -0,0 +1,31 @@
// 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.
package edu.wpi.first.wpilibj.examples.armsimulation;
import edu.wpi.first.math.util.Units;
public class Constants {
public static final int kMotorPort = 0;
public static final int kEncoderAChannel = 0;
public static final int kEncoderBChannel = 1;
public static final int kJoystickPort = 0;
public static final String kArmPositionKey = "ArmPosition";
public static final String kArmPKey = "ArmP";
// The P gain for the PID controller that drives this arm.
public static final double kDefaultArmKp = 50.0;
public static final double kDefaultArmSetpointDegrees = 75.0;
// distance per pulse = (angle per revolution) / (pulses per revolution)
// = (2 * PI rads) / (4096 pulses)
public static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
public static final double kArmReduction = 200;
public static final double kArmMass = 8.0; // Kilograms
public static final double kArmLength = Units.inchesToMeters(30);
public static final double kMinAngleRads = Units.degreesToRadians(-75);
public static final double kMaxAngleRads = Units.degreesToRadians(255);
}

134
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
View File

@@ -4,150 +4,48 @@
package edu.wpi.first.wpilibj.examples.armsimulation;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
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;
import edu.wpi.first.wpilibj.examples.armsimulation.subsystems.Arm;
/** 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;
private static final int kEncoderBChannel = 1;
private static final int kJoystickPort = 0;
public static final String kArmPositionKey = "ArmPosition";
public static final String kArmPKey = "ArmP";
// The P gain for the PID controller that drives this arm.
private static double kArmKp = 50.0;
private static double armPositionDeg = 75.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 gearbox containing two Vex 775pro motors.
private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
// 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 = 200;
private static final double m_armMass = 8.0; // Kilograms
private static final double m_armLength = Units.inchesToMeters(30);
// 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(-75),
Units.degreesToRadians(255),
true,
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)));
private final Arm m_arm = new Arm();
private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
@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));
// Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
if (!Preferences.containsKey(kArmPositionKey)) {
Preferences.setDouble(kArmPositionKey, armPositionDeg);
}
if (!Preferences.containsKey(kArmPKey)) {
Preferences.setDouble(kArmPKey, kArmKp);
}
}
public void robotInit() {}
@Override
public void simulationPeriodic() {
// 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());
// Next, we update it. The standard loop time is 20ms.
m_armSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_armSim.getAngleRads());
// 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()));
m_arm.simulationPeriodic();
}
@Override
public void teleopInit() {
// Read Preferences for Arm setpoint and kP on entering Teleop
armPositionDeg = Preferences.getDouble(kArmPositionKey, armPositionDeg);
if (kArmKp != Preferences.getDouble(kArmPKey, kArmKp)) {
kArmKp = Preferences.getDouble(kArmPKey, kArmKp);
m_controller.setP(kArmKp);
}
m_arm.loadPreferences();
}
@Override
public void teleopPeriodic() {
if (m_joystick.getTrigger()) {
// Here, we run PID control like normal, with a constant setpoint of 75 degrees.
var pidOutput =
m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(armPositionDeg));
m_motor.setVoltage(pidOutput);
// Here, we run PID control like normal.
m_arm.reachSetpoint();
} else {
// Otherwise, we disable the motor.
m_motor.set(0.0);
m_arm.stop();
}
}
@Override
public void close() {
m_arm.close();
super.close();
}
@Override
public void disabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_motor.set(0.0);
m_arm.stop();
}
}

134
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java Normal file
View File

@@ -0,0 +1,134 @@
// 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.
package edu.wpi.first.wpilibj.examples.armsimulation.subsystems;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.examples.armsimulation.Constants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
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;
public class Arm implements AutoCloseable {
// The P gain for the PID controller that drives this arm.
private double m_armKp = Constants.kDefaultArmKp;
private double m_armSetpointDegrees = Constants.kDefaultArmSetpointDegrees;
// The arm gearbox represents a gearbox containing two Vex 775pro motors.
private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
// Standard classes for controlling our arm
private final PIDController m_controller = new PIDController(m_armKp, 0, 0);
private final Encoder m_encoder =
new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
// Simulation classes help us simulate what's going on, including gravity.
// 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,
Constants.kArmReduction,
SingleJointedArmSim.estimateMOI(Constants.kArmLength, Constants.kArmMass),
Constants.kArmLength,
Constants.kMinAngleRads,
Constants.kMaxAngleRads,
true,
VecBuilder.fill(Constants.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)));
/** Subsystem constructor. */
public Arm() {
m_encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse);
// Put Mechanism 2d to SmartDashboard
SmartDashboard.putData("Arm Sim", m_mech2d);
m_armTower.setColor(new Color8Bit(Color.kBlue));
// Set the Arm position setpoint and P constant to Preferences if the keys don't already exist
Preferences.initDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
Preferences.initDouble(Constants.kArmPKey, m_armKp);
}
/** Update the simulation model. */
public void simulationPeriodic() {
// 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());
// Next, we update it. The standard loop time is 20ms.
m_armSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_armSim.getAngleRads());
// 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()));
}
/** Load setpoint and kP from preferences. */
public void loadPreferences() {
// Read Preferences for Arm setpoint and kP on entering Teleop
m_armSetpointDegrees = Preferences.getDouble(Constants.kArmPositionKey, m_armSetpointDegrees);
if (m_armKp != Preferences.getDouble(Constants.kArmPKey, m_armKp)) {
m_armKp = Preferences.getDouble(Constants.kArmPKey, m_armKp);
m_controller.setP(m_armKp);
}
}
/** Run the control loop to reach and maintain the setpoint from the preferences. */
public void reachSetpoint() {
var pidOutput =
m_controller.calculate(
m_encoder.getDistance(), Units.degreesToRadians(m_armSetpointDegrees));
m_motor.setVoltage(pidOutput);
}
public void stop() {
m_motor.set(0.0);
}
@Override
public void close() {
m_motor.close();
m_encoder.close();
m_mech2d.close();
m_armPivot.close();
m_controller.close();
m_arm.close();
}
}

37
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java Normal file
View File

@@ -0,0 +1,37 @@
// 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.
package edu.wpi.first.wpilibj.examples.elevatorsimulation;
import edu.wpi.first.math.util.Units;
public class Constants {
public static final int kMotorPort = 0;
public static final int kEncoderAChannel = 0;
public static final int kEncoderBChannel = 1;
public static final int kJoystickPort = 0;
public static final double kElevatorKp = 5;
public static final double kElevatorKi = 0;
public static final double kElevatorKd = 0;
public static final double kElevatorkS = 0.0; // volts (V)
public static final double kElevatorkG = 0.762; // volts (V)
public static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s))
public static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²))
public static final double kElevatorGearing = 10.0;
public static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
public static final double kCarriageMass = 4.0; // kg
public static final double kSetpointMeters = 0.75;
// Encoder is reset to measure 0 at the bottom, so minimum height is 0.
public static final double kMinElevatorHeightMeters = 0.0;
public static final double kMaxElevatorHeightMeters = 1.25;
// distance per pulse = (distance per revolution) / (pulses per revolution)
// = (Pi * D) / ppr
public static final double kElevatorEncoderDistPerPulse =
2.0 * Math.PI * kElevatorDrumRadius / 4096;
}

127
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
View File

@@ -4,137 +4,50 @@
package edu.wpi.first.wpilibj.examples.elevatorsimulation;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
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;
import edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems.Elevator;
/** This is a sample program to demonstrate the use of elevator simulation. */
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
private static final int kEncoderAChannel = 0;
private static final int kEncoderBChannel = 1;
private static final int kJoystickPort = 0;
private static final double kElevatorKp = 5;
private static final double kElevatorKi = 0;
private static final double kElevatorKd = 0;
private static final double kElevatorkS = 0.0; // volts (V)
private static final double kElevatorkG = 0.762; // volts (V)
private static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s))
private static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²))
private static final double kElevatorGearing = 10.0;
private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
private static final double kCarriageMass = 4.0; // kg
private static final double kSetpoint = Units.inchesToMeters(30);
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)
// = (Pi * D) / ppr
private static final double kElevatorEncoderDistPerPulse =
2.0 * Math.PI * kElevatorDrumRadius / 4096;
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
// Standard classes for controlling our elevator
private final ProfiledPIDController m_controller =
new ProfiledPIDController(
kElevatorKp, kElevatorKi, kElevatorKd, new TrapezoidProfile.Constraints(2.45, 2.45));
ElevatorFeedforward m_feedforward =
new ElevatorFeedforward(kElevatorkS, kElevatorkG, kElevatorkV, kElevatorkA);
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 final ElevatorSim m_elevatorSim =
new ElevatorSim(
m_elevatorGearbox,
kElevatorGearing,
kCarriageMass,
kElevatorDrumRadius,
kMinElevatorHeight,
kMaxElevatorHeight,
true,
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));
private final Joystick m_joystick = new Joystick(Constants.kJoystickPort);
private final Elevator m_elevator = new Elevator();
@Override
public void robotInit() {
m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse);
public void robotInit() {}
// Publish Mechanism2d to SmartDashboard
// To view the Elevator Sim in the simulator, select Network Tables -> SmartDashboard ->
// Elevator Sim
SmartDashboard.putData("Elevator Sim", m_mech2d);
@Override
public void robotPeriodic() {
// Update the telemetry, including mechanism visualization, regardless of mode.
m_elevator.updateTelemetry();
}
@Override
public void simulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
// 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()));
// Update the simulation model.
m_elevator.simulationPeriodic();
}
@Override
public void teleopPeriodic() {
if (m_joystick.getTrigger()) {
// Here, we set the constant setpoint of 30in.
m_controller.setGoal(kSetpoint);
// Here, we set the constant setpoint of 0.75 meters.
m_elevator.reachGoal(Constants.kSetpointMeters);
} else {
// Otherwise, we update the setpoint to 0.
m_controller.setGoal(0.0);
m_elevator.reachGoal(0.0);
}
// With the setpoint value we run PID control like normal
double pidOutput = m_controller.calculate(m_encoder.getDistance());
double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
m_motor.setVoltage(pidOutput + feedforwardOutput);
}
// To view the Elevator in the simulator, select Network Tables -> SmartDashboard -> Elevator Sim
@Override
public void disabledInit() {
// This just makes sure that our simulation code knows that the motor's off.
m_motor.set(0.0);
m_elevator.stop();
}
@Override
public void close() {
m_elevator.close();
super.close();
}
}

125
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java Normal file
View File

@@ -0,0 +1,125 @@
// 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.
package edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.ElevatorFeedforward;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.examples.elevatorsimulation.Constants;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
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.PWMSim;
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;
public class Elevator implements AutoCloseable {
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
// Standard classes for controlling our elevator
private final ProfiledPIDController m_controller =
new ProfiledPIDController(
Constants.kElevatorKp,
Constants.kElevatorKi,
Constants.kElevatorKd,
new TrapezoidProfile.Constraints(2.45, 2.45));
ElevatorFeedforward m_feedforward =
new ElevatorFeedforward(
Constants.kElevatorkS,
Constants.kElevatorkG,
Constants.kElevatorkV,
Constants.kElevatorkA);
private final Encoder m_encoder =
new Encoder(Constants.kEncoderAChannel, Constants.kEncoderBChannel);
private final PWMSparkMax m_motor = new PWMSparkMax(Constants.kMotorPort);
// Simulation classes help us simulate what's going on, including gravity.
private final ElevatorSim m_elevatorSim =
new ElevatorSim(
m_elevatorGearbox,
Constants.kElevatorGearing,
Constants.kCarriageMass,
Constants.kElevatorDrumRadius,
Constants.kMinElevatorHeightMeters,
Constants.kMaxElevatorHeightMeters,
true,
VecBuilder.fill(0.01));
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
private final PWMSim m_motorSim = new PWMSim(m_motor);
// 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", m_elevatorSim.getPositionMeters(), 90));
/** Subsystem constructor. */
public Elevator() {
m_encoder.setDistancePerPulse(Constants.kElevatorEncoderDistPerPulse);
// Publish Mechanism2d to SmartDashboard
// To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim
SmartDashboard.putData("Elevator Sim", m_mech2d);
}
/** Advance the simulation. */
public void simulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.update(0.020);
// Finally, we set our simulated encoder's readings and simulated battery voltage
m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
// SimBattery estimates loaded battery voltages
RoboRioSim.setVInVoltage(
BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
}
/**
* Run control loop to reach and maintain goal.
*
* @param goal the position to maintain
*/
public void reachGoal(double goal) {
m_controller.setGoal(goal);
// With the setpoint value we run PID control like normal
double pidOutput = m_controller.calculate(m_encoder.getDistance());
double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
m_motor.setVoltage(pidOutput + feedforwardOutput);
}
/** Stop the control loop and motor output. */
public void stop() {
m_controller.setGoal(0.0);
m_motor.set(0.0);
}
/** Update telemetry, including the mechanism visualization. */
public void updateTelemetry() {
// Update elevator visualization with position
m_elevatorMech2d.setLength(m_encoder.getDistance());
}
@Override
public void close() {
m_encoder.close();
m_motor.close();
m_mech2d.close();
}
}

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
View File

@@ -8,7 +8,6 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
@@ -18,7 +17,7 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
public class Robot extends TimedRobot {
static final int kPotChannel = 1;
static final int kMotorChannel = 7;
static final int kJoystickChannel = 0;
static final int kJoystickChannel = 3;
// The elevator can move 1.5 meters from top to bottom
static final double kFullHeightMeters = 1.5;
@@ -37,7 +36,7 @@ public class Robot extends TimedRobot {
// Scaling is handled internally
private final AnalogPotentiometer m_potentiometer =
new AnalogPotentiometer(kPotChannel, kFullHeightMeters);
private final MotorController m_elevatorMotor = new PWMSparkMax(kMotorChannel);
private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(kMotorChannel);
private final Joystick m_joystick = new Joystick(kJoystickChannel);
private int m_index;
@@ -64,7 +63,17 @@ public class Robot extends TimedRobot {
if (m_joystick.getTriggerPressed()) {
// index of the elevator setpoint wraps around.
m_index = (m_index + 1) % kSetpointsMeters.length;
System.out.println("m_index = " + m_index);
m_pidController.setSetpoint(kSetpointsMeters[m_index]);
}
}
@Override
public void close() {
m_elevatorMotor.close();
m_potentiometer.close();
m_pidController.close();
m_index = 0;
super.close();
}
}

153
wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/armsimulation/ArmSimulationTest.java Normal file
View File

@@ -0,0 +1,153 @@
// 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.
package edu.wpi.first.wpilibj.examples.armsimulation;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.simulation.JoystickSim;
import edu.wpi.first.wpilibj.simulation.PWMSim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj.simulation.SimHooks;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.parallel.ResourceLock;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.ValueSource;
@ResourceLock("timing")
class ArmSimulationTest {
private Robot m_robot;
private Thread m_thread;
private PWMSim m_motorSim;
private EncoderSim m_encoderSim;
private JoystickSim m_joystickSim;
@BeforeEach
void startThread() {
HAL.initialize(500, 0);
SimHooks.pauseTiming();
DriverStationSim.resetData();
m_robot = new Robot();
m_thread = new Thread(m_robot::startCompetition);
m_encoderSim = EncoderSim.createForChannel(Constants.kEncoderAChannel);
m_motorSim = new PWMSim(Constants.kMotorPort);
m_joystickSim = new JoystickSim(Constants.kJoystickPort);
m_thread.start();
SimHooks.stepTiming(0.0); // Wait for Notifiers
}
@AfterEach
void stopThread() {
m_robot.endCompetition();
try {
m_thread.interrupt();
m_thread.join();
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
m_robot.close();
m_encoderSim.resetData();
m_motorSim.resetData();
Preferences.remove(Constants.kArmPKey);
Preferences.remove(Constants.kArmPositionKey);
Preferences.removeAll();
RoboRioSim.resetData();
DriverStationSim.resetData();
DriverStationSim.notifyNewData();
}
@ValueSource(doubles = {Constants.kDefaultArmSetpointDegrees, 25.0, 50.0})
@ParameterizedTest
void teleopTest(double setpoint) {
assertTrue(Preferences.containsKey(Constants.kArmPositionKey));
assertTrue(Preferences.containsKey(Constants.kArmPKey));
assertEquals(
Constants.kDefaultArmSetpointDegrees,
Preferences.getDouble(Constants.kArmPositionKey, Double.NaN));
Preferences.setDouble(Constants.kArmPositionKey, setpoint);
// teleop init
{
DriverStationSim.setAutonomous(false);
DriverStationSim.setEnabled(true);
DriverStationSim.notifyNewData();
assertTrue(m_motorSim.getInitialized());
assertTrue(m_encoderSim.getInitialized());
}
{
// advance 50 timesteps
SimHooks.stepTiming(3);
// Ensure elevator is still at 0.
assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 1.5);
}
{
// Press button to reach setpoint
m_joystickSim.setTrigger(true);
m_joystickSim.notifyNewData();
// advance 75 timesteps
SimHooks.stepTiming(1.5);
assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 1.5);
// advance 25 timesteps to see setpoint is held.
SimHooks.stepTiming(0.5);
assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 1.5);
}
{
// Unpress the button to go back down
m_joystickSim.setTrigger(false);
m_joystickSim.notifyNewData();
// advance 75 timesteps
SimHooks.stepTiming(3.0);
assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 1.5);
}
{
// Press button to go back up
m_joystickSim.setTrigger(true);
m_joystickSim.notifyNewData();
// advance 75 timesteps
SimHooks.stepTiming(1.5);
assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 1.5);
// advance 25 timesteps to see setpoint is held.
SimHooks.stepTiming(0.5);
assertEquals(setpoint, Units.radiansToDegrees(m_encoderSim.getDistance()), 1.5);
}
{
// Disable
DriverStationSim.setAutonomous(false);
DriverStationSim.setEnabled(false);
DriverStationSim.notifyNewData();
// advance 75 timesteps
SimHooks.stepTiming(3.5);
assertEquals(0.0, m_motorSim.getSpeed(), 0.01);
assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 1.5);
}
}
}

139
wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java Normal file
View File

@@ -0,0 +1,139 @@
// 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.
package edu.wpi.first.wpilibj.examples.elevatorsimulation;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.simulation.JoystickSim;
import edu.wpi.first.wpilibj.simulation.PWMSim;
import edu.wpi.first.wpilibj.simulation.RoboRioSim;
import edu.wpi.first.wpilibj.simulation.SimHooks;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.api.parallel.ResourceLock;
@ResourceLock("timing")
class ElevatorSimulationTest {
private Robot m_robot;
private Thread m_thread;
private PWMSim m_motorSim;
private EncoderSim m_encoderSim;
private JoystickSim m_joystickSim;
@BeforeEach
void startThread() {
HAL.initialize(500, 0);
SimHooks.pauseTiming();
DriverStationSim.resetData();
m_robot = new Robot();
m_thread = new Thread(m_robot::startCompetition);
m_encoderSim = EncoderSim.createForChannel(Constants.kEncoderAChannel);
m_motorSim = new PWMSim(Constants.kMotorPort);
m_joystickSim = new JoystickSim(Constants.kJoystickPort);
m_thread.start();
SimHooks.stepTiming(0.0); // Wait for Notifiers
}
@AfterEach
void stopThread() {
m_robot.endCompetition();
try {
m_thread.interrupt();
m_thread.join();
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
m_robot.close();
m_encoderSim.resetData();
m_motorSim.resetData();
RoboRioSim.resetData();
DriverStationSim.resetData();
DriverStationSim.notifyNewData();
}
@Test
void teleopTest() {
// teleop init
{
DriverStationSim.setAutonomous(false);
DriverStationSim.setEnabled(true);
DriverStationSim.notifyNewData();
assertTrue(m_motorSim.getInitialized());
assertTrue(m_encoderSim.getInitialized());
}
{
// advance 50 timesteps
SimHooks.stepTiming(1);
// Ensure elevator is still at 0.
assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
}
{
// Press button to reach setpoint
m_joystickSim.setTrigger(true);
m_joystickSim.notifyNewData();
// advance 75 timesteps
SimHooks.stepTiming(1.5);
assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
// advance 25 timesteps to see setpoint is held.
SimHooks.stepTiming(0.5);
assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
}
{
// Unpress the button to go back down
m_joystickSim.setTrigger(false);
m_joystickSim.notifyNewData();
// advance 75 timesteps
SimHooks.stepTiming(1.5);
assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
}
{
// Press button to go back up
m_joystickSim.setTrigger(true);
m_joystickSim.notifyNewData();
// advance 75 timesteps
SimHooks.stepTiming(1.5);
assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
// advance 25 timesteps to see setpoint is held.
SimHooks.stepTiming(0.5);
assertEquals(Constants.kSetpointMeters, m_encoderSim.getDistance(), 0.05);
}
{
// Disable
DriverStationSim.setAutonomous(false);
DriverStationSim.setEnabled(false);
DriverStationSim.notifyNewData();
// advance 75 timesteps
SimHooks.stepTiming(1.5);
assertEquals(0.0, m_motorSim.getSpeed(), 0.05);
assertEquals(0.0, m_encoderSim.getDistance(), 0.05);
}
}
}