mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
f42bc45ee8
This is a cleanup of the FlywheelSim class with a few added features. - One FlywheelSim constructor that takes a plant, DCMotor, and a optional number of measurementStdDevs. The documentation now states how to construct the plant either through LinearSystemId.createFlywheelSystem or identifyVelocitySystem. - The gearbox, gearing and moment of Inertia (J) are now private final fields. The gearing is determined from the plant in the constructor as well as the moment of inertia. There are getter methods that allow the flywheelSim to return the gearbox, gearing, and moment of inertia. - The getCurrentDrawAmps function now uses m_x instead of getAngularVelocityRadPerSec in accordance with more accuracy and matches the patter in other sims. - Added getter methods for the InputVoltage, angularAcceleration and torque - (Java only) A third getter method for returning the AngularVelocity of the flywheel using a MutableMeasure as a backing field that is set when getAngularVelocity is called. This summarily returns the angularVelocity as just a Measure object. This allows the user of this class to handle unit conversions with less numerical manipulation. Alterations in C++ for this feature were not needed.
56 lines
2.1 KiB
C++
56 lines
2.1 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include <gtest/gtest.h>
|
|
#include <units/angular_acceleration.h>
|
|
#include <units/angular_velocity.h>
|
|
|
|
#include "frc/Encoder.h"
|
|
#include "frc/RobotController.h"
|
|
#include "frc/controller/PIDController.h"
|
|
#include "frc/controller/SimpleMotorFeedforward.h"
|
|
#include "frc/motorcontrol/PWMVictorSPX.h"
|
|
#include "frc/simulation/BatterySim.h"
|
|
#include "frc/simulation/DifferentialDrivetrainSim.h"
|
|
#include "frc/simulation/ElevatorSim.h"
|
|
#include "frc/simulation/EncoderSim.h"
|
|
#include "frc/simulation/FlywheelSim.h"
|
|
#include "frc/simulation/LinearSystemSim.h"
|
|
#include "frc/simulation/PWMSim.h"
|
|
#include "frc/simulation/RoboRioSim.h"
|
|
#include "frc/system/plant/LinearSystemId.h"
|
|
|
|
TEST(StateSpaceSimTest, FlywheelSim) {
|
|
const frc::LinearSystem<1, 1, 1> plant =
|
|
frc::LinearSystemId::IdentifyVelocitySystem<units::radian>(
|
|
0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq);
|
|
frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2)};
|
|
frc::PIDController controller{0.2, 0.0, 0.0};
|
|
frc::SimpleMotorFeedforward<units::radian> feedforward{
|
|
0_V, 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
|
|
frc::Encoder encoder{0, 1};
|
|
frc::sim::EncoderSim encoderSim{encoder};
|
|
frc::PWMVictorSPX motor{0};
|
|
|
|
frc::sim::RoboRioSim::ResetData();
|
|
encoderSim.ResetData();
|
|
|
|
for (int i = 0; i < 100; i++) {
|
|
// RobotPeriodic runs first
|
|
auto voltageOut = controller.Calculate(encoder.GetRate(), 200.0);
|
|
motor.SetVoltage(units::volt_t{voltageOut} +
|
|
feedforward.Calculate(200_rad_per_s));
|
|
|
|
// Then, SimulationPeriodic runs
|
|
frc::sim::RoboRioSim::SetVInVoltage(
|
|
frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
|
|
sim.SetInput(
|
|
frc::Vectord<1>{motor.Get() * frc::RobotController::GetInputVoltage()});
|
|
sim.Update(20_ms);
|
|
encoderSim.SetRate(sim.GetAngularVelocity().value());
|
|
}
|
|
|
|
ASSERT_TRUE(std::abs(200 - encoder.GetRate()) < 0.1);
|
|
}
|