# How to do Trapezoidal acceleration

I want to add trapezoidal acceleration to the drive-based code. How do I do it with Java code?

Take a look at the documentation here:

Thank you.
Which is better, curve acceleration or trapezoidal acceleration?

With a trapezoidal profile you end up with infinite jerk at the transition points. So it is up to you if having infinite jerk matters for your mechanism.

5 Likes

Should I create my own class?

private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);

I can see that happening for a lighter mass such as the video illustration, but for a heaver mass an encoder analysis shows it will naturally curve and is about 120-200ms latent. I think trapezoidal is ideal because it’s simple and easier to shave off the latency by providing an empirical tuned additive offset set to both ramps on voltage applied.

1 Like

It’s called an ExampleSmartMotorController as a stand-in for what ever form of PID you have. If you’re using a TalonSRX with the position PID, you would use the TalonSRX class, if you’re using a Spark Max, use that, if you’re using the WPILib new PID controller, use that.

Both the TalonSRX and SparkMax also have trapezoidal controllers built in. You can read about them in their respective documentation (Motion Magic for Talon SRX and Smart Motion for Spark Max).

WPILib’s ProfiledPIDController has a trapezoidal motion profile built in as well.

Looks like that simulation neglected to add a proper feedforward. Second order current effects of FRC mechanisms have a really fast time constant due to the low motor inductance, and in practice it’s negligible. First order models can be controlled perfectly over a trapezoid profile using finite voltage inputs because voltage is analogous to force which is analogous to acceleration, and trapezoid profiles have finite accelerations. Therefore, there’s no benefit to s-curve profiles for FRC performance requirements.

I’ve verified these assumptions on my own team’s robot mechanisms, like our elevator, and gotten perfect tracking with them (feedforward does most of the work).

2 Likes

Indeed, this is an open loop / teleop drive example and very simple approach to reduce latency speed control (vs. a set point position control). Some sources suggest that feedforward is distinctly different from an open loop control. If feedforward equations can work with an open loop model please share. Thanks.

Feedforward is open-loop control by definition in that the controller block diagram has no feedback and thus no “closed loops”. https://github.com/wpilibsuite/frc-characterization can give you a feedforward model of the form u = K_s + K_v v + K_a a where u is required voltage to track a desired velocity and acceleration pair, K_s is voltage required due to friction, v is the desired velocity, a is the desired acceleration, and K_v and K_a are arbitrary constants converting from velocity or acceleration to voltage respectively.

It can be shown that this is equivalent to inverting the plant of an elevator with a voltage input and position and velocity states. Specifically, the following plant ignoring gravity (compensating for gravity is as simple as adding a constant voltage offset)

\dot{x} = \begin{bmatrix}0 & 1 \\ 0 & -\frac{K_v}{K_a}\end{bmatrix} x + \begin{bmatrix}0 \\ \frac{1}{K_a}\end{bmatrix}u

where x = \begin{bmatrix}position \\ velocity\end{bmatrix} and u = \begin{bmatrix}voltage\end{bmatrix}.

https://docs.wpilib.org/en/latest/docs/software/advanced-control/controllers/feedforward.html explains what feedforwards are and demonstrates the feedforward models WPILib 2020 provides.

2 Likes

I tried writing the code but it didn’t work.
PID processing is written in lines 215 to 215 of Robot.java.
The custom class for Drive is https://github.com/Teityan/PIDTest/blob/master/src/main/java/frc/robot/Drive.java
It becomes.
The value of the joystick has been obtained normally, and Spark has been connected normally, but the motor does not turn.
The explanation is small, sorry, could you give me any advice?

I put 2 Sparks in each SpeedController, but PID doesn’t work in Spark?

I want to use ProfiledPIDController with a JE PLG & Spark (via PWM), in the newcommand framework.

It’s not working. When I watch in Shuffleboard/LiveWindow, I can’t get the controller to accept a non-zero setpoint. I can’t tell if it’s immediately returning to zero or what.
When I try to set the Setpoint from LiveWindow, it immediately resets to zero.

Is there something about how I’m approaching this where I’m actually calling a bunch of goal(0) somewhere?

On this axis, I don’t need a feedforward, so I’ve zeroed out where appropriate. Or at least what I think is appropriate. Maybe I bungled something.

Subsystem

package frc.robot.subsystems;

import static edu.wpi.first.wpilibj.smartdashboard.SmartDashboard.putNumber;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.controller.*;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
import frc.robot.C;

public class X extends ProfiledPIDSubsystem {
//Set up PLG
private final Spark Xaxis = new Spark(C.Port.slidePWM); // slide,
private final Encoder m_encoder = new Encoder(C.Port.slideEncA, C.Port.slideEncB);
//no feedforward term

/**

• Creates a new X.
*/
public X() {
super(new ProfiledPIDController(C.slide.kP,0,0,new TrapezoidProfile.Constraints(C.slide.maxVel,C.slide.maxAccel)));
m_encoder.setDistancePerPulse(C.slide.DPPmm); //Unit is mm, DPP = distance per pulse
m_encoder.setReverseDirection(true);
setGoal(C.slide.minPosition); //set to 0 on intializing
}

@Override
public void periodic(){
super.periodic();
putNumber(“EncoderX”, m_encoder.getDistance());
}

@Override
public void useOutput(double output, TrapezoidProfile.State setpoint) {
Xaxis.setVoltage(output); //no FF term
}

@Override
public double getMeasurement() {
return m_encoder.getDistance(); //no constant setpoint adjustment required
}

public void jogUp() {
Xaxis.setVoltage(C.jogVolts);
}
public void jogDown() {
Xaxis.setVoltage(-1*C.jogVolts);
}

public void stop(){
Xaxis.setVoltage(0);
}

public double enc(){
return m_encoder.getDistance();
}
public void resetEnc(){
m_encoder.reset();
}

public void goalUp(){

setGoal(200.0);


}

}

Button binding

 final JoystickButton slideGoalUp = new JoystickButton(m_ctrl, C.OI.Start);
// slideGoalUp.whenPressed(() -> m_x.setGoal(200.0), m_x); //when press Start, move goal to 200
// SmartDashboard.putData(new InstantCommand(() -> m_x.setGoal(200.0), m_x));
slideGoalUp.whenPressed(new InstantCommand(m_x::goalUp, m_x));

final JoystickButton slideGoal100 = new JoystickButton(m_ctrl, C.OI.Back);
slideGoal100.whenPressed(new setGoal100(m_x));


}

I tried modifying Armbot, instead of modifying my program, and am getting the same problem.
I am technically on a VMX-Pi platform, but I don’t see why that would impact whether the lambda (doesn’t) work to pass a goal to the controller.

By going to Test Mode, I was able to set a new setpoint in livewindow, but when I returned to Teleop… nothing happened.

Armbot, modified to use my pinouts: