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

Add feedforward components (#2045)

Browse Source 
Add helper classes for computing feedforwards with parameters supplied by the characterization tool.
This commit is contained in:
Oblarg
2019-11-09 23:16:42 -05:00
committed by Peter Johnson
parent 5f33d6af12
commit 7dc7c71b58
13 changed files with 457 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java Normal file
View File

@@ -0,0 +1,72 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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.controller;
/**
* A helper class that computes feedforward outputs for a simple arm (modeled as a motor
* acting against the force of gravity on a beam suspended at an angle).
*/
public class ArmFeedforward {
private final double m_ks;
private final double m_kcos;
private final double m_kv;
private final double m_ka;
/**
* Creates a new ArmFeedforward with the specified gains. Units of the gain values
* will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kcos The gravity gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
*/
public ArmFeedforward(double ks, double kcos, double kv, double ka) {
m_ks = ks;
m_kcos = kcos;
m_kv = kv;
m_ka = ka;
}
/**
* Creates a new ArmFeedforward with the specified gains. Acceleration gain is
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kcos The gravity gain.
* @param kv The velocity gain.
*/
public ArmFeedforward(double ks, double kcos, double kv) {
this(ks, kcos, kv, 0);
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param velocityRadPerSec The velocity setpoint.
* @param accelRadPerSecSquared The acceleration setpoint.
* @return The computed feedforward.
*/
public double calculate(double positionRadians, double velocityRadPerSec,
double accelRadPerSecSquared) {
return m_ks * Math.signum(velocityRadPerSec) + m_kcos * Math.cos(positionRadians)
+ m_kv * velocityRadPerSec
+ m_ka * accelRadPerSecSquared;
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
* be zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
public double calculate(double positionRadians, double velocity) {
return calculate(positionRadians, velocity, 0);
}
}

69
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java Normal file
View File

@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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.controller;
/**
* A helper class that computes feedforward outputs for a simple elevator (modeled as a motor
* acting against the force of gravity).
*/
public class ElevatorFeedforward {
private final double m_ks;
private final double m_kg;
private final double m_kv;
private final double m_ka;
/**
* Creates a new ElevatorFeedforward with the specified gains. Units of the gain values
* will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kg The gravity gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
*/
public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
m_ks = ks;
m_kg = kg;
m_kv = kv;
m_ka = ka;
}
/**
* Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kg The gravity gain.
* @param kv The velocity gain.
*/
public ElevatorFeedforward(double ks, double kg, double kv) {
this(ks, kg, kv, 0);
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward.
*/
public double calculate(double velocity, double acceleration) {
return m_ks * Math.signum(velocity) + m_kg + m_kv * velocity + m_ka * acceleration;
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
* be zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
public double calculate(double velocity) {
return calculate(velocity, 0);
}
}

64
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java Normal file
View File

@@ -0,0 +1,64 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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.controller;
/**
* A helper class that computes feedforward outputs for a simple permanent-magnet DC motor.
*/
public class SimpleMotorFeedforward {
private final double m_ks;
private final double m_kv;
private final double m_ka;
/**
* Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values
* will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
*/
public SimpleMotorFeedforward(double ks, double kv, double ka) {
m_ks = ks;
m_kv = kv;
m_ka = ka;
}
/**
* Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is
* defaulted to zero. Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
*/
public SimpleMotorFeedforward(double ks, double kv) {
this(ks, kv, 0);
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward.
*/
public double calculate(double velocity, double acceleration) {
return m_ks * Math.signum(velocity) + m_kv * velocity + m_ka * acceleration;
}
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
* be zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
*/
public double calculate(double velocity) {
return calculate(velocity, 0);
}
}