Holding up a wrist with a NEO

So our team has a wrist mechanism that we want to put on our robot this year. We have it to where we can make it go to a specific position, like straight up or down or sticking straight out. The problem arises when we add weight. The spark max brake mode can’t hold up what we need to hold up without running the motor. So my question is if there is a way to calculate at runtime, how much power to supply the motor with for the wrist to hold up our mechanism.

Use the WPILib armFeedforward class with gains from either theoretical calculation (e.g. http://reca.lc) or system identification.


Will that account for voltage drop of the battery if the batteries aren’t all the same?

1 Like

Most motor controllers offer voltage compensation features, and WPILib has a default implementation through the setVoltage method on MotorController.

1 Like

so, the arm isn’t controlled by gyroscope, it just uses position control on the NEO so would the arm feedforward be appropriate here?

We have had this issue before and for some things we use a ball shifting gearbox(or parts thereof) with the second gear tied to a fixed position (usually a whole broached in a plat for a half inch axel). You run the wrist or elevator to a specific position one one gear in the shifting gearbox and fire the the shift to lock the gearbox to the other gear that is locked in place then that keeps you from moving at all. We use this to lock elevators in place but had to switch to a larger diameter piston to have enough pressure to hold the elevator in place with heavy loads.

Ideally, because we don’t have a shifting gearbox, we’d like to do this all in software. Is it possible to do this with a combination of feedforward and pid control?

If I want to hold the arm up, the desired velocity should be 0 correct? When the arm falls, it has a negative velocity so would the feedforward work if I set the velocity to 0? Or would the feedforward think I want to just let the arm fall?

You don’t want a feed forward based on velocity, but instead have it change based on the angle the arm/wrist is at. If you use the cosine of the angle the wrist is at it can adjust to counter gravity.

Use Position control mode on the NEO, and add the feedforward on top of that. Use this overload of motor.getPidController().setReference, you can add an arbitrary FF term on top of the feedback control


Yes, the velocity setpoint would be 0

How would I know what the feedforward value would be?

You’ll want to use the angle of the wrist with respect to the ground, something like someConstant * cos(theta). This is basically what ArmFeedforward does. The angle theta is the angle between the wrist and the ground, you can calculate it by setting an appropriate position conversion factor (sparkMax.getEncoder().setPositionConversionFactor) at construction time, and then call sparkMax.getEncoder().getPosition() to get the current position in degrees or radians. The constant kg in ArmFeedforward can be found with some fancy modeling, or you can just experiment a bit to find a good value.


So I wouldn’t need a gyroscope to find the angle? Could I just use the position of the motor’s encoder and map the rotations to radian values?

1 Like

No need for a gyro (not really practical to put one on a wrist), just use the built in encoder on the NEO. As long as you make sure to start the wrist in the same position each time you start the robot, you should be able to map the NEO encoder’s position to an angle in radians or degrees.

The easiest way to figure that out is:

  1. Start the robot with the wrist in a known position. We’ll call this startAngle
  2. In your code, put the value of motor.getEncoder().getPosition() on SmartDashboard
  3. Move the wrist to a known angle (maybe 90 degrees). We’ll call this endAngle
  4. Record the value of getPosition() at endAngle, and use that to set the position conversion factor motor.getEncoder().setPositionConversionFactor((endAngle - startAngle) / valueAtEndAngle)

If you do that, then getPosition() will from now on give you an angle in degrees

1 Like

and then just pipe that into the feedforward?

1 Like

Yeah, just put the angle into the ArmFeedforward.calculate method. Something like

private ArmFeedforward armFeedforward = new ArmFeedforward(ks, kg, 0);

// In constructor
public Wrist() {
 motor.getEncoder().setPositionConversionFactor((endAngle - startAngle) / valueAtEndAngle);

public void setAngle(Rotation2d angle) {
 // Here I'm not using the `kv` part of ArmFeedforward, just letting built in PID do the rest
  motor.getPidController().setReference(angle.getRadians(), 0, armFeedforward.calculate(angle.getRadians(), 0));
// This is assuming your position conversion is configured to return radians

awesome thanks!!!

1 Like

I’m guessing I’ll have to tune the P, I and D constants as well, correct? Can I use the Sysid tool to get all the constants I need?

I don’t have any experience with the SysID tool, but it may be easier to tune the PID manually. You can tune it using the REV Hardware Client to avoid having to debug your code and do tuning at the same time.