Lesson 3.0 - Angelbots1339/ProgramingLessons GitHub Wiki

In this lesson you will learn how to control more advanced mechanisms, such as elevators and arms. The code will be written in various files, all found under src\main\java\frc\robot\.

Open Loop/Feedforward Control means providing the mechanism with the control signal you think it needs to make the mechanism do what you want, without any knowledge of where the mechanism currently is. The feedforward controller does not adjust this in response to the measured behavior of the system to try to correct for errors from the guess.

Closed Loop Control means you change the control signal sent to the mechanism depending on sensor feedback. It is ideal if any outside forces interfere, or if your model of the system is not perfect or nonexistent. This usually involves Motors and Encoders.

!

Open Loop in its most basic form is just feeding a motor an arbitrary voltage. For example, spinning a motor based on a joystick input, or running an intake motor at a constant arbitrary voltage.

But Open Loop control isn't always that simple. For more advanced control, you can create a mathematical model of the mechanism. This model simulates how the mechanism should behave, and will calculate the control signal you need to get the desired output.

WPI has many of these models built-in, so you don't have to write out all the math, but you still need to give it some basic mechanism-specific values:

• Ks: The voltage needed to make the mechanism move at all, and overcome static friction. If you feed it any value smaller than this one, it won't even move.
• Kv: The voltage needed to hold a constant velocity. If there's a lot of friction in the system, for example, this might be higher.
• Ka: The voltage needed to accelerate a given amount.
• Kg: The voltage needed to counteract gravity. Useful on elevators and arms, you can use this value to hold the mechanism steady in place as well as making the rest of the Feedforward behave better.

Keep in mind that these values are dependent on units. The explanations above all use volts as the unit, which is pretty common, but it's important that everything is using the same units.

If you notice, all the K values correspond to velocity. Feedforward control doesn't work well for position. In order to use it, you would have to calculate the desired velocity from the difference between your position setpoints. Basically, you can't plug in position, and have to turn it into a velocity first before plugging in. This still doesn't work the best though, and in order to do better, you can use a Motion Profile, which you will learn about later.

Create and use a SimpleMotorFeedforward with the Flywheel values in Constants.java, and then tune Ks.

To use the values from Constants.java, it should already be imported, so you can just call Constants.value to get what you need. Ex: Constants.kFlywheelkS

You will be controlling a simulated flywheel. To view the simulation, follow the steps on How to run simulation, and then once the Glass GUI is open, inside the NetworkTables window look under Transitory Tables > Shuffleboard > Flywheel > Flywheel Velocity.

In this case, Kv represents the volts needed per RPM $\frac{v}{rpm}$.

Your feedforward inside of reachVelocity() must work before tuning.

To start tuning Kv:

• Make sure isTestMode() inside of Flywheel.java returns true.
• Run the simulation, and look under Transitory Tables > Shuffleboard > Flywheel > Flywheel Volts. You can click on the box to change the voltage given to the flywheel sim.
• Change the voltage, and then look at how the flywheel velocity responds.
• Note down a voltage and the corresponding RPM. Divide the Voltage by the RPM to get an estimate for what Kv should be.
• Then, take it out of test mode, and put your Kv inside Constants.java.
• Just change this Kv value and restart the sim until it gets close enough to the setpoint of 2500 RPM.

How to run tests Expected result

!

private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.kFlywheelkS, Constants.kFlywheelkV, Constants.kFlywheelkA);

/**
 * @param goal for the flywheel in RPM
 */
@Override
public void reachVelocity(double goal) {

  setVolts(feedforward.calculate(goal));
}

As mentioned earlier, plain Feedforward doesn't work very well for position. On top of that, having no knowledge of the mechanism's current state isn't always ideal, especially for FRC robots with many external factors influencing the mechanisms.

A PID loop is one of the most common Closed Loop control methods. Similar to Feedforward, it has some values you need to tune, but it takes into account the current state of the mechanism. PID is very useful for controlling position.

The letters in PID stand for the 3 steps the controller looks at:

P - The Proportional term usually has the biggest impact. This step in the calculation takes the error, which is just the difference between the desired setpoint and current position, and multiplies it by the P constant. When the mechanism is far away it moves fast, and as it gets closer it slows down.

I - The Integral term tries to drive the total accumulated error to zero, and outputs a value proportional to this buildup. In other words, if the mechanism gets stuck away from the setpoint, the I term will build up over time and then apply higher and higher voltage until it goes towards the setpoint. If you know calculus, it takes the integral of the error and then multiplies it by the I constant.

D - The Derivative term tries to drive the derivative of the mechanism's position to be the exact same as the derivative of the error. This has a few effects. If the setpoint from the mechanism is constantly getting higher and the PID loop can't ever catch up, this gives it a boost so it follows the setpoint closer. Also, if the PID loop is oscillating around its setpoint, meaning it keeps overshooting and then overcorrecting the opposite direction, the D term will provide dampening and stop the oscillation.

The outputs from each term are then added together, producing the final output of the loop.

Realistically though, all you need to know is that P is the main driving force of the loop, and really gets you most of the way to the setpoint. I is able to give the loop a kick if it gets stuck, and D helps if the setpoint is constantly changing, while also dampening oscillation.

Keep in mind, if the mechanism is far enough away from its setpoint, it could try to feed the motor an astronomical voltage. You should always clamp the output of your PID loops, or in other words, limit the maximum voltage that can be given to the motor.

1. Complete both WPI's Flywheel control tuning , and Arm control tuning examples to better understand what the control loop values actually do.

! !

2. Create and use an PIDController to control Elevator1 with the values from Constants.java, then tune for your own Kg and Kp. Make sure you clamp the output.

You will be controlling a simulated elevator. To view the simulation, follow the steps on How to run simulation, and then once the Glass GUI is open, follow the steps on Displaying Mech2D In Sim.

To use the values in Constants.java, it should already be imported, so you can just call Constants.value to get what you need.

In this case, just take the PID output and add Kg to it to compensate for gravity. You won't do a full feedforward loop.

To tune for Kg:

• Make sure isTestMode() inside of Elevator1.java returns true.
• When you run the simulation and the Glass GUI opens up, inside the NetworkTables window look under Transitory Tables > Shuffleboard > Elevator > ElevatorVolts.
• You can click in this box and manually set the voltage sent to the elevator. Mess with this value until the elevator holds its exact position.
• Once you have the voltage for Kg, put it in Constants.java under kElevatorkG.

To test your solution, make sure isTestMode() returns false, and then run the sim. You can then change the Elevator Setpoint value to test your pid loop. You can also graph the position and setpoint to get a better idea of how good your control loop actually is: How to Graph In Simulation.

How to run tests Expected result

!

 PIDController pid = new PIDController(Constants.kElevatorKp, Constants.kElevatorKi, Constants.kElevatorKd);

/**
 * @param goal for the elevator in meters. The elevator's range is 0 to 1.25 meters.
 */
@Override
public void reachGoal(double goal) {

  pid.setSetpoint(goal);
   double pidOut = pid.calculate(elevatorSim.getPositionMeters()) + Constants.kElevator1kG;
   setVolts(MathUtil.clamp(pidOut, -12, 12));
}

Using exclusively Feedforward control on an elevator isn't ideal, and as mentioned earlier, Motion Profiles can help make Feedforward work better for position, but most of the time you don't want to use exclusively Feedforward.

The issue with exclusively PID control on an elevator is that elevators are usually affected by gravity, and so your Closed Loop will not perform as good as you might hope, often oscillating and falling out of the setpoint. This presents itself in a jerking motion as it tries to hold a steady position. You can remedy this by adding a constant Feedforward value that will counteract gravity, just like you just did in the previous lesson. This will work pretty well.

You can combine Open Loop with Closed Loop control when needed to get the best of both. In general, the Feedforward control will get the mechanism most of the way there, and then the PID loop will compensate for any outside forces or small error.

While feedforward and feedback control offer convenient ways to achieve a given setpoint, we are often still faced with the problem of generating setpoints for our mechanisms. While the naive approach of immediately commanding a mechanism to its desired state may work, it is often suboptimal. To improve the handling of our mechanisms, we often wish to command mechanisms to a sequence of setpoints that smoothly interpolate between its current state, and its desired goal state.

With PID and Feedforward normally, you go from the current position straight to the setpoint. Motion Profiles allow you to generate a bunch of setpoints and interpolate between them, leading to an overall smoother mechanism movement, which becomes important when you're moving fast.

The most common motion profile is Trapezoidal: ! You can see the trapezoid namesake in blue.

Motion Profiles build on top of PID and Feedforward; you can't use motion profiles on their own. You have to combine them with another method of control. The Motion Profile gives you setpoints, then you use PID and Feedforward to reach those setpoints.

Create and use a motion profile with PID and Feedforward to control the Arm.

You will be controlling a simulated arm. To view the simulation, follow the steps on How to run simulation, and then once the Glass GUI is open, follow the steps on Displaying Mech2D In Sim.

To use the values in Constants.java, it should already be imported, so you can just call Constants.value to get what you need.

How to run tests Expected result

!

public static Command getSequentialAuto(Drive drive, Intake intake) {

      Command command = new SequentialCommandGroup(
              new StartEndCommand(
                      () -> drive.drive(0.5),
                      () -> drive.drive(0),
                      drive).until(() -> drive.getPos().getX() == 3),
              new StartEndCommand(
                      () -> drive.drive(0, 45),
                      () -> drive.drive(0, 0),
                      drive).until(() -> drive.getPos().getRotation().getDegrees() == 90),
              new ParallelRaceGroup(
                      new StartEndCommand(
                              () -> drive.drive(0.5),
                              () -> drive.drive(0),
                              drive).until(() -> drive.getPos().getY() == 3),
                      new StartEndCommand(
                              () -> intake.runIntake(1),
                              () -> intake.runIntake(0),
                              intake)));

      return command;
  }

Closing