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..71acb0d501a --- /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.0_V; +static constexpr auto kElevatorkV = 0.762_V / 1_mps; +static constexpr auto kElevatorkA = 0.762_V / 1_mps_sq; + +static constexpr double kElevatorGearing = 10.0; +static constexpr units::meter_t kElevatorDrumRadius = 2_in; +static constexpr units::kilogram_t kCarriageMass = 4.0_kg; + +static constexpr units::meter_t kSetpoint = 75_cm; +static constexpr units::meter_t kMinElevatorHeight = 0_cm; +static constexpr units::meter_t kMaxElevatorHeight = 1.25_m; + +// distance per pulse = (distance per revolution) / (pulses per revolution) +// = (Pi * D) / ppr +static constexpr double kArmEncoderDistPerPulse = + 2.0 * std::numbers::pi * kElevatorDrumRadius.value() / 4096.0; + +} // namespace Constants 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/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp b/wpilibcExamples/src/test/cpp/examples/ElevatorSimulation/cpp/ElevatorSimulationTest.cpp new file mode 100644 index 00000000000..72e911c7827 --- /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(); + 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_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/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/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..d988d7a8811 --- /dev/null +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/ElevatorSimulationTest.java @@ -0,0 +1,135 @@ +// 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.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(); + } + + @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); + } + } +}