Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add core State-space classes (#2614)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
2020-08-14 23:40:33 -07:00
committed by GitHub
parent e5b84e2f87
commit 3b283ab9aa
84 changed files with 11747 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

157
wpimath/src/main/native/include/frc/system/plant/DCMotor.h Normal file
View File

@@ -0,0 +1,157 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "units/angular_velocity.h"
#include "units/current.h"
#include "units/impedance.h"
#include "units/torque.h"
#include "units/voltage.h"
namespace frc {
/**
* Holds the constants for a DC motor.
*/
class DCMotor {
public:
using radians_per_second_per_volt_t =
units::unit_t<units::compound_unit<units::radians_per_second,
units::inverse<units::volt>>>;
using newton_meters_per_ampere_t =
units::unit_t<units::compound_unit<units::newton_meters,
units::inverse<units::ampere>>>;
units::volt_t nominalVoltage;
units::newton_meter_t stallTorque;
units::ampere_t stallCurrent;
units::ampere_t freeCurrent;
units::radians_per_second_t freeSpeed;
// Resistance of motor
units::ohm_t R;
// Motor velocity constant
radians_per_second_per_volt_t Kv;
// Torque constant
newton_meters_per_ampere_t Kt;
/**
* Constructs a DC motor.
*
* @param nominalVoltage Voltage at which the motor constants were measured.
* @param stallTorque Current draw when stalled.
* @param stallCurrent Current draw when stalled.
* @param freeCurrent Current draw under no load.
* @param freeSpeed Angular velocity under no load.
* @param numMotors Number of motors in a gearbox.
*/
constexpr DCMotor(units::volt_t nominalVoltage,
units::newton_meter_t stallTorque,
units::ampere_t stallCurrent, units::ampere_t freeCurrent,
units::radians_per_second_t freeSpeed, int numMotors = 1)
: nominalVoltage(nominalVoltage),
stallTorque(stallTorque * numMotors),
stallCurrent(stallCurrent),
freeCurrent(freeCurrent),
freeSpeed(freeSpeed),
R(nominalVoltage / stallCurrent),
Kv(freeSpeed / (nominalVoltage - R * freeCurrent)),
Kt(stallTorque * numMotors / stallCurrent) {}
/**
* Returns current drawn by motor with given speed and input voltage.
*
* @param speed The current angular velocity of the motor.
* @param inputVoltage The voltage being applied to the motor.
*/
constexpr units::ampere_t Current(units::radians_per_second_t speed,
units::volt_t inputVoltage) const {
return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage;
}
/**
* Returns instance of CIM.
*/
static constexpr DCMotor CIM(int numMotors = 1) {
return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors);
}
/**
* Returns instance of MiniCIM.
*/
static constexpr DCMotor MiniCIM(int numMotors = 1) {
return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors);
}
/**
* Returns instance of Bag motor.
*/
static constexpr DCMotor Bag(int numMotors = 1) {
return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors);
}
/**
* Returns instance of Vex 775 Pro.
*/
static constexpr DCMotor Vex775Pro(int numMotors = 1) {
return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors);
}
/**
* Returns instance of Andymark RS 775-125.
*/
static constexpr DCMotor RS775_125(int numMotors = 1) {
return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors);
}
/**
* Returns instance of Banebots RS 775.
*/
static constexpr DCMotor BanebotsRS775(int numMotors = 1) {
return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors);
}
/**
* Returns instance of Andymark 9015.
*/
static constexpr DCMotor Andymark9015(int numMotors = 1) {
return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors);
}
/**
* Returns instance of Banebots RS 550.
*/
static constexpr DCMotor BanebotsRS550(int numMotors = 1) {
return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors);
}
/**
* Returns instance of NEO brushless motor.
*/
static constexpr DCMotor NEO(int numMotors = 1) {
return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors);
}
/**
* Returns instance of NEO 550 brushless motor.
*/
static constexpr DCMotor NEO550(int numMotors = 1) {
return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors);
}
/**
* Returns instance of Falcon 500 brushless motor.
*/
static constexpr DCMotor Falcon500(int numMotors = 1) {
return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
}
};
} // namespace frc

213
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h Normal file
View File

@@ -0,0 +1,213 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "frc/StateSpaceUtil.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/DCMotor.h"
#include "units/moment_of_inertia.h"
namespace frc {
class LinearSystemId {
public:
/**
* Constructs the state-space model for an elevator.
*
* States: [[position], [velocity]]
* Inputs: [[voltage]]
* Outputs: [[position]]
*
* @param motor Instance of DCMotor.
* @param m Carriage mass.
* @param r Pulley radius.
* @param G Gear ratio from motor to carriage.
*/
static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
units::kilogram_t m,
units::meter_t r, double G) {
auto A = frc::MakeMatrix<2, 2>(
0.0, 1.0, 0.0,
(-std::pow(G, 2) * motor.Kt /
(motor.R * units::math::pow<2>(r) * m * motor.Kv))
.to<double>());
auto B = frc::MakeMatrix<2, 1>(
0.0, (G * motor.Kt / (motor.R * r * m)).to<double>());
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<2, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a single-jointed arm.
*
* States: [[angle], [angular velocity]]
* Inputs: [[voltage]]
* Outputs: [[angle]]
*
* @param motor Instance of DCMotor.
* @param J Moment of inertia.
* @param G Gear ratio from motor to carriage.
*/
static LinearSystem<2, 1, 1> SingleJointedArmSystem(
DCMotor motor, units::kilogram_square_meter_t J, double G) {
auto A = frc::MakeMatrix<2, 2>(
0.0, 1.0, 0.0,
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
auto B =
frc::MakeMatrix<2, 1>(0.0, (G * motor.Kt / (motor.R * J)).to<double>());
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<2, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a 1 DOF velocity-only system from
* system identification data.
*
* States: [[velocity]]
* Inputs: [[voltage]]
* Outputs: [[velocity]]
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds^2 per distance.
*/
static LinearSystem<1, 1, 1> IdentifyVelocitySystem(double kV, double kA) {
auto A = frc::MakeMatrix<1, 1>(-kV / kA);
auto B = frc::MakeMatrix<1, 1>(1.0 / kA);
auto C = frc::MakeMatrix<1, 1>(1.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<1, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a 1 DOF position system from system
* identification data.
*
* States: [[position], [velocity]]
* Inputs: [[voltage]]
* Outputs: [[position]]
*
* The parameters provided by the user are from this feedforward model:
*
* u = K_v v + K_a a
*
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds^2 per distance.
*/
static LinearSystem<2, 1, 1> IdentifyPositionSystem(double kV, double kA) {
auto A = frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kV / kA);
auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA);
auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<2, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a 2 DOF drivetrain velocity system
* from system identification data.
*
* States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]]
*
* @param kVlinear The linear velocity gain, in volt seconds per distance.
* @param kAlinear The linear acceleration gain, in volt seconds^2 per
* distance.
* @param kVangular The angular velocity gain, in volt seconds per angle.
* @param kAangular The angular acceleration gain, in volt seconds^2 per
* angle.
*/
static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(double kVlinear,
double kAlinear,
double kVangular,
double kAangular) {
double c = 0.5 / (kAlinear * kAangular);
double A1 = c * (-kAlinear * kVangular - kVlinear * kAangular);
double A2 = c * (kAlinear * kVangular - kVlinear * kAangular);
double B1 = c * (kAlinear + kAangular);
double B2 = c * (kAangular - kAlinear);
auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
return LinearSystem<2, 2, 2>(A, B, C, D);
}
/**
* Constructs the state-space model for a flywheel.
*
* States: [[angular velocity]]
* Inputs: [[voltage]]
* Outputs: [[angular velocity]]
*
* @param motor Instance of DCMotor.
* @param J Moment of inertia.
* @param G Gear ratio from motor to carriage.
*/
static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
units::kilogram_square_meter_t J,
double G) {
auto A = frc::MakeMatrix<1, 1>(
(-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
auto B = frc::MakeMatrix<1, 1>((G * motor.Kt / (motor.R * J)).to<double>());
auto C = frc::MakeMatrix<1, 1>(1.0);
auto D = frc::MakeMatrix<1, 1>(0.0);
return LinearSystem<1, 1, 1>(A, B, C, D);
}
/**
* Constructs the state-space model for a drivetrain.
*
* States: [[left velocity], [right velocity]]
* Inputs: [[left voltage], [right voltage]]
* Outputs: [[left velocity], [right velocity]]
*
* @param motor Instance of DCMotor.
* @param m Drivetrain mass.
* @param r Wheel radius.
* @param rb Robot radius.
* @param G Gear ratio from motor to wheel.
* @param J Moment of inertia.
*/
static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
DCMotor motor, units::kilogram_t m, units::meter_t r, units::meter_t rb,
units::kilogram_square_meter_t J, double G) {
auto C1 = -std::pow(G, 2) * motor.Kt /
(motor.Kv * motor.R * units::math::pow<2>(r));
auto C2 = G * motor.Kt / (motor.R * r);
auto A = frc::MakeMatrix<2, 2>(
((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>());
auto B = frc::MakeMatrix<2, 2>(
((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>());
auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
return LinearSystem<2, 2, 2>(A, B, C, D);
}
};
} // namespace frc