Flywheel velocity control

VEX V5 General Discussion Technical Discussion
#1

So I’ve been playing with my flywheel test setup and some closed loop velocity control. I have some preliminary results that I will share in this post and hopefully update the thread as I refine the control code.

My test setup looks like this.

I created this with the following constraints for my design.

  1. Capable of using 1 through 4 motors.

  2. Motors do not share an axle, I really don’t like designs that have a motor at each end of an axle the needs to be cut to a precise length to work and is hard to assemble.

  3. Motors set to 160 rpm speed gearing, first stage external gearing at 5:1. This gives me a shaft that has a theoretical free speed of 800 rpm that becomes the input to further stages of gearing able to spin my flywheel

A closeup of the first stage gearing.

When I run this and measure the free speed of the output from the first stage gears, it’s essentially the free speed of the motors, slightly over 800 rpm.

For the velocity tests my flywheel is connected using chain and two sprockets, a 30 tooth on the drive shaft and 12 tooth on the flywheel. The final design may use a 6 tooth but I wanted to keep speed down for this round. I like to use chain as it allows flexibility on where to place the flywheel.

My original plan was to compare PID and TBH, the newly discovered (for vex forum at least) “take back half” algorithm. I’m writing the control code using the ConVEX library, this gives me a bit more control when streaming real time data, I will port back to ROBOTC later. Anyway, as many of you have found, tuning PID is hard and after playing with the PID constants for a while and not meeting my requirements I pretty much decided to stop pursuing it. The TBH controller was really easy to setup, it’s really just a P controller with what I would call a “fiddle factor”, that is, it uses some of the knowledge we have about the ballistics of the flywheel system to simplify getting to our end goal, that is, the flywheel running at a constant velocity.

To analyze the system I stream several of the variables in the control code to the real time graphing program I created last year. The following graph shows the most simple version of the TBH algorithm, the intermediate “tbh” variable is not initialized and starts at 0.

I’m showing five variables on this graph, from the bottom;

actual velocity - this is my attempt to measure velocity at the motor using an IME

set velocity - the required velocity at the motor to achieve the flywheel speed I want

current - this is the estimated current for a single motor calculated using the algorithm from my smart motor library.

TBH - this is the “take back half” (for want of a better name) variable that is used each time the error between set and actual velocity changes sign.

Motor drive - this is the control command sent to the motor.

Unfortunately as all of these variables have a very different range of values there is no easy way of showing the Y scale on the graph.

The TBH algorithm has only one variable that needs to be chosen, this is the gain of the P controller section of the code. A small value of gain will cause the motor to accelerate slowly, a large value will cause fast acceleration. I chose a value to keep the motor current reasonable during startup, the maximum measured current was about 1.8A with an average current of about 1A for the first second of operation. This is well within the capabilities of the motor PTC so no PTC problems during flywheel spin up.

As the TBH variable is initially set to 0, you can see how motor control speed is dropped to half when the error in velocity first becomes zero. The flywheel reaches the desired velocity and equilibrium after about 6 seconds.

The first improvement to make is a better estimate of the initial value of the TBH variable. We know approximately what the required drive for the motor is from the results of this first test and can use this information. Let say that we know the motor will receive a control value of 90 to achieve the desired flywheel speed. At the first zero crossing the motor drive is calculated as follows.

motor drive = 0.5 * (motor drive + tbh)

we know we want the result of this calculation to be 90, lets assume just before this calculation we have been sending the maximum value of 127 to the motor
90 = 0.5 * (127 + tbh)
so tbh would need to be 53.

This graph shows the result of using a predicted motor drive at the first zero crossing and pre calculated value of tbh (which is calculated just before it is used and does not show in the graph).

The time for stable operation is reduced from 6 seconds to about 1.5 so quite an improvement.

(hit the image limit so continued in the next post)

5 Likes
Flywheel velocity control - revisited
Vex iq code blocks flywheel PID control
#2

Next I wanted to see how battery voltage would affect this. The above tests were done with a battery voltage of 7.5V

Here is performance when the battery is 8V.

and 7V.

It can be seen that my predicted control value does not work as well for these two cases, when the battery is at 8V less drive is needed, when at 7V more drive is needed. An improvement to the code will be to modify the predicted value based on measured battery level.

I wanted a way to test the actual speed of the flywheel rather than assuming it was simply a scaled version of the motor speed. One option would be the old red quad encoder, however, as we have found running these at such a high speed more or less destroyed them. So this is what I came up with as an alternative. Paper disk attached to flywheel with a hole in it. VEX light detector measuring the light every rotation of the flywheel.

The output of the light detector is measured using an oscilloscope (you knew it would appear eventually !) and the period measured.

My target speed was 1800 rpm, or 30 rotations every second, scope shows this is achieved quite well.

A few more interesting numbers.
To achieve 1800 rpm I needed motors running at 144rpm (total 25:2 gear ratio, 144*25/2 = 1800). The systems stabilizes with a motor control value of 72 with each motor using approximately 0.35A (I have three motors installed), again, well within the PTC capability.

The next graph shows what happens with (simulated) balls being fired. I was really just momentarily slowing the flywheel by hitting it with my hand, not completely accurate but good enough for this exercise.

I hit the wheel three times, you can see the dip in velocity between 1000 and 1200 on the x axis. What I notice from this test is that the TBH algorithm causes the velocity to first settle slightly too high before dropping based on the P controller gain. This feels like the zero crossing detection should be turned off after lock has been achieved, a test for another day.

So what did I learn from these tests.

  1. Measuring rpm is critical and not easy. I always had between 2 and 5 percent jitter on my rpm measurements of the motor. This is not really news so I didn’t mention it earlier, but the fact is that, for whatever reason, unless you filter heavily, the most basic measurement you need for this code to work, the motor speed, will have jitter. The jitter did not seem to migrate all the way through to the flywheel, I suspect the use of chain really helps with this, it dampens out the high frequency variations.

  2. Tuning velocity PID in a slow to respond system is hard. I had more luck last year with my triple lift.

  3. Tuning the TBH algorithm is fairly easy with gain being determined by the acceleration, and therefore peak motor current, you are comfortable using.

  4. It compensates well for varying battery voltage but can be improved further.

  5. There will be more “fiddle factors” that can be applied when starting to shoot balls.

Now I have not included any code yet, that’s partly because it’s a work in progress and partly because it’s native C code and will not work in ROBOTC (or EasyC) currently. I will show the core function (see below), this is called periodically (25mS in the current version) and updates the motor drive (which is in the range 0 to 1.0).

/*-----------------------------------------------------------------------------*/
/** @brief      Update the velocity tbh controller variables                   */
/** @param[in]  fw pointer to flywheel controller structure                    */
/*-----------------------------------------------------------------------------*/

void
FwControlUpdateVelocityTbh( fw_controller *fw )
{
    // calculate error in velocity
    // target is desired velocity
    // current is measured velocity
    fw->v_pid.error = fw->v_pid.target - fw->v_pid.current;

    // Use Kp as gain
    fw->v_pid.drive =  fw->v_pid.drive + (fw->v_pid.error * fw->v_pid.Kp);

    // Clip - we are only going forwards
    if( fw->v_pid.drive > 1 )
          fw->v_pid.drive = 1;
    if( fw->v_pid.drive < 0 )
          fw->v_pid.drive = 0;

    // Check for zero crossing
    if( sgn(fw->v_pid.error) != sgn(fw->v_pid.last_error) ) {
        // First zero crossing after a new set velocity command
        if( fw->v_pid.first_cross ) {
            // Set drive to the open loop approximation
            fw->v_pid.drive = fw->v_pid.drive_approx;
            fw->v_pid.first_cross = 0;
        }
        else
            fw->v_pid.drive = 0.5 * ( fw->v_pid.drive + fw->v_pid.drive_at_zero );

        // Save this drive value in the "tbh" variable
        fw->v_pid.drive_at_zero = fw->v_pid.drive;
    }

    // Save last error
    fw->v_pid.last_error = fw->v_pid.error;
}
4 Likes
#3

Still haven’t finished reading but it all seems very awesome and helpful for this year’s game, so commenting first. Great job Mr. Pearman.

One idea: few days ago I made a thread about using ratchet and pawl to basically eliminate the concern of suddenly stopping flywheel motors destroying everything. And a few other teams, including the 3 day build thread, already started to play with it. So will the TBH work better with a ratchet than PID velocity? IDK just a thought.

Another thing: The color sensor velocity sensing is pretty awesome, but why not print out a sheet of paper with multiple black lines spaced precisely to increase the resolution of the velocity reading?

Also if we can do this:

Then we might not need PID control for base at all… just time and a battery voltage scale factor… :stuck_out_tongue:

Question:


drive =  drive + (error * Kp);

To me this looks like integral calculation… so I am guessing because we predominantly use this algorithm in speed control, we call this “proportional gain” in velocity control?

In summary, thank you. I am studying the code and possibly will do videos explaining the algorithm.

Edit: one more thing – I would really love to see ROBOTC adding some more graphics feature like this. We all know that copying debug stream into excel is just, well, inefficient and impossible with people who don’t have excel…

#4

Here’s a (simplified from yesterday) ROBOTC version of the test code I used. The motor measured rpm calculation is not filtered but that doesn’t seem to make much difference.

flywheel.c

2 Likes
Flywheel velocity control - revisited
#5

The light detector needs a lot of light, the difference between reflected light with black and white stripes may not be enough. You may be able to do that with the line detector sensor, however, all I really wanted was a way to measure the time of rotation of the flywheel, this was good enough. It’s only used during testing and would not be on the final design, I just wanted to confirm that the speed I see at the motor is really being translated into the correct speed at the flywheel.

2 Likes
#6

What exactly is meant by “Predicted Drive”? This code looks very clean and simple, and thanks for releasing it!

#7

The issue is what happens when the code is started with a new target velocity. As the first zero (that is the error reaches zero) crossing point the following code would normally be executed (in the plain vanilla implementation).

fw->drive = 0.5 * ( fw->drive + fw->drive_at_zero );

but what should the drive_at_zero variable be set to. The first graph in post #1 shows what happens when drive_at_zero is initially 0, stability takes a long time to reach. Some implementations assume that the drive variable is 1.0 (meaning motor control value of 127 on our hardware), drive_at_zero can then be set to an initial value such that after the calculation drive will be approximately correct. I did not want to run the flywheel with full acceleration so the value of drive at the first crossing point is unknown, however, we do know approximately (based on experiments) how much drive is needed to keep the flywheel running at almost the correct speed, this is what I call “predicted drive”, a value that is close and that can be used as a starting place before the closed loop code does its thing.

Another way of looking at this number is that this would be the motor control value if there were no other variables, battery voltage was constant, friction did not change etc. remember, I work with a 0 to 1.0 scale, a predicted drive of 0.55 means a motor control value of 70 (0.55 x 127).

1 Like
#8

While I am happy that the VEX world has taken a liking to an alogrithm which I have found to be extremely underused in both FRC and VEX, I would like to point out ONE TEENSY THING, really tiny, seriously, that your post, and seemingly everyone I have ever presented it to seems to make upon learning of it, and that is that it is NOT a P controller. If it was it would never get rid of SS properly. It is an I controller, and is meant to be used at much higher gains, to the point where it essentially turns into a bang-bang controller.

How close you get to the “bang-bang” side of things depends on the quality of the flywheel, but I’m sure VEX teams can make something that lets them at the very least double the gain you were using, if not triple or quadruple.

No it would not work with a ratchet without major modification unfortunately. It requires a direct relationship with the system (flywheel) in order to achieve its low SS and aggressive spin ups.

#9

Well, yes, in as much as you are integrating the error in velocity into the motor drive. In my own mind I saw it as a P controller (the change in motor drive is proportional to the velocity error) as when I’ve implemented PID for velocity control in the past I still had I and D terms as part of the equation. However, if we want to call it an I controller then that fine.

In this version I wanted the gain lower to control peak motor current and reduce stress on the mechanical components. As I said in an earlier post, this was an initial implementation just to see how good it was. I will leave it to the community to iterate and improve.

2 Likes
#10

First, thank you very much for sharing this. I also found PID almost useless for this purpose. I modified this for a two wheel shooter with pretty good results. The one issue is that after each firing, the wheels actually over compensate a bit. Seems like the rapid loss of velocity may cause a brief over adjustment and the code seem better at increasing up to the value faster, than reducing speed if it gets too fast. My guess is that this is because once over the limit, momentum take a greater delta to adjust downward. Seems like it takes about 3-4 seconds to lower back down. Any suggestions to reduce the over compensation effect?

#11

Not really. I didn’t do much with this project after the original post. There’s only one variable to play with in the code, have you tried adjusting that? I had some NbN balls show up today so perhaps I will put this back together and do some more tests over the next week or so.

1 Like
#12

How exactly does the program control the speed of the launcher based on the Battery Level?

#13

So the TBH algorithm is an I loop. This mean that the control loop monitors the speed of the flywheel and keeps trying larger and larger power outputs until it achieves its goal. This means that at 7.5V the robot may have to run 127 power to get the desired speed or only run 80 power when at full 8.7V. The code will find the power requirement to get the speed required. You just need to limit the intake to only put balls in the flywheel when it is ready/ at desired speed.

2 Likes
#14

But where in the program does it even call the battery level to be taken into account? I cant find it anywhere.

#15

It never does. It monitors speed. If the flywheel is spinning at the goal speed than we don’t care what power level it took to get there. The algorithm chooses a different power level every time you run it.
The program will find the correct power value no matter what the voltage is.

1 Like
#16

Can you show me how to modify it for a two wheel shooter? I tried modifying it myself and since I am new to robotc I had zero success and would love to have some help. I did however also find that the bottom button that sets all the motor values to zero was unresponsive possibly because of it getting stuck in one of the loops. I can probably code in a master off switch but I was just curious as to weather this was supposed to happen. Thanks again.

#17

Here is some example code made which has been modified by Adam T over on the TBH control revisited thread (which has a simplified version of the TBH robotC code). I put it here with the change mentioned in the thread proceeding his post.

1 Like
#18

The code example downloads with errors for compilation to begin with. Was that intentional in order to stop people from stealing the code or is that the wrong code?
6761b53919101f07c400-7b5cf7297b243debf5c9f111b676d0978a716798 (1).zip (3.67 KB)

#19

It’s not my code, it was created by Adam T and I currently have no means of testing it sorry :D. But if you can fix up any errors and suggest an edit either here or on github that would be great. On tuesday I can try and fix up any errors if you want :stuck_out_tongue:

#20

Ok so Here is an update. I decided to go ahead and try to alter jpearman’s original code to my flywheel and after a bit of rough trials I managed to do it.

I have attached the user control code I had previously with its autonomous and the altered jpearman code for my flywheel. I’ve also attached pictures of my flywheel setup. You can find all of that here. It seems to work correctly and when I set the motors to a set speed using the buttons I can see the flywheel catch up to the desired speed. There is just one problem and that is I don’t know how to set my own speed for the code.

Remaining issues:
I need to know how what and where to plug in the desired motor value that I will translate to autonomous and user control later.
I also need to know how to translate this code into a competition code.
Notes:
When doing it with a two wheel, make sure that you define all of the motors correctly and make sure that they are all set to “value” in the code or else it breaks your flywheel.

Thanks again for all of the help from everybody. I really do appreciate all the welcome assistance since this is my first rodeo with robot C.