mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
d6f185d8e5
I started with the output of styleguide#217, then renamed a few classes to fix compilation. ntcore's StorageTest needed some manual renaming since it put the Test word in the middle instead of at the end. One limitation of wpiformat is test cases that were only named "Test" were unmodified, and an error was generated. These test cases were manually given more descriptive names: * TimedRobotTest mode test cases had "Mode" appended to the name. Java tests were renamed to match. * UvAsyncTest and UvAsyncFunctionTest cases were given alternate names
92 lines
2.9 KiB
C++
92 lines
2.9 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 <iostream>
|
|
|
|
#include <units/time.h>
|
|
|
|
#include "frc/Encoder.h"
|
|
#include "frc/RobotController.h"
|
|
#include "frc/controller/PIDController.h"
|
|
#include "frc/motorcontrol/PWMVictorSPX.h"
|
|
#include "frc/simulation/ElevatorSim.h"
|
|
#include "frc/simulation/EncoderSim.h"
|
|
#include "frc/system/NumericalIntegration.h"
|
|
#include "frc/system/plant/DCMotor.h"
|
|
#include "frc/system/plant/LinearSystemId.h"
|
|
#include "gtest/gtest.h"
|
|
|
|
TEST(ElevatorSimTest, StateSpaceSim) {
|
|
frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg,
|
|
units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 3_m,
|
|
{0.01});
|
|
frc2::PIDController controller(10, 0.0, 0.0);
|
|
|
|
frc::PWMVictorSPX motor(0);
|
|
frc::Encoder encoder(0, 1);
|
|
frc::sim::EncoderSim encoderSim(encoder);
|
|
|
|
for (size_t i = 0; i < 100; ++i) {
|
|
controller.SetSetpoint(2.0);
|
|
auto nextVoltage = controller.Calculate(encoderSim.GetDistance());
|
|
motor.Set(nextVoltage / frc::RobotController::GetInputVoltage());
|
|
|
|
Eigen::Vector<double, 1> u{motor.Get() *
|
|
frc::RobotController::GetInputVoltage()};
|
|
sim.SetInput(u);
|
|
sim.Update(20_ms);
|
|
|
|
const auto& y = sim.GetOutput();
|
|
encoderSim.SetDistance(y(0));
|
|
}
|
|
|
|
EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().to<double>(), 0.2);
|
|
}
|
|
|
|
TEST(ElevatorSimTest, MinMax) {
|
|
frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg,
|
|
units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m,
|
|
{0.01});
|
|
for (size_t i = 0; i < 100; ++i) {
|
|
sim.SetInput(Eigen::Vector<double, 1>{0.0});
|
|
sim.Update(20_ms);
|
|
|
|
auto height = sim.GetPosition();
|
|
EXPECT_TRUE(height > -0.05_m);
|
|
}
|
|
|
|
for (size_t i = 0; i < 100; ++i) {
|
|
sim.SetInput(Eigen::Vector<double, 1>{12.0});
|
|
sim.Update(20_ms);
|
|
|
|
auto height = sim.GetPosition();
|
|
EXPECT_TRUE(height < 1.05_m);
|
|
}
|
|
}
|
|
|
|
TEST(ElevatorSimTest, Stability) {
|
|
static constexpr double kElevatorGearing = 100.0;
|
|
static constexpr units::meter_t kElevatorDrumRadius = 0.5_in;
|
|
static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
|
|
frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
|
|
|
|
frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
|
|
m_elevatorGearbox, kCarriageMass, kElevatorDrumRadius, kElevatorGearing);
|
|
|
|
Eigen::Vector<double, 2> x0{0.0, 0.0};
|
|
Eigen::Vector<double, 1> u0{12.0};
|
|
|
|
Eigen::Vector<double, 2> x1{0.0, 0.0};
|
|
for (size_t i = 0; i < 50; i++) {
|
|
x1 = frc::RKDP(
|
|
[&](const Eigen::Vector<double, 2>& x,
|
|
const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 2> {
|
|
return system.A() * x + system.B() * u;
|
|
},
|
|
x1, u0, 0.020_s);
|
|
}
|
|
|
|
EXPECT_NEAR(x1(0), system.CalculateX(x0, u0, 1_s)(0), 0.1);
|
|
}
|