From 43975ac7cc1287dc83191a1bda3898c901fa6e31 Mon Sep 17 00:00:00 2001 From: Starlight220 <53231611+Starlight220@users.noreply.github.com> Date: Sun, 12 Feb 2023 16:50:57 +0200 Subject: [PATCH] [examples] ArmSimulation, ElevatorSimulation: Extract mechanism to class (#5052) --- .../cpp/examples/ArmSimulation/cpp/Robot.cpp | 158 ++--------------- .../ArmSimulation/cpp/subsystems/Arm.cpp | 64 +++++++ .../ArmSimulation/include/Constants.h | 46 +++++ .../examples/ArmSimulation/include/Robot.h | 26 +++ .../ArmSimulation/include/subsystems/Arm.h | 67 +++++++ .../examples/ElevatorSimulation/cpp/Robot.cpp | 163 +++--------------- .../cpp/subsystems/Elevator.cpp | 55 ++++++ .../ElevatorSimulation/include/Constants.h | 55 ++++++ .../ElevatorSimulation/include/Robot.h | 26 +++ .../include/subsystems/Elevator.h | 69 ++++++++ .../examples/PotentiometerPID/include/Robot.h | 2 +- .../ArmSimulation/cpp/ArmSimulationTest.cpp | 152 ++++++++++++++++ .../cpp/examples/ArmSimulation/cpp/main.cpp | 17 ++ .../cpp/ElevatorSimulationTest.cpp | 126 ++++++++++++++ .../examples/ElevatorSimulation/cpp/main.cpp | 17 ++ .../examples/armsimulation/Constants.java | 31 ++++ .../wpilibj/examples/armsimulation/Robot.java | 134 ++------------ .../armsimulation/subsystems/Arm.java | 134 ++++++++++++++ .../elevatorsimulation/Constants.java | 37 ++++ .../examples/elevatorsimulation/Robot.java | 127 +++----------- .../subsystems/Elevator.java | 125 ++++++++++++++ .../examples/potentiometerpid/Robot.java | 15 +- .../armsimulation/ArmSimulationTest.java | 153 ++++++++++++++++ .../ElevatorSimulationTest.java | 139 +++++++++++++++ 24 files changed, 1430 insertions(+), 508 deletions(-) create mode 100644 wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/subsystems/Elevator.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Robot.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h create mode 100644 wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp create mode 100644 wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/main.cpp create mode 100644 wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp create mode 100644 wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/main.cpp create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Constants.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java create mode 100644 wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/armsimulation/ArmSimulationTest.java create mode 100644 wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index 8e51ccbcc95..8641f4c2839 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -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 +#include "Robot.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/** - * 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; - - 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( - "Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue}); - frc::MechanismLigament2d* m_arm = m_armBase->Append( - "Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow}); - - public: - void RobotInit() override { - m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); - - // Put Mechanism 2d to SmartDashboard - frc::SmartDashboard::PutData("Arm Sim", &m_mech2d); - - // 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 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 Robot::SimulationPeriodic() { + m_arm.SimulationPeriodic(); +} - 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 Robot::TeleopInit() { + m_arm.LoadPreferences(); +} - 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 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 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() { diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp new file mode 100644 index 00000000000..95934cefaf8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp @@ -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 +#include +#include +#include + +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); +} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h new file mode 100644 index 00000000000..0018a8aa586 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h @@ -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 + +#include +#include +#include +#include +#include +#include + +/** + * 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; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Robot.h new file mode 100644 index 00000000000..fdcb5ce7f9a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Robot.h @@ -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 +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h new file mode 100644 index 00000000000..811c645b26c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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( + "Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue}); + frc::MechanismLigament2d* m_arm = m_armBase->Append( + "Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow}); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp index d0835eb11cd..1be347c82be 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -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 +#include "Robot.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#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; - - 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 = 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::Constraints m_constraints{2.45_mps, - 2.45_mps_sq}; - frc::ProfiledPIDController 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( - "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 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()})); +void Robot::RobotPeriodic() { + // Update the telemetry, including mechanism visualization, regardless of + // mode. + m_elevator.UpdateTelemetry(); +} - // Update the Elevator length based on the simulated elevator height - m_elevatorMech2d->SetLength( - units::inch_t{m_elevatorSim.GetPosition()}.value()); - } +void Robot::SimulationPeriodic() { + // Update the simulation model. + m_elevator.SimulationPeriodic(); +} - 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); +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); } - // 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() { diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/subsystems/Elevator.cpp new file mode 100644 index 00000000000..35e11ffa34a --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/subsystems/Elevator.cpp @@ -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 +#include +#include + +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); +} diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h new file mode 100644 index 00000000000..7be706f9f5c --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Constants.h @@ -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 + +#include +#include +#include +#include +#include +#include +#include + +/** + * 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 diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Robot.h new file mode 100644 index 00000000000..53f50eabfea --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/Robot.h @@ -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 +#include + +#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; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h new file mode 100644 index 00000000000..0f1313a9514 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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::Constraints m_constraints{2.45_mps, + 2.45_mps_sq}; + frc::ProfiledPIDController 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( + "Elevator", m_elevatorSim.GetPosition().value(), 90_deg); +}; diff --git a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h index f0002bc78cf..bec50ee3e53 100644 --- a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h @@ -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; diff --git a/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp b/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp new file mode 100644 index 00000000000..30e85f3e36c --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp @@ -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 + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "Robot.h" + +class ArmSimulationTest : public testing::TestWithParam { + Robot m_robot; + std::optional 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() + .value(), + 1.5); + + // see setpoint is held. + frc::sim::StepTiming(0.5_s); + + EXPECT_NEAR(setpoint.value(), + units::radian_t(m_encoderSim.GetDistance()) + .convert() + .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() + .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() + .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& info) { + return testing::PrintToString(info.param.value()) + .append(std::string(info.param.abbreviation())); + }); diff --git a/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/main.cpp b/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/main.cpp new file mode 100644 index 00000000000..285c1d52670 --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/main.cpp @@ -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 + +#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; +} diff --git a/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp b/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp new file mode 100644 index 00000000000..f0b68c558c6 --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp @@ -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 + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "Robot.h" + +using namespace Constants; + +class ElevatorSimulationTest : public testing::Test { + Robot m_robot; + std::optional 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); + } +} diff --git a/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/main.cpp b/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/main.cpp new file mode 100644 index 00000000000..285c1d52670 --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/main.cpp @@ -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 + +#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; +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Constants.java new file mode 100644 index 00000000000..68f82858ec6 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Constants.java @@ -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); +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java index a46be5bb03a..80a11215cd5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java @@ -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(); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java new file mode 100644 index 00000000000..de464e83103 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java @@ -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(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java new file mode 100644 index 00000000000..f5a7567b377 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Constants.java @@ -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; +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java index 2cf88fa3f79..de39b88a60a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java @@ -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(); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java new file mode 100644 index 00000000000..7ac9fd2b091 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -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(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java index 9be35540b01..65549bb1067 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java @@ -8,7 +8,6 @@ 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 @@ 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 void teleopPeriodic() { 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(); + } } diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/armsimulation/ArmSimulationTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/armsimulation/ArmSimulationTest.java new file mode 100644 index 00000000000..6409063bdca --- /dev/null +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/armsimulation/ArmSimulationTest.java @@ -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); + } + } +} diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java new file mode 100644 index 00000000000..7faeec68f33 --- /dev/null +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java @@ -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); + } + } +}