Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add physics simulation support with state-space (#2615)

Browse Source 
This includes physics simulation support for arms/elevator models, as well as differential drivetrains.

Swerve might be added at a later date.

Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2020-09-20 09:39:52 -07:00
committed by GitHub
parent 0503225928
commit b61f08d3fa
43 changed files with 3787 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp Normal file
View File

@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <iostream>
#include <units/time.h>
#include "frc/Encoder.h"
#include "frc/PWMVictorSPX.h"
#include "frc/RobotController.h"
#include "frc/StateSpaceUtil.h"
#include "frc/controller/PIDController.h"
#include "frc/simulation/ElevatorSim.h"
#include "frc/simulation/EncoderSim.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "gtest/gtest.h"
TEST(ElevatorSim, StateSpaceSim) {
frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 8_kg, 13.67,
units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 3_m,
true, {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());
auto u = frc::MakeMatrix<1, 1>(motor.Get() *
frc::RobotController::GetInputVoltage());
sim.SetInput(u);
sim.Update(20_ms);
const auto& y = sim.Y();
encoderSim.SetDistance(y(0));
}
EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().to<double>(), 0.2);
}
TEST(ElevatorSim, MinMax) {
frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 8_kg, 13.67,
units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m,
true, {0.01});
for (size_t i = 0; i < 100; ++i) {
sim.SetInput(frc::MakeMatrix<1, 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(frc::MakeMatrix<1, 1>(12.0));
sim.Update(20_ms);
auto height = sim.GetPosition();
EXPECT_TRUE(height < 1.05_m);
}
}

25
wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <wpi/math>
#include "frc/simulation/SingleJointedArmSim.h"
#include "gtest/gtest.h"
TEST(SingleJointedArmTest, Disabled) {
frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 10_kg,
9.5_in, -180_deg, 0_deg, false, {0.0});
sim.ResetState(frc::MakeMatrix<2, 1>(0.0, 0.0));
for (size_t i = 0; i < 12 / 0.02; ++i) {
sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
sim.Update(20_ms);
}
// The arm should swing down.
EXPECT_NEAR(sim.GetAngle().to<double>(), -wpi::math::pi / 2, 0.01);
}

57
wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp Normal file
View File

@@ -0,0 +1,57 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include <iostream>
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>
#include "frc/Encoder.h"
#include "frc/PWMVictorSPX.h"
#include "frc/RobotController.h"
#include "frc/controller/PIDController.h"
#include "frc/controller/SimpleMotorFeedforward.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/simulation/SingleJointedArmSim.h"
#include "frc/system/plant/LinearSystemId.h"
#include "gtest/gtest.h"
TEST(StateSpaceSimTest, TestFlywheelSim) {
const frc::LinearSystem<1, 1, 1> plant =
frc::LinearSystemId::IdentifyVelocitySystem(0.02, 0.01);
frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2), 1.0};
frc2::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};
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()}).to<double>());
sim.SetInput(frc::MakeMatrix<1, 1>(
motor.Get() * frc::RobotController::GetInputVoltage()));
sim.Update(20_ms);
encoderSim.SetRate(sim.GetAngularVelocity().to<double>());
}
ASSERT_TRUE(std::abs(200 - encoder.GetRate()) < 0.1);
}