Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +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

139
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h Normal file
View File

@@ -0,0 +1,139 @@
/*----------------------------------------------------------------------------*/
/* 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 <array>
#include <units/angle.h>
#include <units/length.h>
#include <units/mass.h>
#include <units/moment_of_inertia.h>
#include "frc/simulation/LinearSystemSim.h"
#include "frc/system/plant/DCMotor.h"
namespace frc::sim {
/**
* Represents a simulated arm mechanism.
*/
class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
public:
/**
* Creates a simulated arm mechanism.
*
* @param system The system representing this arm.
* @param motor The type and number of motors on the arm gearbox.
* @param G The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param mass The mass of the arm.
* @param armLength The length of the arm.
* @param minAngle The minimum allowed angle for the arm.
* @param maxAngle The maximum allowed angle for the arm.
* @param addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, const DCMotor motor,
double G, units::kilogram_t mass,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool addNoise,
const std::array<double, 1>& measurementStdDevs);
/**
* Creates a simulated arm mechanism.
*
* @param motor The type and number of motors on the arm gearbox.
* @param j The moment of inertia of the arm.
* @param G The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param mass The mass of the arm.
* @param armLength The length of the arm.
* @param minAngle The minimum allowed angle for the arm.
* @param maxAngle The maximum allowed angle for the arm.
* @param addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
SingleJointedArmSim(const DCMotor& motor, units::kilogram_square_meter_t J,
double G, units::kilogram_t mass,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool addNoise,
const std::array<double, 1>& measurementStdDevs);
/**
* Creates a simulated arm mechanism.
*
* @param motor The type and number of motors on the arm gearbox.
* @param G The gear ratio of the arm (numbers greater than 1
* represent reductions).
* @param mass The mass of the arm.
* @param armLength The length of the arm.
* @param minAngle The minimum allowed angle for the arm. This is
* measured from horizontal, with straight out being 0.
* @param maxAngle The maximum allowed angle for the arm. This is
* measured from horizontal, with straight out being 0.
* @param addNoise Whether the sim should automatically add some
* encoder noise.
* @param measurementStdDevs The standard deviation of the measurement noise.
*/
SingleJointedArmSim(const DCMotor& motor, double G, units::kilogram_t mass,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool addNoise,
const std::array<double, 1>& measurementStdDevs);
/**
* Returns whether the arm has hit the lower limit.
*
* @param x The current arm state.
* @return Whether the arm has hit the lower limit.
*/
bool HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const;
/**
* Returns whether the arm has hit the upper limit.
*
* @param x The current arm state.
* @return Whether the arm has hit the upper limit.
*/
bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
/**
* Returns the current arm angle.
*
* @return The current arm angle.
*/
units::radian_t GetAngle() const;
/**
* Returns the current arm velocity.
*
* @return The current arm velocity.
*/
units::radians_per_second_t GetVelocity() const;
/**
* Updates the state estimate of the arm.
*
* @param currentXhat The current state estimate.
* @param u The system inputs (voltage).
* @param dt The time difference between controller updates.
*/
Eigen::Matrix<double, 2, 1> UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
units::ampere_t GetCurrentDraw() const override;
private:
units::meter_t m_r;
units::radian_t m_minAngle;
units::radian_t m_maxAngle;
units::kilogram_t m_mass;
const DCMotor m_motor;
double m_gearing;
};
} // namespace frc::sim