edf42001
commented
1 year ago Reports are that reducing slew rate reduced slamming from a 9 -> 6 on a scale to 10 which is a good start. But for tipped cone we don't want to slam the intake into the ground so we should try accel feedforward. Here's a tutorial I created with chatGPT for whoever wants to try it:
Here is our current useOutput function, which is a method of WPILib's ProfiledPIDSubsystem class.
@Override
protected void useOutput(double output, State setpoint) {
// Calculate the feedforward from the setpoint
double speed = feedForward * setpoint.velocity;
//accounts for gravity in speed
speed += gravityCompensation;
// Add PID output to speed to account for error in elevator
speed += output;
//use setSpeed instead of elevatorIO.setSpeed because need to go through limit switches
setSpeed(speed);
}We'd like to add an acceleration feedforward term here, but unfortunately, State setpoint doesn't come with an acceleration setpoint. Can you show how we could calculate acceleration from the velocity setpoint and do acceleration feedforward with that?
Sure, I'd be happy to help! To calculate acceleration from velocity, you can use the following equation:
acceleration = (velocity_setpoint - current_velocity) / timestep
Where velocity_setpoint is the desired velocity setpoint, current_velocity is the current velocity of the elevator, and timestep is the time elapsed between the current state and the previous state.
Here's an updated version of your useOutput method that incorporates acceleration feedforward:
private double prev_velocity = 0; // to store the previous velocity for acceleration calculation
@Override
protected void useOutput(double output, State setpoint) {
// Calculate the feedforward from the setpoint
double speed = feedForward * setpoint.velocity;
// Calculate acceleration feedforward using the velocity setpoint and current velocity
double acceleration_ff = accelerationFeedForwardGain * (setpoint.velocity - prev_velocity) / getPeriod();
prev_velocity = setpoint.velocity;
// Account for gravity in speed
speed += gravityCompensation;
// Add PID output to speed to account for error in elevator
speed += output;
// Add acceleration feedforward to speed
speed += acceleration_ff;
// Use setSpeed instead of elevatorIO.setSpeed because need to go through limit switches
setSpeed(speed);
}Here, we have added a new variable prev_velocity to store the previous velocity value. Then, we calculate the acceleration feedforward term by subtracting the current velocity from the velocity setpoint, dividing by the time elapsed since the last state, and multiplying by the acceleration feedforward gain. We then add this term to the speed variable, along with the other terms.
Note that getPeriod() is a method that returns the time elapsed since the last update, which is needed to convert the velocity difference into an acceleration.
I hope this helps! Let me know if you have any further questions.
I don't think WPILib has a getPeriod() method. Could you show how we can calculate the time since the last update using WPILib?
Sure! In WPILib, you can calculate the time since the last update using the Timer class. Here's an updated version of the code that calculates the time since the last update using Timer.getFPGATimestamp():
private double prev_time = 0; // to store the previous time for timestep calculation
@Override
protected void useOutput(double output, State setpoint) {
// Calculate the feedforward from the setpoint
double speed = feedForward * setpoint.velocity;
// Calculate acceleration feedforward using the velocity setpoint and current velocity
double time = Timer.getFPGATimestamp();
double dt = time - prev_time;
double acceleration_ff = accelerationFeedForwardGain * (setpoint.velocity - prev_velocity) / dt;
prev_velocity = setpoint.velocity;
prev_time = time;
// Account for gravity in speed
speed += gravityCompensation;
// Add PID output to speed to account for error in elevator
speed += output;
// Add acceleration feedforward to speed
speed += acceleration_ff;
// Use setSpeed instead of elevatorIO.setSpeed because need to go through limit switches
setSpeed(speed);
}In this updated version, we added a new variable prev_time to store the previous time value. Then, we calculate the time elapsed since the last update by subtracting the previous time from the current time obtained by calling Timer.getFPGATimestamp(). We use this value as the timestep in the acceleration calculation.
I hope this helps! Let me know if you have any further questions.
Do you know how we might go about finding a good value for accelerationFeedForwardGain?
Finding a good value for the accelerationFeedForwardGain can be done through experimentation and tuning.
One method is to use the "zigzag" test, where the elevator moves up and down repeatedly between two fixed positions. During this test, you can observe the elevator's response and adjust the accelerationFeedForwardGain until the elevator moves smoothly without overshooting or oscillating.
Another method is to use the "step" test, where the elevator is commanded to move to a specific position and then stop. You can observe the elevator's response and adjust the accelerationFeedForwardGain until the elevator reaches the target position quickly and without overshooting.
It's important to note that the accelerationFeedForwardGain value depends on several factors, including the elevator's mass, motor characteristics, and the desired performance. So, the gain value may need to be adjusted if any of these factors change.
Overall, tuning the accelerationFeedForwardGain requires experimentation and observation of the elevator's response to find the optimal value for your specific elevator system.
The ideas are short, but explaining my reasoning is long (which is why this issue is long). First, here are the ideas in order of "least major change" to "slightly larger change", and therefore the order I think we should try them in:
Overview
Now, the explanation. First, what did we learn from the graph? It turns out, I was completely wrong. We didn't take a picture (though we do have logs), but here's my recreation of the graph, where blue is setpoint and red is actual:
So the real velocity was lagging behind the setpoint velocity, not in front of it like my prediction that the feedforward gain being to high would have caused.
Slew rate
Now, why might the slew rate limit cause problems in this case? FIrst, the lagging. It could be caused by friction, motor dynamics, a poorly tuned feedforward, or lack of acceleration feedforward, but the point is: the elevator mechanism is sluggish, at a given time, the elevator isn't going as fast as the the setpoint says it should. This means, it's position will start to fall behind. Now the P gain would kick in, to add extra power, increasing its speed and reducing the position error.
However, if the slew rate limit is low, it might not be able to add that extra power as fast as it wants. I had Anna L add the slew rate limiter a bit ago when we observed the arm and elevator would jerk to a start when you controlled them manually, as we immediatley jump the applied power to 0.2. We calculated a slew rate that would be well above what the elevator would ever need in motion profiling. Specifically, our max speed was around 0.25, and max accel was 0.5. So it would take 0.5 seconds to accelerate to "full speed". However, full speed was 0.25 m/s * feedforoward (0.8), or 0.2 percent power. So, that would be 0.5 seconds from 0 to 0.2, or 2.5 seconds for o to 100%. Those are the units spark max feed forward takes "time to full power". So, to not avoid stepping on motion profiling's toes, we set a slew rate limit of 0.75 seconds from 0 to full, well under the 2.5 it does in motion profiling.
But then we increased the elevator top speed to 1.25 m/s, and acceleration to 2.5. Still 0.5 seconds to "full", but now "full" actually means 100% (1.25 * 0.8). 0.5 is < 0.75, so anticipating this problem, I suggested reducing it to 0.4. But maybe that wasn't low enough, as again, the lag in the velocity means that kP comes into effect and increases the velocity more.
I suggest decreasing 0.4 -> 0.1 (units are time to full, so this is actually a 4x increase in speed 1/0.4 = 2.5, 1/0.1 = 10)
kP
Increasing kP from 3 might allow the elevator to track better, meaning it "may" not overshoot as much (and might have a better time getting all the way to the top setpoint)
Acceleration feedforward
This would fix the velocity lagging behind problem. It applies extra power when accelerating, and extra, negative power when deaccaelerating. This will help the elevator overcome inertia and slow down, not overshooting. See chat-gpt based discord conversation for details https://discord.com/channels/410639121711759371/410643657163538432/1085396306576556112 https://discord.com/channels/410639121711759371/410643657163538432/1085403129136283768