001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math.controller; 006 007import edu.wpi.first.math.controller.proto.ArmFeedforwardProto; 008import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct; 009 010/** 011 * A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting 012 * against the force of gravity on a beam suspended at an angle). 013 */ 014public class ArmFeedforward { 015 public final double ks; 016 public final double kg; 017 public final double kv; 018 public final double ka; 019 020 public static final ArmFeedforwardProto proto = new ArmFeedforwardProto(); 021 public static final ArmFeedforwardStruct struct = new ArmFeedforwardStruct(); 022 023 /** 024 * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate 025 * units of the computed feedforward. 026 * 027 * @param ks The static gain. 028 * @param kg The gravity gain. 029 * @param kv The velocity gain. 030 * @param ka The acceleration gain. 031 */ 032 public ArmFeedforward(double ks, double kg, double kv, double ka) { 033 this.ks = ks; 034 this.kg = kg; 035 this.kv = kv; 036 this.ka = ka; 037 } 038 039 /** 040 * Creates a new ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero. 041 * Units of the gain values will dictate units of the computed feedforward. 042 * 043 * @param ks The static gain. 044 * @param kg The gravity gain. 045 * @param kv The velocity gain. 046 */ 047 public ArmFeedforward(double ks, double kg, double kv) { 048 this(ks, kg, kv, 0); 049 } 050 051 /** 052 * Calculates the feedforward from the gains and setpoints. 053 * 054 * @param positionRadians The position (angle) setpoint. This angle should be measured from the 055 * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If 056 * your encoder does not follow this convention, an offset should be added. 057 * @param velocityRadPerSec The velocity setpoint. 058 * @param accelRadPerSecSquared The acceleration setpoint. 059 * @return The computed feedforward. 060 */ 061 public double calculate( 062 double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) { 063 return ks * Math.signum(velocityRadPerSec) 064 + kg * Math.cos(positionRadians) 065 + kv * velocityRadPerSec 066 + ka * accelRadPerSecSquared; 067 } 068 069 /** 070 * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be 071 * zero). 072 * 073 * @param positionRadians The position (angle) setpoint. This angle should be measured from the 074 * horizontal (i.e. if the provided angle is 0, the arm should be parallel with the floor). If 075 * your encoder does not follow this convention, an offset should be added. 076 * @param velocity The velocity setpoint. 077 * @return The computed feedforward. 078 */ 079 public double calculate(double positionRadians, double velocity) { 080 return calculate(positionRadians, velocity, 0); 081 } 082 083 // Rearranging the main equation from the calculate() method yields the 084 // formulas for the methods below: 085 086 /** 087 * Calculates the maximum achievable velocity given a maximum voltage supply, a position, and an 088 * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal 089 * profile are simultaneously achievable - enter the acceleration constraint, and this will give 090 * you a simultaneously-achievable velocity constraint. 091 * 092 * @param maxVoltage The maximum voltage that can be supplied to the arm. 093 * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if 094 * the provided angle is 0, the arm should be parallel with the floor). If your encoder does 095 * not follow this convention, an offset should be added. 096 * @param acceleration The acceleration of the arm. 097 * @return The maximum possible velocity at the given acceleration and angle. 098 */ 099 public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) { 100 // Assume max velocity is positive 101 return (maxVoltage - ks - Math.cos(angle) * kg - acceleration * ka) / kv; 102 } 103 104 /** 105 * Calculates the minimum achievable velocity given a maximum voltage supply, a position, and an 106 * acceleration. Useful for ensuring that velocity and acceleration constraints for a trapezoidal 107 * profile are simultaneously achievable - enter the acceleration constraint, and this will give 108 * you a simultaneously-achievable velocity constraint. 109 * 110 * @param maxVoltage The maximum voltage that can be supplied to the arm. 111 * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if 112 * the provided angle is 0, the arm should be parallel with the floor). If your encoder does 113 * not follow this convention, an offset should be added. 114 * @param acceleration The acceleration of the arm. 115 * @return The minimum possible velocity at the given acceleration and angle. 116 */ 117 public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) { 118 // Assume min velocity is negative, ks flips sign 119 return (-maxVoltage + ks - Math.cos(angle) * kg - acceleration * ka) / kv; 120 } 121 122 /** 123 * Calculates the maximum achievable acceleration given a maximum voltage supply, a position, and 124 * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal 125 * profile are simultaneously achievable - enter the velocity constraint, and this will give you a 126 * simultaneously-achievable acceleration constraint. 127 * 128 * @param maxVoltage The maximum voltage that can be supplied to the arm. 129 * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if 130 * the provided angle is 0, the arm should be parallel with the floor). If your encoder does 131 * not follow this convention, an offset should be added. 132 * @param velocity The velocity of the arm. 133 * @return The maximum possible acceleration at the given velocity. 134 */ 135 public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) { 136 return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kg - velocity * kv) / ka; 137 } 138 139 /** 140 * Calculates the minimum achievable acceleration given a maximum voltage supply, a position, and 141 * a velocity. Useful for ensuring that velocity and acceleration constraints for a trapezoidal 142 * profile are simultaneously achievable - enter the velocity constraint, and this will give you a 143 * simultaneously-achievable acceleration constraint. 144 * 145 * @param maxVoltage The maximum voltage that can be supplied to the arm. 146 * @param angle The angle of the arm. This angle should be measured from the horizontal (i.e. if 147 * the provided angle is 0, the arm should be parallel with the floor). If your encoder does 148 * not follow this convention, an offset should be added. 149 * @param velocity The velocity of the arm. 150 * @return The minimum possible acceleration at the given velocity. 151 */ 152 public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) { 153 return maxAchievableAcceleration(-maxVoltage, angle, velocity); 154 } 155}