diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index 8e51ccbcc95..c3234c77261 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -2,150 +2,33 @@ // 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 +void Robot::RobotInit() {} -/** - * 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()})); +void Robot::SimulationPeriodic() { + m_arm.SimulationPeriodic(); +} - // Update the Mechanism Arm angle based on the simulated arm angle - m_arm->SetAngle(m_armSim.GetAngle()); - } +void Robot::TeleopInit() { + m_arm.LoadPreferences(); +} - 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::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 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::DisabledInit() { + // This just makes sure that our simulation code knows that the motor's off. + 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); - } -}; #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..d0487a8b2a6 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/subsystems/Arm.cpp @@ -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. + +#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 + if (!frc::Preferences::ContainsKey(kArmPositionKey)) { + frc::Preferences::SetDouble(kArmPositionKey, armSetpoint.value()); + } + if (!frc::Preferences::ContainsKey(kArmPKey)) { + frc::Preferences::SetDouble(kArmPKey, kArmKp); + } +} + +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 + armSetpoint = units::degree_t{ + frc::Preferences::GetDouble(kArmPositionKey, armSetpoint.value())}; + if (kArmKp != frc::Preferences::GetDouble(kArmPKey, kArmKp)) { + kArmKp = frc::Preferences::GetDouble(kArmPKey, kArmKp); + m_controller.SetP(kArmKp); + } +} + +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{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..3042962ac52 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Constants.h @@ -0,0 +1,47 @@ +// 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..144987a0628 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/Robot.h @@ -0,0 +1,27 @@ +// 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 RobotPeriodic() 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..dd42630ecce --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.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 "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 kArmKp = kDefaultArmKp; + units::radian_t 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{kArmKp, 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/test/cpp/examples/ArmSimulation/cpp/ArmTest.cpp b/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmTest.cpp new file mode 100644 index 00000000000..209b4193879 --- /dev/null +++ b/wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmTest.cpp @@ -0,0 +1,131 @@ +// 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(); + frc::sim::DriverStationSim::ResetData(); + + 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(); + } +}; + +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()); + } + + { + // advance 50 timesteps + frc::sim::StepTiming(3_s); + + // Ensure elevator is still at 0. + EXPECT_NEAR(kMinAngle.value(), units::degree_t(units::radian_t(m_encoderSim.GetDistance())).value(), 1.5); + } + + { + // Press button to reach setpoint + m_joystickSim.SetTrigger(true); + m_joystickSim.NotifyNewData(); + + // advance 75 timesteps + frc::sim::StepTiming(1.5_s); + + EXPECT_NEAR(setpoint.value(), units::degree_t(units::radian_t(m_encoderSim.GetDistance())).value(), 1.5); + + // advance 25 timesteps to see setpoint is held. + frc::sim::StepTiming(0.5_s); + + EXPECT_NEAR(setpoint.value(), units::degree_t(units::radian_t(m_encoderSim.GetDistance())).value(), 1.5); + } + + { + // 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(kMinAngle.value(), units::degree_t(units::radian_t(m_encoderSim.GetDistance())).value(), 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::degree_t(units::radian_t(m_encoderSim.GetDistance())).value(), 1.5); + + // advance 25 timesteps to see setpoint is held. + frc::sim::StepTiming(0.5_s); + + EXPECT_NEAR(setpoint.value(), units::degree_t(units::radian_t(m_encoderSim.GetDistance())).value(), 1.5); + } + + { + // 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); + EXPECT_NEAR(kMinAngle.value(), units::degree_t(units::radian_t(m_encoderSim.GetDistance())).value(), 1.5); + } +} 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/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..eecb5a74319 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Constants.java @@ -0,0 +1,27 @@ +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..8a69e751630 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java @@ -0,0 +1,131 @@ +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 kArmKp = Constants.kDefaultArmKp; + private double 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(kArmKp, 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 + if (!Preferences.containsKey(Constants.kArmPositionKey)) { + Preferences.setDouble(Constants.kArmPositionKey, armSetpointDegrees); + } + if (!Preferences.containsKey(Constants.kArmPKey)) { + Preferences.setDouble(Constants.kArmPKey, kArmKp); + } + } + + /** 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 + armSetpointDegrees = Preferences.getDouble(Constants.kArmPositionKey, armSetpointDegrees); + if (kArmKp != Preferences.getDouble(Constants.kArmPKey, kArmKp)) { + kArmKp = Preferences.getDouble(Constants.kArmPKey, kArmKp); + m_controller.setP(kArmKp); + } + } + + /** 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(armSetpointDegrees)); + m_motor.setVoltage(pidOutput); + } + + public void stop() { + m_motor.set(0.0); + } + + public void close() { + m_motor.close(); + m_encoder.close(); + m_mech2d.close(); + m_armPivot.close(); + m_arm.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..a65f6aad11b --- /dev/null +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/armsimulation/ArmSimulationTest.java @@ -0,0 +1,147 @@ +// 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.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.removeAll(); + } + + @ValueSource(doubles = {Constants.kDefaultArmSetpointDegrees, 25.0, 50.0, 0.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); + } + } +}