Complicated turret feedforward

Hello, I am trying to incorporate ideas seen in multiple posts (post 1 about rotational feedforward and post 2 about tangential feedforward) to code the feedforward for our turret this year. As I want the turret to stay locked on the target as we move around the field, I think the feedforward will have to take into account the tangential velocity of the robot with respect to the target as well as the rotational velocity of the robot (we are using mecanum drive).

I think I have figured out how to find the tangential feedforward based on the robot’s tangential velocity and distance to the target, and the rotational feedforward would just be the opposite of the robot’s angular velocity.

Also I was thinking I should use the WPILib SimpleMotorFeedforward class along with the SysID tool to make it more accurate.

So I was wondering how exactly to implement all of this together on top of PID control. I was thinking something along the lines of

motorTurret.setVoltage(pidController.calculate(getEncoderVelocity(), setpoint) + simpleFeedforward.calculate(setpoint) + tangentialFeedforward + rotationalFeedforward);

Would this work? Should I be using a velocity or positional PID here? I am pretty new to complicated control stuff like this. Thanks.

-Samuel

You need to be careful that you “cascade” the controllers in a coherent way given their dimensions. Your tangential and rotational feedforwards here are probably in units of velocity; that suggests that they should modify the setpoint itself, and thus cascade to your SimpleMotorFeedforward and PIDControllers.

I am curious as to why you’re performing velocity control on your turret; you can probably do fine with a “flat” position controller (in which case you only need to cascade to the SimpleMotorFeedforward).

1 Like

Honestly I don’t really know what I am doing, and I think I have confused myself. I don’t have any programming mentors on my team so I have had to learn all of this on my own.

The end goal for this is to have the turret constantly track the target throughout the match and be able to score into the high goal while moving in any direction by compensating for the robot’s movement in our shot. I am working on the turret stuff now and am going to figure out all the details of tuning the actual shot and accounting for the robot’s velocity by offsetting the angle of the shot later. I know this is something that will be hard to pull off but I am somewhat confident I can figure it all out.

Here is my turret subsystem:

And here is the command so far for tracking the target throughout the match which I will have running 24/7:

Also, could you explain what you mean by cascading the controllers?

I have thought about it some more and I think I understand it better. Is this what you were thinking when you talked about modifying the setpoint directly?

setpointV = setpointV + tangentialFeedforward + rotationalFeedforward;
motorTurret.setVoltage(pidController.calculate(getEncVelocity(), setpointV) + simpleFeedforward.calculate(setpointV));

The problem I am having with just using a position pid instead of a velocity one is that I am not sure what I would be putting as the position setpoint. With the velocity control, I can just make the desired velocity setpoint (before adding the feedforward) proportional to the Limelight tx angle. If I wanted to use position control then I think I would have to take into account the robot’s position on the field because if I was at a different place on the field then the angle (relative to the robot) that the turret would need to be in order to score would be different. Is this correct or am I completely missing something simple?

Edit: if I did position control, would setting the setpoint to be turretAngle - tx work? Then I could do:

double pidOutput = pidController.calculate(angle, angle - tx);
motorTurret.setVoltage(simpleFeedforward.calculate(pidOutput) + tangentialFeedforward + rotationalFeedforward);

Is this what you meant when you said this?

Assuming both velocity and position control work, is there any benefit to using one over the other or would they have the similar results/smoothness?

Edit 2: I have come to the realization that

pidController.calculate(angle, angle - tx)

would give the same exact output as

pidController.calculate(tx, 0)

which is probably what you meant in the first place. Oh man.

Also, for the position control, instead of this line:

motorTurret.setVoltage(simpleFeedforward.calculate(pidOutput) + tangentialFeedforward + rotationalFeedforward);

would I be better off with this one:

motorTurret.setVoltage(simpleFeedforward.calculate(pidOutput + tangentialFeedforward + rotationalFeedforward));

Yes, you seem to have figured out the gist of the thing. Keep track of the dimensions, and it will all work out.

Position control is equivalent to velocity control in this situation, once you have tuned it properly - every cascaded controller has a flat equivalent. The position controller is simpler in structure, however.

2 Likes

Awesome! So go with this line for the position control then?

motorTurret.setVoltage(simpleFeedforward.calculate(pidOutput + tangentialFeedforward + rotationalFeedforward));

Thanks

Not quite; the PID output is a voltage, not a velocity. It should be added to the output of the feedforward calculation. The feedforward calculation looks correct otherwise.

Oh ok that makes sense. So this then?

motorTurret.setVoltage(pidOutput + simpleFeedforward.calculate(tangentialFeedforward + rotationalFeedforward));

The way I understand things, the feedforward should be doing most of the work here if I want it to run smoothly, so do you think I should be making sure my pidOutput is never more than say 3 volts (by choosing a lower kp value) or something like that?

1 Like

Your choice of kP should handle that, yes.

If feedforward essentially just converts a desired velocity into a voltage, isn’t it arbitrary whether the PID output is a velocity or a voltage? Couldn’t I just use this line

motorTurret.setVoltage(simpleFeedforward.calculate(pidOutput + tangentialFeedforward + rotationalFeedforward));

and tune the PID so that it’s a velocity? Which approach would give better results and why?

If SimpleMotorFeedforward were a linear function of velocity, this would be mathematically equivalent but would put your gains in funky, harder-to-interpret units.

SimpleMotorFeedforward is not entirely linear, though, so this won’t really work correctly as you’ve written it (it’ll change the sign of the kS correction at an incorrect place).

1 Like

Oh ok. My problem with this line

motorTurret.setVoltage(pidOutput + simpleFeedforward.calculate(tangentialFeedforward + rotationalFeedforward));

is that if my drivetrain is not moving (rotationally or translationally), then I effectively only have a PID loop with no feedforward. Isn’t this not ideal?

I think I understand your concern. The turret would not be able to move even if it were offset if the bot wasn’t moving since the pidControl would be limited to 3 volts. Honestly, I think your solution is a little overcomplicated. (or maybe I’m just underthinking it) I would suggest trying to rewrite your code to make it a little simpler. There are 2 ideas I can think of. For positional control, try:

  1. Finding the angle the turret should go to (seems you already have a good start with your vision tracking math)
  2. Converting that into motor ticks (angle / 360 (or 2pi) * encoder_resolution)
  3. Passing that value into a positional control system (the talonfx has a built-in system for this, try taking at look at our code if you need some inspiration (either look at the climber.cpp in obob’s code, or the positional control in swervemodule.cpp in any of our black widow repos) CTRE also has some decent documentation.

Make sure to add a ramp rate and limits to make sure you don’t break your turret

For velocity control, you could do:

  1. Finding the angle the turret should go to
  2. Using a PID controller to convert angle to velocity (either a wpilib one, or something you implement yourself, like we did here):
    int error_theta = (theta - getAngle()).to<int>() % 360; // Get difference between old and new angle && gets the equivalent value between -360 and 360
    if (error_theta < -180) error_theta += 360; // Ensure angle is between -180 and 360
    if (error_theta > 180) error_theta -= 360; // Optimizes angle if over 180
    if (std::abs(error_theta) < 5) error_theta = 0; // Dead-zone to prevent oscillation
    double p_rotation = error_theta * rot_p; // Modifies error_theta in order to get a faster turning speed
    if (std::abs(p_rotation) > max_rot_speed.value()) p_rotation = max_rot_speed.value() * ((p_rotation > 0) ? 1 : -1); // Constrains turn speed
  3. Converting to actual motor control (feed forward? talonfx velocity control loop?)

Best of luck!

Actually I just realized I could just share our turret tracking code lol:

SetTarget/Reference is a REV thing but you could use position control for CTRE

Edit: This guide also looks pretty nice: Case Study: Aiming Using Vision — Limelight 1.0 documentation

Ah, yeah, my earlier post was in error.

You can indeed treat the position PID controller as outputting a reference velocity and cascade that to the feedforward. This changes the units accordingly, and the benefit is a correct application of the kS term in the case when the other feedforwards are zero or cancel each other out.

Ok nice. Do you think tuning this feedforward accurately would correct for the problem of camera latency (described here) as long as I kept my kP low, or should I be implementing that separately?

If I am moving fast, my pid will effectively be fighting against the feedforward for those ~25ms until I receive my next limelight image. Because the pid wants to go back towards the old target while my feedforwards “knows” the target is actually somewhere else and so is moving towards the actual target. Thoughts?

Feedforward might help the overall system response enough that latency compensation is not necessary, but I don’t think I can really predict whether that’ll be the case or not. You’ll have to try it and see!

1 Like

Slightly off topic, but are you planning to use a Limelight to track the target for the whole match? In 2020, our local officials interpreted R.203.m to mean our Limelight could not stay illuminated for the whole match.

high intensity light sources used on the ROBOT (e.g. super bright LED
sources marketed as ‘military grade’ or ‘self-defense’) may only be
illuminated for a brief time while targeting and may need to be shrouded to
prevent any exposure to participants. Complaints about the use of such light
sources will be followed by re-inspection and possible disablement of the
device.

I think the key here is “high intensity” - to me I’m interpreting this as referring to lights with an blinding level of glare from a relatively wide angle. The references to “self-defense lighting” suggest it is the kind of brightness and field of view that can quickly hurt people and create lasting safety issues. The Limelight certainly is producing a useful, bright light, but not on the same magnitude as these products.

In past years, most notably 2012, teams would use very high intensity lights to allow drivers to manually align their robots with goals. This rule was created to require teams to be able to switch these lights off.

That was the plan. I am however currently working on using odometry to tell the turret approximately what angle to goes towards even with the limelight off. And then when we are about to shoot we can just turn it back on, and adjust from there. So if I can get that working then we’ll just switch to that if the referees get mad at us.