Flywheel Velocity PID

VEX V5 Technical Support
1

Hello everyone,

We are working on a PID velocity controller for our flywheel. We have spent around 2 weeks or so trying to get our flywheel to accurately shoot from a distance. However, we’re having some trouble getting it to shoot at a consistent speed. We’ve tried using the inbulit PID velocity loop provided with the V5 motors and playing around with its constants. We’ve also made a custom loop in which we also adjusted the constants. We also tried adding stabilizing code that waits for the flywheel’s speed to be within a certain range for a certain amount of time before shooting. Even that didn’t too much though.

Currently, the best we’ve gotten it is that when the flywheel starts, it oscillates for about 5-7 seconds and then appears to stabilize. We’re not really sure how to get rid of this oscillation in the beginning. We would appreciate some advice on how to get rid of this oscillation and other advice on how to get a consistent shot.

Here is our PID loop:

//Constants//
float kp = .07;
float ki = 0;
float kd = .11;

//PID Variables Here//
int currentVelocity = flywheel.get_actual_velocity();
int error = targetFlywheelSpeed - currentVelocity;
int lastError = 0;
int totalError = 0;
int integralActiveZone = 15;

int onTargetCount = 0;
float finalAdjustment = error * kp; //add the rest of PID to this calculation

//Temp Variable//
int deltaTime = 0;

while (true)
{
	if (maintainFlywheelSpeedRequested == true)
	{
		currentVelocity = flywheel.get_actual_velocity();

		error = targetFlywheelSpeed - currentVelocity;

		if (error == 0)
		{
			lastError = 0;
		}

		if (abs(error) < integralActiveZone && error != 0)
		{
			totalError += error;
		}
		else
		{
			totalError = 0;
		}

		finalAdjustment = ((error * kp) + (totalError * ki) + ((error - lastError) * kd));
		currentFlywheelVoltage += finalAdjustment;
		if (currentFlywheelVoltage > 127)
		{
			currentFlywheelVoltage = 127;
		}
		else if (currentFlywheelVoltage < 0)
		{
			currentFlywheelVoltage = 0;
		}

		flywheel.move(currentFlywheelVoltage);
                    lastError = error;
    }
2

One of my teams has a flywheel using the built in PID and, eventually, arrived at a quick (3ish second) spin up to a stable speed just using motor.spin() to a target percentage. Their early season build behaved a bit like what you describe. They were using one, but now use two motors 5:1 with 600 cartridges. They were using normal axles and 2 unweighted 4" wheels and a pretty wonky structural build. They now have weighted wheels on HS axles and have carefully adjusted and braced the structure to reduce friction. They also now have grease at gears and bearings. The wheel spins for a good 20+ seconds after motor.stop(coast).

So, the issue might be friction, not program? Hopefully this helps.

3

Thanks for the prompt response. I see how the problem could be friction, but we have been perfecting this flywheel since the beginning of the season. It doesn’t feel like there is much friction in the system. We’re able to practice for around 20-30 minutes with the flywheel running for most of that time before it begins to heat up. However, I’ll measure how long our’s coasts for and report back.

4

This raises a question for me. I’d see what it looks like if you comment it out. The derivative typically works against the integral and proportional error. Cancelling it out whenever the error hits zero would decrease your resistance to change, I think.

It’s hard to have a good opinion without looking at the rest of your code, though, specifically the flywheel.get_actual_velocity() and flywheel.move().

5

Those are PROS functions. See here:

https://pros.cs.purdue.edu/v5/api/cpp/motors.html#get-actual-velocity
https://pros.cs.purdue.edu/v5/api/cpp/motors.html#move

6

That’s an interesting thought. I’ll give that a shot in a few hours and let you know how it goes.

As for those functions, those are inbuilt functions with PROS as @hotel mentioned.

7

Ah, that’s helpful. Thanks!

8

We just tried removing this statement. It didn’t seem to make a difference, but we’re going to try tuning some constants with that removed to see if it gives us better results. What you said does make sense, so hopefully we’ll have some luck.

9

We’ve been working on tuning those constants for the past 3 hours now. It does stabilize after some time (error stays between 6 rpm of the motor above or below the target), but it takes too much time for it to be helpful in competition. Any other advice?

10

Are you running 18:1 motors?

I ask because doing the maths, you might have inadvertently created a slew-like function with your ramp up.

18:1 motors max out at 200 rpm. If you’re spinning at 0 and request 200, with a proportional constant of .07, your best bet is an error around 14. Your derivative is going to initially be (200-0)*.11, or 22 on the first loop. So your voltage request is going to go from 0 to 36, which is a 28% jump on the 127 scale.

The second loop you’ll have a smaller proportional error and your derivative component will actually be negative since your error is decreasing as you approach your set speed. So, say you get to 50rpm after the first loop, your derivative will be (150-200)*.11, or -5.5. That’s going to continue to drag your acceleration down by decreasing the voltage you request from the motor.

You might try logic that slams the voltage request to max until the RPMs get within some percent (say 10%) of your target speed. Then let the PID take over to maintain that set speed. Kind of a feedforward approach.

if (currentVelocity/targetFlyWheelSpeed < .9) {
    finalAdjustment = 127;
}
1 Like
11

For your code here, the advice I give to you for your code is to print out your motor’s actual velocity and the value you are trying to set the motor to. Try and set your target to different things and see how they react. Try max speed and see when it evens out, along with how long. Then try at half speed. Any differences? Do this for some more speeds. Write this information down and try to see if you can find patterns and outliers. This info will help you know what you are actually looking for in the code other than just knowing a problem exists. If the oscillation happens when the control loop is setting the motor speed to 0, make sure you didn’t set the brake mode for the flywheel motor to like brake or hold, as that can make some flywheels freak out.

Sometimes its a build problem however unlikely it may be (its likely). Make sure its not violently shaking and all the screws are tightened. Also make sure everything can easily turn and is greased.

How does it preform when just setting the pure move function? No PID or any other control loops. Just straight take this much power via the motorNameHere.move(int speed) function. One of the big things about the V5 system is being able to supply the same power without caring about the battery power, so it may be unnecessary to spend time on a control loop if it functions fine without it.

You’ve also said you have tried the inbuilt velocity loop so we can skip doing that. But from first testing it had oscillation problems with our flywheel. I have no idea if it has been changed since we got our V5’s, but IIRC the velocity control was reversing! It’s a no no for a flywheel to go from moving one way full power straight to hard reversing (from everyone I’ve worked on atleast). The gearing just doesn’t agree with it, and that second motor that is still moving forwards really dislikes it (the inbuilt PID is for each individual motor, not a grouping). I believe this is what your “else if (currentFlywheelVoltage < 0) {currentFlywheelVoltage = 0;}” is doing so I don’t think that would be your problem.

You also probably don’t need a full PID, and instead can just use a P-Loop. It’s what I am doing with our flywheel, which is mostly perfect from all of our shooting tests, and is only about 1rpm above target sometimes according to the get_actual_velocity function (which I assume is from coasting down instead of hard breaking). It’s also lighter on the cortex so that’s a plus, and it also is simpler code-wise so less areas for you to mess up.

Since you asked for it, tips for shooting consistently other than just having a consistent speed. Good robot build quality. Make sure nothing moves that isn’t meant to, the friction is good (not too much but not too little), everything on the wheel is symmetrical (if not when it hits that unsymmetrical part of the wheel it will fly off course). Change the speed of the flywheel so the drivers are more comfortable, and make sure they can easily line up. Something my team has found useful is feedback for when the flywheel is up to speed (shooting too early will cause misses). Our controllers vibrate when the flywheel is within 1rpm of the target speed. Driver practice is a really big one. Being a bit lined off by a few inches can be the difference between a miss and a hit with most bots. Maybe see if you can get your hands on a vision sensor since you seem like a capable programmer. You could make it so the flywheel always shoots at the right speed for whatever distance, and even turn to get a perfect shot. And while the V5’s are much better than the cortexes with power and motor speeds, always try to have the most charged battery plugged into your robot.

Also its a biiiiit late for me (I should have been asleep hours ago), so sorry about the bad grammar or nonsensical wording.

12

Thank you for this advice. We tried out several different ideas from this thread and had some success, so thanks to everyone who posted.

We tried setting the motors to different voltages and then recording the average speed it reaches after settling. We did this for a number of different voltages and then created an equation based on these inputs and outputs. Our velocity controller sets the flywheel to this speed initially. Once the error is small enough, we begin applying a P control that modifies the current being applied to the motor by small amounts. The velocity we use to calculate the error is an average velocity. It consists 40% of the current speed, and then 20% each for the past three previous velocities. This is just to put more emphasis on the current speed and ignore tiny changes in velocity that would result in a large adjustments being applied.

We also have a “stabilization” feature that forces the flywheel’s velocity to be within a specific threshold for a certain amount of time before permitting the flywheel to shoot. This has helped us make the shot more accurate, but it takes quite a while to shoot. The first shot is generally quick, but then the second shot usually takes an extra 3 seconds or so. Sometimes, it even takes up to 10 seconds to work. Any advice on how to fix this? The code follows:

while (true)
{
	if (maintainFlywheelSpeedRequested == true)
	{
		currentVelocity = flywheel.get_actual_velocity();
		averageVelocity = ((currentVelocity + currentVelocity + lastVelocity1 +
												lastVelocity2 + lastVelocity3) /
											 5);
		error = targetFlywheelSpeed - averageVelocity;
		if (error < -2 || error > 17)
		{
			currentFlywheelVoltage = determineFlywheelVoltage(targetFlywheelSpeed);
		}
		else
		{
			finalAdjustment = (error * kp) + (totalError * ki);
			currentFlywheelVoltage += finalAdjustment;
			if (abs(error) > integralActiveZone && error != 0)
			{
				totalError += error;
			}
			else
			{
				totalError = 0;
			}
		}
		lastVelocity3 = lastVelocity2;
		lastVelocity2 = lastVelocity1;
		lastVelocity1 = currentVelocity;

		flywheel.move(currentFlywheelVoltage);

		if (abs(error) < 4)
		{
			onTargetCount++;
		}
		else
		{
			onTargetCount = 0;
			flywheelOnTarget = false;
		}
		if (onTargetCount >= 35)
		{
			flywheelOnTarget = true;
		}
		else
		{
			flywheelOnTarget = false;
		}
         }
    pros::delay(20);
   }
13

@Electrobotz can you please post some sample code in C++ for forward and backward movement using PID? We use C++, on v5 systems, in VCS.

14

Sure thing. I’m about to turn off for the night though, so I’ll send you some simple code tomorrow. My current code has some other things that may be confusing if you’re just looking at PID for the first time.

15

I don’t know many commands available in VCS since I use the PROS api, but I’ll basically write the code and explain it to show you one possible implementation.
void driveForward(int ticks) {
float averagePos = (rightMotor.get_position() + leftMotor.get_position()) / 2; //This is getting the average of the two sides of the robot. I would use all the encoders you have on your base (inside the V5 motors) to get the most accuracy.
float error;
while(averagePos < ticks) {
error = ticks- averagePos; //Subtract the current position from the target position to see how far the robot is from where it needs to be
rightMotor.move(error * kp);
leftMotor.move(error * kp); //Assign the motors speeds based on how far they are from their target; the higher error is, the higher the speed will be because it is farther from its target
averagePos = (rightMotor.get_position() + leftMotor.get_position()) / 2; //keep updating the average position
}

What I’ve written here is only a P loop. For driving, I’ve never needed the whole PID so I’ve never written the whole thing. This is a very simplistic approach and will probably need tuning according to your setup. However, it should get you started. If you have any questions, feel free to ask. Just make a new thread with an appropriate title so others looking for help regarding the same topic can easily find the information. Good luck!

Also, if anyone would like to explain to me how to make code format like code on this new forum, that would be great!!

16

Three accent characters ``` before and after the code block.

For driving a distance and slowing down as that distance converges on zero a P loop is really all you need. There won’t be any oscillation or rate of change issues to smooth out with the I and D terms.

If you want to drive in a straight line for that distance by comparing the distance travelled between the left and right sides of the robot, that’s different story.

17

One issue here is that the += integrates, so you have no actual P. You have an integral and then a 2nd integral. This would give you a very slow response.

18

I know. I’m just not sure how else to do it. Velocity PID has been something that confuses me because if error is 0 (the flywheel is at its target), error * kp would be 0, setting proportion to 0 and effectively setting the motor to 0. Even as error gets smaller and smaller, it would start reducing the motor speed, which is not what we want here. We want it to hold its position as it gets closer and closer. By doing what I did here, I am essentially slowing down the “acceleration” of the motor as it gets closer to its target.

If I’m misunderstanding the concept though, please correct me. I would love to see that there is a way to get a faster response. We recently sped up our intake and I would love for our flywheel’s response speed to match our intake speed.

19

It’s ok, it confuses a lot of people, So what happens is that a pure P loop can never get to the target value because, as you said, the error approaches 0 so:

Kp * (0 error) = 0

But P can react instantly to error because the output would just be:

output = Kp * (error)

On the other hand, your integral is summing up the errors over time. Ki is generally much much smaller (10x - 100x…) than Kp so it does not put out much the first time through your control loop (or after you reset you integrator to 0), but it will put out a good value over time.

output =( kp * error) + (Ki * sumOfErrors)

So, when you first turn on your control loop the P portion takes control (since I is 0 to start) and gives you quick reaction. So almost all output is due to P. As you approach the target the P portion heads toward 0 output and the I portion has been summing errors so it has an output value that now ramps up to 0 error in your target and actual speeds.

So be careful how you reset your integrator because if your P portion doesn’t have enough gain to get you into your IntegralActiveZone then you will never get to your setpoint, or zero error, since you keep resetting the one term that will get you there, the I term.

Also, everyone seems to forget that:

output = PID + C

That C term is important, because if you know of a power value that you can put out that will never overshoot your speed when unloaded you can use the C term to get a head start. Or, you can do something similar by tying C in with the integrator and resetting the integrator to C instead of 0.

BTW, in NBN our teams ended up with bang-bang instead of PID for instantaneous response. It work very well but you had to pay attention to friction and and the power you “banged” between.

1 Like
20

Thank you for that. This explanation is what I have been looking for, so I am very thankful.

So would it be a good idea to set I to C when error is large and it is not in its “active zone”?
Once it reaches its active zone, it would be modified according to the total error. P would be very small, so it would be making very minimal adjustments, while the I term would now be holding the current speed.