mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpimath] Add core State-space classes (#2614)
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
@@ -562,5 +562,74 @@
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "StateSpaceFlywheel",
|
||||
"description": "An example state-space controller for a flywheel.",
|
||||
"tags": [
|
||||
"StateSpaceFlywheel",
|
||||
"Flywheel",
|
||||
"State Space",
|
||||
"Model",
|
||||
"Digital",
|
||||
"Sensors",
|
||||
"Actuators",
|
||||
"Joystick"
|
||||
],
|
||||
"foldername": "statespaceflywheel",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "StateSpaceFlywheelSysId",
|
||||
"description": "An example state-space controller for controlling a flywheel with System Identification.",
|
||||
"tags": [
|
||||
"StateSpaceFlywheelSysId",
|
||||
"FRC Characterization",
|
||||
"Flywheel",
|
||||
"Characterization",
|
||||
"State space",
|
||||
"Digital",
|
||||
"Sensors",
|
||||
"Actuators",
|
||||
"Joystick"
|
||||
],
|
||||
"foldername": "statespaceflywheelsysid",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "StateSpaceElevator",
|
||||
"description": "An example state-space controller for controlling an elevator.",
|
||||
"tags": [
|
||||
"Elevator",
|
||||
"State Space",
|
||||
"Digital",
|
||||
"Sensors",
|
||||
"Actuators",
|
||||
"Joystick"
|
||||
],
|
||||
"foldername": "statespaceelevator",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
},
|
||||
{
|
||||
"name": "StateSpaceArm",
|
||||
"description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.",
|
||||
"tags": [
|
||||
"Arm",
|
||||
"State space",
|
||||
"Digital",
|
||||
"Sensors",
|
||||
"Actuators",
|
||||
"Joystick"
|
||||
],
|
||||
"foldername": "statespacearm",
|
||||
"gradlebase": "java",
|
||||
"mainclass": "Main",
|
||||
"commandversion": 2
|
||||
}
|
||||
]
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.statespacearm;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,153 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.statespacearm;
|
||||
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.PWMVictorSPX;
|
||||
import edu.wpi.first.wpilibj.SpeedController;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
|
||||
import edu.wpi.first.wpilibj.estimator.KalmanFilter;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystemLoop;
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
/**
|
||||
* This is a sample program to demonstrate how to use a state-space controller
|
||||
* to control an arm.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private static final int kMotorPort = 0;
|
||||
private static final int kEncoderAChannel = 0;
|
||||
private static final int kEncoderBChannel = 1;
|
||||
private static final int kJoystickPort = 0;
|
||||
private static final double kRaisedPosition = Units.degreesToRadians(90.0);
|
||||
private static final double kLoweredPosition = Units.degreesToRadians(0.0);
|
||||
|
||||
// Moment of inertia of the arm, in kg * m^2. Can be estimated with CAD. If finding this constant
|
||||
// is difficult, LinearSystem.identifyPositionSystem may be better.
|
||||
private static final double kArmMOI = 1.2;
|
||||
|
||||
// Reduction between motors and encoder, as output over input. If the arm spins slower than
|
||||
// the motors, this number should be greater than one.
|
||||
private static final double kArmGearing = 10.0;
|
||||
|
||||
private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(
|
||||
Units.degreesToRadians(45), Units.degreesToRadians(90)); // Max arm speed and acceleration.
|
||||
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
|
||||
|
||||
// The plant holds a state-space model of our arm. This system has the following properties:
|
||||
//
|
||||
// States: [position, velocity], in radians and radians per second.
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [position], in radians.
|
||||
private final LinearSystem<N2, N1, N1> m_armPlant =
|
||||
LinearSystemId.createSingleJointedArmSystem(
|
||||
DCMotor.getNEO(2),
|
||||
kArmMOI,
|
||||
kArmGearing);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
private final KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(
|
||||
Nat.N2(), Nat.N1(),
|
||||
m_armPlant,
|
||||
VecBuilder.fill(0.015, 0.17), // How accurate we
|
||||
// think our model is, in radians and radians/sec
|
||||
VecBuilder.fill(0.01), // How accurate we think our encoder position
|
||||
// data is. In this case we very highly trust our encoder position reading.
|
||||
0.020);
|
||||
|
||||
// A LQR uses feedback to create voltage commands.
|
||||
private final LinearQuadraticRegulator<N2, N1, N1> m_controller
|
||||
= new LinearQuadraticRegulator<>(m_armPlant,
|
||||
VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms.
|
||||
// Position and velocity error tolerances, in radians and radians per second. Decrease this
|
||||
// to more heavily penalize state excursion, or make the controller behave more
|
||||
// aggressively. In this example we weight position much more highly than velocity, but this
|
||||
// can be tuned to balance the two.
|
||||
1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this
|
||||
// will penalize state excursion more heavily, while decreasing this will penalize control
|
||||
// effort more heavily. Useful for balancing weights for systems with more states such
|
||||
// as drivetrains.
|
||||
VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
|
||||
// heavily penalize control effort, or make the controller less aggressive. 12 is a good
|
||||
// starting point because that is the (approximate) maximum voltage of a battery.
|
||||
0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
|
||||
// lower if using notifiers.
|
||||
|
||||
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
|
||||
private final LinearSystemLoop<N2, N1, N1> m_loop = new LinearSystemLoop<>(
|
||||
m_armPlant,
|
||||
m_controller,
|
||||
m_observer,
|
||||
12.0,
|
||||
0.020);
|
||||
|
||||
// An encoder set up to measure arm position in radians.
|
||||
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
|
||||
|
||||
private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
|
||||
|
||||
// A joystick to read the trigger from.
|
||||
private final Joystick m_joystick = new Joystick(kJoystickPort);
|
||||
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// We go 2 pi radians in 1 rotation, or 4096 counts.
|
||||
m_encoder.setDistancePerPulse(Math.PI * 2 / 4096.0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
// Reset our loop to make sure it's in a known state.
|
||||
m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
|
||||
|
||||
// Reset our last reference to the current state.
|
||||
m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(),
|
||||
m_encoder.getRate());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
// Sets the target position of our arm. This is similar to setting the setpoint of a
|
||||
// PID controller.
|
||||
TrapezoidProfile.State goal;
|
||||
if (m_joystick.getTrigger()) {
|
||||
// the trigger is pressed, so we go to the high goal.
|
||||
goal = new TrapezoidProfile.State(kRaisedPosition, 0.0);
|
||||
} else {
|
||||
// Otherwise, we go to the low goal
|
||||
goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
|
||||
}
|
||||
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
|
||||
m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference))
|
||||
.calculate(0.020);
|
||||
m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
|
||||
|
||||
// Correct our Kalman filter's state vector estimate with encoder data.
|
||||
m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
|
||||
|
||||
// Update our LQR to generate new voltage commands and use the voltages to predict the next
|
||||
// state with out Kalman filter.
|
||||
m_loop.predict(0.020);
|
||||
|
||||
// Send the new calculated voltage to the motors.
|
||||
// voltage = duty cycle * battery voltage, so
|
||||
// duty cycle = voltage / battery voltage
|
||||
double nextVoltage = m_loop.getU(0);
|
||||
m_motor.setVoltage(nextVoltage);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.statespaceelevator;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,155 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.statespaceelevator;
|
||||
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.PWMVictorSPX;
|
||||
import edu.wpi.first.wpilibj.SpeedController;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
|
||||
import edu.wpi.first.wpilibj.estimator.KalmanFilter;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystemLoop;
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N2;
|
||||
|
||||
/**
|
||||
* This is a sample program to demonstrate how to use a state-space controller
|
||||
* to control an elevator.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private static final int kMotorPort = 0;
|
||||
private static final int kEncoderAChannel = 0;
|
||||
private static final int kEncoderBChannel = 1;
|
||||
private static final int kJoystickPort = 0;
|
||||
private static final double kHighGoalPosition = Units.feetToMeters(3);
|
||||
private static final double kLowGoalPosition = Units.feetToMeters(0);
|
||||
|
||||
private static final double kCarriageMass = 4.5; // kilograms
|
||||
|
||||
// A 1.5in diameter drum has a radius of 0.75in, or 0.019in.
|
||||
private static final double kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0;
|
||||
|
||||
// Reduction between motors and encoder, as output over input. If the elevator spins slower than
|
||||
// the motors, this number should be greater than one.
|
||||
private static final double kElevatorGearing = 6.0;
|
||||
|
||||
private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(
|
||||
Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration.
|
||||
private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
|
||||
|
||||
// The plant holds a state-space model of our elevator. This system has the following properties:
|
||||
//
|
||||
// States: [position, velocity], in meters and meters per second.
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [position], in meters.
|
||||
private final LinearSystem<N2, N1, N1> m_elevatorPlant = LinearSystemId.createElevatorSystem(
|
||||
DCMotor.getNEO(2),
|
||||
kCarriageMass,
|
||||
kDrumRadius,
|
||||
kElevatorGearing
|
||||
);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
private final KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(
|
||||
Nat.N2(), Nat.N1(),
|
||||
m_elevatorPlant,
|
||||
VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we
|
||||
// think our model is, in meters and meters/second.
|
||||
VecBuilder.fill(0.001), // How accurate we think our encoder position
|
||||
// data is. In this case we very highly trust our encoder position reading.
|
||||
0.020);
|
||||
|
||||
// A LQR uses feedback to create voltage commands.
|
||||
private final LinearQuadraticRegulator<N2, N1, N1> m_controller
|
||||
= new LinearQuadraticRegulator<>(m_elevatorPlant,
|
||||
VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position
|
||||
// and velocity error tolerances, in meters and meters per second. Decrease this to more
|
||||
// heavily penalize state excursion, or make the controller behave more aggressively. In
|
||||
// this example we weight position much more highly than velocity, but this can be
|
||||
// tuned to balance the two.
|
||||
1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this
|
||||
// will penalize state excursion more heavily, while decreasing this will penalize control
|
||||
// effort more heavily. Useful for balancing weights for systems with more states such
|
||||
// as drivetrains.
|
||||
VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
|
||||
// heavily penalize control effort, or make the controller less aggressive. 12 is a good
|
||||
// starting point because that is the (approximate) maximum voltage of a battery.
|
||||
0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
|
||||
// lower if using notifiers.
|
||||
|
||||
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
|
||||
private final LinearSystemLoop<N2, N1, N1> m_loop = new LinearSystemLoop<>(
|
||||
m_elevatorPlant,
|
||||
m_controller,
|
||||
m_observer,
|
||||
12.0,
|
||||
0.020);
|
||||
|
||||
// An encoder set up to measure elevator height in meters.
|
||||
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
|
||||
|
||||
private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
|
||||
|
||||
// A joystick to read the trigger from.
|
||||
private final Joystick m_joystick = new Joystick(kJoystickPort);
|
||||
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// Circumference = pi * d, so distance per click = pi * d / counts
|
||||
m_encoder.setDistancePerPulse(Math.PI * 2 * kDrumRadius / 4096.0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
// Reset our loop to make sure it's in a known state.
|
||||
m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
|
||||
|
||||
// Reset our last reference to the current state.
|
||||
m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(),
|
||||
m_encoder.getRate());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
// Sets the target position of our arm. This is similar to setting the setpoint of a
|
||||
// PID controller.
|
||||
TrapezoidProfile.State goal;
|
||||
if (m_joystick.getTrigger()) {
|
||||
// the trigger is pressed, so we go to the high goal.
|
||||
goal = new TrapezoidProfile.State(kHighGoalPosition, 0.0);
|
||||
} else {
|
||||
// Otherwise, we go to the low goal
|
||||
goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
|
||||
}
|
||||
// Step our TrapezoidalProfile forward 20ms and set it as our next reference
|
||||
m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference))
|
||||
.calculate(0.020);
|
||||
m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
|
||||
|
||||
// Correct our Kalman filter's state vector estimate with encoder data.
|
||||
m_loop.correct(VecBuilder.fill(m_encoder.getDistance()));
|
||||
|
||||
// Update our LQR to generate new voltage commands and use the voltages to predict the next
|
||||
// state with out Kalman filter.
|
||||
m_loop.predict(0.020);
|
||||
|
||||
// Send the new calculated voltage to the motors.
|
||||
// voltage = duty cycle * battery voltage, so
|
||||
// duty cycle = voltage / battery voltage
|
||||
double nextVoltage = m_loop.getU(0);
|
||||
m_motor.setVoltage(nextVoltage);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.statespaceflywheel;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.statespaceflywheel;
|
||||
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.PWMVictorSPX;
|
||||
import edu.wpi.first.wpilibj.SpeedController;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
|
||||
import edu.wpi.first.wpilibj.estimator.KalmanFilter;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystemLoop;
|
||||
import edu.wpi.first.wpilibj.system.plant.DCMotor;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
|
||||
/**
|
||||
* This is a sample program to demonstrate how to use a state-space controller
|
||||
* to control a flywheel.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private static final int kMotorPort = 0;
|
||||
private static final int kEncoderAChannel = 0;
|
||||
private static final int kEncoderBChannel = 1;
|
||||
private static final int kJoystickPort = 0;
|
||||
private static final double kSpinupRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(500.0);
|
||||
|
||||
private static final double kFlywheelMomentOfInertia = 0.00032; // kg * m^2
|
||||
|
||||
// Reduction between motors and encoder, as output over input. If the flywheel spins slower than
|
||||
// the motors, this number should be greater than one.
|
||||
private static final double kFlywheelGearing = 1.0;
|
||||
|
||||
// The plant holds a state-space model of our flywheel. This system has the following properties:
|
||||
//
|
||||
// States: [velocity], in radians per second.
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [velocity], in radians per second.
|
||||
private final LinearSystem<N1, N1, N1> m_flywheelPlant = LinearSystemId.createFlywheelSystem(
|
||||
DCMotor.getNEO(2),
|
||||
kFlywheelMomentOfInertia,
|
||||
kFlywheelGearing);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
|
||||
Nat.N1(), Nat.N1(),
|
||||
m_flywheelPlant,
|
||||
VecBuilder.fill(3.0), // How accurate we think our model is
|
||||
VecBuilder.fill(0.01), // How accurate we think our encoder
|
||||
// data is
|
||||
0.020);
|
||||
|
||||
// A LQR uses feedback to create voltage commands.
|
||||
private final LinearQuadraticRegulator<N1, N1, N1> m_controller
|
||||
= new LinearQuadraticRegulator<>(m_flywheelPlant,
|
||||
VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease
|
||||
// this to more heavily penalize state excursion, or make the controller behave more
|
||||
// aggressively.
|
||||
1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this
|
||||
// will penalize state excursion more heavily, while decreasing this will penalize control
|
||||
// effort more heavily. Useful for balancing weights for systems with more states such
|
||||
// as drivetrains.
|
||||
VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
|
||||
// heavily penalize control effort, or make the controller less aggressive. 12 is a good
|
||||
// starting point because that is the (approximate) maximum voltage of a battery.
|
||||
0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
|
||||
// lower if using notifiers.
|
||||
|
||||
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
|
||||
private final LinearSystemLoop<N1, N1, N1> m_loop = new LinearSystemLoop<>(
|
||||
m_flywheelPlant,
|
||||
m_controller,
|
||||
m_observer,
|
||||
12.0,
|
||||
0.020);
|
||||
|
||||
// An encoder set up to measure flywheel velocity in radians per second.
|
||||
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
|
||||
|
||||
private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
|
||||
|
||||
// A joystick to read the trigger from.
|
||||
private final Joystick m_joystick = new Joystick(kJoystickPort);
|
||||
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// We go 2 pi radians per 4096 clicks.
|
||||
m_encoder.setDistancePerPulse(
|
||||
2.0 * Math.PI / 4096.0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
m_loop.reset(VecBuilder.fill(m_encoder.getRate()));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
// Sets the target speed of our flywheel. This is similar to setting the setpoint of a
|
||||
// PID controller.
|
||||
if (m_joystick.getTriggerPressed()) {
|
||||
// We just pressed the trigger, so let's set our next reference
|
||||
m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
|
||||
} else if (m_joystick.getTriggerReleased()) {
|
||||
// We just released the trigger, so let's spin down
|
||||
m_loop.setNextR(VecBuilder.fill(0.0));
|
||||
}
|
||||
|
||||
// Correct our Kalman filter's state vector estimate with encoder data.
|
||||
m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
|
||||
|
||||
// Update our LQR to generate new voltage commands and use the voltages to predict the next
|
||||
// state with out Kalman filter.
|
||||
m_loop.predict(0.020);
|
||||
|
||||
// Send the new calculated voltage to the motors.
|
||||
// voltage = duty cycle * battery voltage, so
|
||||
// duty cycle = voltage / battery voltage
|
||||
double nextVoltage = m_loop.getU(0);
|
||||
m_motor.setVoltage(nextVoltage);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.statespaceflywheelsysid;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
* Unless you know what you are doing, do not modify this file except to
|
||||
* change the parameter class to the startRobot call.
|
||||
*/
|
||||
public final class Main {
|
||||
private Main() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Main initialization function. Do not perform any initialization here.
|
||||
*
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.statespaceflywheelsysid;
|
||||
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.PWMVictorSPX;
|
||||
import edu.wpi.first.wpilibj.SpeedController;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
|
||||
import edu.wpi.first.wpilibj.estimator.KalmanFilter;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystem;
|
||||
import edu.wpi.first.wpilibj.system.LinearSystemLoop;
|
||||
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.wpiutil.math.Nat;
|
||||
import edu.wpi.first.wpiutil.math.VecBuilder;
|
||||
import edu.wpi.first.wpiutil.math.numbers.N1;
|
||||
|
||||
/**
|
||||
* This is a sample program to demonstrate how to use a state-space controller
|
||||
* to control a flywheel.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private static final int kMotorPort = 0;
|
||||
private static final int kEncoderAChannel = 0;
|
||||
private static final int kEncoderBChannel = 1;
|
||||
private static final int kJoystickPort = 0;
|
||||
private static final double kSpinupRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(500.0);
|
||||
|
||||
// Volts per (radian per second)
|
||||
private static final double kFlywheelKv = 0.023;
|
||||
|
||||
// Volts per (radian per second squared)
|
||||
private static final double kFlywheelKa = 0.001;
|
||||
|
||||
// The plant holds a state-space model of our flywheel. This system has the following properties:
|
||||
//
|
||||
// States: [velocity], in radians per second.
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [velocity], in radians per second.
|
||||
//
|
||||
// The Kv and Ka constants are found using the FRC Characterization toolsuite.
|
||||
private final LinearSystem<N1, N1, N1> m_flywheelPlant = LinearSystemId.identifyVelocitySystem(
|
||||
kFlywheelKv, kFlywheelKa);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
|
||||
Nat.N1(), Nat.N1(),
|
||||
m_flywheelPlant,
|
||||
VecBuilder.fill(3.0), // How accurate we think our model is
|
||||
VecBuilder.fill(0.01), // How accurate we think our encoder
|
||||
// data is
|
||||
0.020);
|
||||
|
||||
// A LQR uses feedback to create voltage commands.
|
||||
private final LinearQuadraticRegulator<N1, N1, N1> m_controller
|
||||
= new LinearQuadraticRegulator<>(m_flywheelPlant,
|
||||
VecBuilder.fill(8.0), // Velocity error tolerance
|
||||
VecBuilder.fill(12.0), // Control effort (voltage) tolerance
|
||||
0.020);
|
||||
|
||||
// The state-space loop combines a controller, observer, feedforward and plant for easy control.
|
||||
private final LinearSystemLoop<N1, N1, N1> m_loop = new LinearSystemLoop<>(
|
||||
m_flywheelPlant,
|
||||
m_controller,
|
||||
m_observer,
|
||||
12.0,
|
||||
0.020);
|
||||
|
||||
// An encoder set up to measure flywheel velocity in radians per second.
|
||||
private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
|
||||
|
||||
private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
|
||||
|
||||
// A joystick to read the trigger from.
|
||||
private final Joystick m_joystick = new Joystick(kJoystickPort);
|
||||
|
||||
@Override
|
||||
public void robotInit() {
|
||||
// We go 2 pi radians per 4096 clicks.
|
||||
m_encoder.setDistancePerPulse(
|
||||
2.0 * Math.PI / 4096.0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
// Reset our loop to make sure it's in a known state.
|
||||
m_loop.reset(VecBuilder.fill(m_encoder.getRate()));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
|
||||
// Sets the target speed of our flywheel. This is similar to setting the setpoint of a
|
||||
// PID controller.
|
||||
if (m_joystick.getTriggerPressed()) {
|
||||
// We just pressed the trigger, so let's set our next reference
|
||||
m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec));
|
||||
} else if (m_joystick.getTriggerReleased()) {
|
||||
// We just released the trigger, so let's spin down
|
||||
m_loop.setNextR(VecBuilder.fill(0.0));
|
||||
}
|
||||
|
||||
// Correct our Kalman filter's state vector estimate with encoder data.
|
||||
m_loop.correct(VecBuilder.fill(m_encoder.getRate()));
|
||||
|
||||
// Update our LQR to generate new voltage commands and use the voltages to predict the next
|
||||
// state with out Kalman filter.
|
||||
m_loop.predict(0.020);
|
||||
|
||||
// Send the new calculated voltage to the motors.
|
||||
// voltage = duty cycle * battery voltage, so
|
||||
// duty cycle = voltage / battery voltage
|
||||
double nextVoltage = m_loop.getU(0);
|
||||
m_motor.setVoltage(nextVoltage);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user