# Flywheel velocity control - revisited

Back in June I posted a thread about an experiment I had done for controlling the velocity of a flywheel. This experiment was done using code I had written using my ConVEX framework. I subsequently posted a simplified version of the code written in ROBOTC.

I have received some requests to explain this a little more, so here is a slightly more detailed explanation of how the code works.

To start with lets look at a diagram of the system we are trying to implement. There are one or more motors that are connected to the flywheel. I’m not showing the mechanical components here, for the sake of this discussion it doesn’t matter how many motors there are but the assumption is that they are all mechanically connected together and work as one big power source.

There is a means of measuring how fast the motor is turning. We don’t have much choice for the means of doing this on a VEX competition robot, I prefer to use an IME, a quad encoder could also be used.

The flywheel velocity controller is a software algorithm that compares the measured speed of the flywheel to a target speed and makes adjustment to the motor control to minimize the difference between these two.

My original ROBOTC code uses some programming techniques that beginners may not be familiar with. Specifically it uses a structure to collect all of the related variables together and then a pointer to this structure to pass it to the functions that will use its contents. I rewrote this code to eliminate this structure and use individual global variables instead. This code is posted here.

flywheel_s.c

Lets look at what is going on in this program.

The speed measurement function.

``````/*-----------------------------------------------------------------------------*/
/** @brief      Calculate the current flywheel motor velocity                  */
/*-----------------------------------------------------------------------------*/
void
FwCalculateSpeed()
{
int     delta_ms;
int     delta_enc;

// Get current encoder value
encoder_counts = FwMotorEncoderGet();

// This is just used so we don't need to know how often we are called
// how many mS since we were last here
delta_ms = nSysTime - nSysTime_last;
nSysTime_last = nSysTime;

// Change in encoder count
delta_enc = (encoder_counts - encoder_counts_last);

// save last position
encoder_counts_last = encoder_counts;

// Calculate velocity in rpm
motor_velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;
}``````

The idea here is to calculate the speed of the flywheel using the number of encoder ticks we count in a certain time interval.

We read the encoder into a variable called encoder_counts, I use a function called FwMotorEncoderGet to get this number, all that function really does is use nMotorEncoder to read the IME attached to one of the motors. We can calculate how many counts the encoder has moved by subtracting the value we read the last time this function was called, this is saved in a variable called delta_enc. We also calculate how much time has elapsed since we last called this function. The ROBOTC function “nSysTime” returns a number that shows how long the cortex has had power. We can use this number, along with the its saved value from the last time the function was called, to determine how many mS have elapsed since we last did the speed calculation.

We calculate the motor speed in revs per minute based on the equation ( change in encoder count / change in time ) which is then converted from encoder_counts per second to motor revs per minute.

for example,
we see a change of 10 encoder counts in 25mS (0.025 seconds) from this we can calculate that there would be a change of 400 counts in one second. We need to know how our motor is geared to then calculate rpm, for a 393 motor with the “speed” gears the wiki tells us there will be 392 counts for one revolution of the motor output. So to convert from ticks/sec to rpm we do the following math. rpm = ticks/sec * 60 / 392, for our example this would be 400 * 60 / 392 = 61 rpm.

The velocity control function
The example code implements the TBH (take back half) algorithm, you could also implement a more traditional PID algorithm in this function block.

``````/*-----------------------------------------------------------------------------*/
/** @brief      Update the velocity tbh controller variables                   */
/*-----------------------------------------------------------------------------*/
void
FwControlUpdateVelocityTbh()
{
// calculate error in velocity
// target_velocity is desired velocity
// current is measured velocity
current_error = target_velocity - motor_velocity;

// Calculate new control value
drive =  drive + (current_error * gain);

// Clip to the range 0 - 1.
// We are only going forwards
if( drive > 1 )
drive = 1;
if( drive < 0 )
drive = 0;

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

// Save this drive value in the "tbh" variable
drive_at_zero = drive;
}

// Save last error
last_error = current_error;
}
``````

The inputs to this function are the global variables “motor_velocity” (which we calculated above), “target_volocity”, the velocity that we would like the motor to have, and “gain”, a constant that is used to control how aggressive the algorithm is at trying to get the motor to the target velocity. The output from this function is in a global variable called “drive”, this variable has a range of 0 to 1 meaning motor stopped to motor full speed. A flywheel only needs to be controlled in one direction so we make sure that “drive” does not go negative.

A ROBOTC task that ties these two functions together

``````/*-----------------------------------------------------------------------------*/
/** @brief     Task to control the velocity of the flywheel                    */
/*-----------------------------------------------------------------------------*/
{
// Set the gain
gain = 0.00025;

// We are using Speed geared motors
// Set the encoder ticks per revolution
ticks_per_rev = MOTOR_TPR_393S;

while(1)
{
// Calculate velocity
FwCalculateSpeed();

// Do the velocity TBH calculations
FwControlUpdateVelocityTbh() ;

// Scale drive into the range the motors need
motor_drive  = (drive * FW_MAX_POWER) + 0.5;

// Final Limit of motor values - don't really need this
if( motor_drive >  127 ) motor_drive =  127;
if( motor_drive < -127 ) motor_drive = -127;

// and finally set the motor control value
FwMotorSet( motor_drive );

// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}``````

This function connects the speed calculation to the velocity controller. We have a while loops that does the calculations and then delays for a small period of time (defined by FW_LOOP_SPEED). The loop calculates motor speed, determines what control value needs to be sent to the motor, scales that up to the 0 to 127 values that VEX motors needs and then send it out to them.

Part 2 in the next post

4 Likes

Part 2, a continuation of post #1.

How to set the target velocity

``````/*-----------------------------------------------------------------------------*/
/** @brief      Set the controller position                                    */
/** @param[in]  desired velocity                                               */
/** @param[in]  predicted_drive estimated open loop motor drive                */
/*-----------------------------------------------------------------------------*/
void
FwVelocitySet( int velocity, float predicted_drive )
{
// set target_velocity velocity (motor rpm)
target_velocity = velocity;

// Set error so zero crossing is correctly detected
current_error = target_velocity - motor_velocity;
last_error    = current_error;

// Set predicted open loop drive value
drive_approx  = predicted_drive;
// Set flag to detect first zero crossing
first_cross   = 1;
// clear tbh variable
drive_at_zero = 0;
}``````

This is the function we call to set the target velocity. We need to give it two numbers, the target motor velocity in rpm and our best guess for the motor drive needed to attain this speed. The best guess speed is part of the TBH algorithm tuning, we determine this by seeing how our motor (flywheel) stabilizes after a few seconds have passed, for example, we want to attain a velocity of 60 rpm, we run the flywheel and use an initial best guess of 1.0, when the software is running we find that the value for drive actually stabilizes at a value of 0.35 (in other words we are sending 0.35 x 127 = 45 to the motors), this becomes our actual best guess that we use in the code. The idea is that we are giving a strong hint to the algorithm to indicate what the final value of drive needs to be under the test conditions. The algorithm will then compensate for things like battery voltage.

An example of how we would use this in Driver Control

``````task main()
{
char  str;

bLCDBacklight = true;

// Start the flywheel control task

// Main user control loop
while(1)
{
// Different speeds set by buttons
if( vexRT Btn8L ] == 1 )
FwVelocitySet( 144, 0.55 );
if( vexRT Btn8U ] == 1 )
FwVelocitySet( 120, 0.38 );
if( vexRT Btn8R ] == 1 )
FwVelocitySet( 50, 0.2 );
if( vexRT Btn8D ] == 1 )
FwVelocitySet( 00, 0 );

// Display useful things on the LCD
sprintf( str, "%4d %4d  %5.2f", target_velocity,  motor_velocity, nImmediateBatteryLevel/1000.0 );
displayLCDString(0, 0, str );
sprintf( str, "%4.2f %4.2f ", drive, drive_at_zero );
displayLCDString(1, 0, str );

// Don't hog the cpu :)
wait1Msec(10);
}
}``````

This is our main (or driver control) function for the program. The velocity control task needs to be started, I can then set one of four different speeds for the motor based on pressing buttons. For example, if I press button 8 Up then I set the target velocity to 100 rpm, my predicted drive value is 0.38, that is, I expect the motors to need a drive value of 0.38 (which is 48 in VEX motor control units) to attain this speed. These predicted values will change based on the exact flywheel design, how many motors, what the gearing is, how much friction exists etc.

So I hope this helps, there are improvements that can be made, for example, the speed calculation may have some jitter (minor changes for each calculation) on the calculated motor velocity. This can be smoothed out by filtering but at the expense of some responsiveness of the motor control. If there are still questions on how to implement this then post here.

2 Likes

Thank you for posting this. As a team we are just beginning to really learn how to do more advanced programming and this looks like a perfect tutorial for us.

1 Like

Motor control units, what are these?

I tend to do all my control calculations with normalized values, that is, I use a range of 0 to 1.0 (or -1.0 to 1.0) for control values and then convert to the actual numbers that ROBOTC needs later on. In the case of this code that is simply done by multiplying by 127. Slightly more sophisticated code may compensate for the difference between a motor on port 1 or 10 and the others, see this post for details on that.

predicted drive, so what is that?

Ok, so the predicted drive is the open loop (that is, using no feedback) control value that will make the flywheel go at the required velocity under “normal” conditions. The best way to figure out these numbers is to turn off the velocity controller (but keeping the velocity calculation running), send the motor values from the debugger until you achieve the correct velocity, note that control value and turn it into a normalized drive by dividing by 127.

For example, lets say you wanted to find the predicted drive for my flywheel setup for a velocity of 120rpm. Start with a control value of perhaps 40, look at motor_velocity variable in the debugger window and see what it reads, it will probably be too low so start increasing the motor control value. For my flywheel the motors needed a value of about 48 to make them run at 120rpm, divide this by 127 to get 0.377, this becomes the predicted drive. It’s the open loop value that worked under those test conditions, from this point on the closed loop control will compensate if the conditions change, battery voltage dropping etc.

Perhaps I will add something to the code to make this test easier.

2 Likes

In easy C the function block “timer” as well as an “assignment” block can be used to find the time elapsed right?

As with robotC I understand you use delta_ms = nSysTime - nSysTime_last to find the time.

I’m just trying to see how this would be done in easyC thanks.

This can be implemented in EasyC but will need the use of a repeating timer or other techniques typical for an environment that does not provide an RTOS. I am not able to discuss any details of an EasyC implementation.

2 Likes

Must the gain be specifically 0.00025?

Or is it just that you have tested and found that 0.00025 is the optimal value?

Gains for Take Back Half controllers are almost always extremely low. This is because it is essentially the I from PID coupled with code that “fixes things” when it overshoots.

The gain is the one value that you will need to experiment with to find the correct value. The number I used in the code was the best value for my flywheel setup (my specific gearing etc.) with certain other requirements that I had, mostly to do with the maximum current I wanted to use during the flywheel acceleration period. It could probably have been a little larger, this would have reduced the time needed to stabilize the flywheel at the target speed.

2 Likes

I’ve been trying to modify this code to work with two encoders in a 2 flywheel system. I can’t figure out why the Motor_FWL starts and runs continuously as soon as the program starts.

It doesn’t seem to be any physical wire connection because I can swap motor names and IME ports in the motors and sensors setup window and the opposite side starts running constantly. I can change to a stable program and it works fine. I’m sure it’s a problem in my code changes, but I can’t seem to track down the problem.

Any suggestions?

``````#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port6,           Motor_FWR,     tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port7,           Motor_FWL,     tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

// Update inteval (in mS) for the flywheel control loop
#define FW_LOOP_SPEED              25

// Maximum power we want to send to the flywheel motors
#define FW_MAX_POWER              127

// encoder counts per revolution depending on motor
#define MOTOR_TPR_269           240.448
#define MOTOR_TPR_393R          261.333
#define MOTOR_TPR_393S          392
#define MOTOR_TPR_393T          627.2

// encoder tick per revolution
float           ticks_per_rev;          ///< encoder ticks per revolution

// Encoder
long            encoder_countsR;         ///< current encoder count
long            encoder_counts_lastR;    ///< current encoder count
long            encoder_countsL;
long            encoder_counts_lastL;

// velocity measurement
float           motor_velocityR;         ///< current velocity in rpm
float           motor_velocityL;
long            nSysTime_lastR;          ///< Time of last velocity calculation
long            nSysTime_lastL;

// TBH control algorithm variables
long            target_velocityR;        ///< target_velocity velocity
long            target_velocityL;
float           current_errorR;          ///< error between actual and target_velocity velocities
float           current_errorL;          ///< error between actual and target_velocity velocities
float           last_errorR;             ///< error last time update called
float           last_errorL;
float           gain;                   ///< gain
float           driveR;                  ///< final drive out of TBH (0.0 to 1.0) RIGHT
float           driveL;                  /// LEFT
float           drive_at_zeroR;          ///< drive at last zero crossing RIGHT
float           drive_at_zeroL;          ///LEFT
long            first_crossR;            ///< flag indicating first zero crossing RIGHT
long            first_crossL;            /// LEFT
float           drive_approxR;           ///< estimated open loop drive RIGHT
float           drive_approxL;           ///LEFT

// final motor drive
long            motor_driveR;            ///< final motor control value RIGHT
long            motor_driveL;            /// LEFT

/*Set the flywheen motors RIGHT  */
void
FwMotorSetR( int valueR )
{
motor Motor_FWR ] = valueR;
}

/*Set the flywheen motors LEFT */
void
FwMotorSetL( int valueL )
{
motor Motor_FWL ] = valueL;
}

/*Get the flywheen motor encoder count RIGHT*/
long
FwMotorEncoderGetR()
{
return( nMotorEncoder Motor_FWR ] );
}

/*Get the flywheen motor encoder count LEFT */
long
FwMotorEncoderGetL()
{
return( nMotorEncoder Motor_FWL ] );
}

/*Set the controller position RIGHT */
void
FwVelocitySetR( int velocityR, float predicted_driveR )
{
// set target_velocity velocity (motor rpm)
target_velocityR = velocityR;

// Set error so zero crossing is correctly detected
current_errorR = target_velocityR - motor_velocityR;
last_errorR    = current_errorR;

// Set predicted open loop drive value
drive_approxR  = predicted_driveR;
// Set flag to detect first zero crossing
first_crossR   = 1;
// clear tbh variable
drive_at_zeroR = 0;
}

/*Set the controller position LEFT  */
void
FwVelocitySetL( int velocityL, float predicted_driveL )
{
// set target_velocity velocity (motor rpm)
target_velocityL = velocityL;

// Set error so zero crossing is correctly detected
current_errorL = target_velocityL - motor_velocityL;
last_errorL    = current_errorL;

// Set predicted open loop drive value
drive_approxL  = predicted_driveL;
// Set flag to detect first zero crossing
first_crossL   = 1;
// clear tbh variable
drive_at_zeroL = 0;
}

/*Calculate the current flywheel motor velocity*/
void
FwCalculateSpeed()
{
int     delta_msR;
int     delta_msL;
int     delta_encR;
int     delta_encL;

// Get current encoder value
encoder_countsR = FwMotorEncoderGetR();
encoder_countsL = FwMotorEncoderGetL();

// This is just used so we don't need to know how often we are called
// how many mS since we were last here
delta_msR = nSysTime - nSysTime_lastR;
nSysTime_lastR = nSysTime;
delta_msL = nSysTime - nSysTime_lastL;
nSysTime_lastL = nSysTime;

// Change in encoder count
delta_encR = (encoder_countsR - encoder_counts_lastR);
delta_encL = (encoder_countsL - encoder_counts_lastL);

// save last position
encoder_counts_lastR = encoder_countsR;
encoder_counts_lastL = encoder_countsL;

// Calculate velocity in rpm
motor_velocityR = (1000.0 / delta_msR) * delta_encR * 60.0 / ticks_per_rev;
motor_velocityL = (1000.0 / delta_msL) * delta_encL * 60.0 / ticks_per_rev;
}

/*Update the velocity tbh controller variables RIGHT*/
void
FwControlUpdateVelocityTbhR()
{
// calculate error in velocity
// target_velocity is desired velocity
// current is measured velocity
current_errorR = target_velocityR - motor_velocityR;

// Calculate new control value
driveR =  driveR + (current_errorR * gain);

// Clip to the range 0 - 1.
// We are only going forwards
if( driveR > 1 )
driveR = 1;
if( driveR < 0 )
driveR = 0;

// Check for zero crossing
if( sgn(current_errorR) != sgn(last_errorR) ) {
// First zero crossing after a new set velocity command
if( first_crossR ) {
// Set drive to the open loop approximation
driveR = drive_approxR;
first_crossR = 0;
}
else
driveR = 0.5 * ( driveR + drive_at_zeroR );

// Save this drive value in the "tbh" variable
drive_at_zeroR = driveR;
}

// Save last error
last_errorR = current_errorR;
}

/*Update the velocity tbh controller variables */
void
FwControlUpdateVelocityTbhL()
{
// calculate error in velocity
// target_velocity is desired velocity
// current is measured velocity
current_errorL = target_velocityL - motor_velocityL;

// Calculate new control value
driveL =  driveL + (current_errorL * gain);

// Clip to the range 0 - 1.
// We are only going forwards
if( driveL > 1 )
driveL = 1;
if( driveL < 0 )
driveL = 0;

// Check for zero crossing
if( sgn(current_errorL) != sgn(last_errorL) ) {
// First zero crossing after a new set velocity command
if( first_crossL ) {
// Set drive to the open loop approximation
driveL = drive_approxL;
first_crossL = 0;
}
else
driveL = 0.5 * ( driveL + drive_at_zeroL );

// Save this drive value in the "tbh" variable
drive_at_zeroL = driveL;
}

// Save last error
last_errorL = current_errorL;
}

/*Task to control the velocity of the flywheel */
{
// Set the gain
gain = 0.00025;

// We are using Speed geared motors
// Set the encoder ticks per revolution
ticks_per_rev = MOTOR_TPR_393S;

while(1)
{
// Calculate velocity
FwCalculateSpeed();

// Do the velocity TBH calculations
FwControlUpdateVelocityTbhR() ;
FwControlUpdateVelocityTbhL() ;

// Scale drive into the range the motors need
motor_driveR  = (driveR * FW_MAX_POWER) + 0.5;
motor_driveL  = (driveL * FW_MAX_POWER) + 0.5;

// Final Limit of motor values - don't really need this
if( motor_driveR >  127 ) motor_driveR =  127;
if( motor_driveR < -127 ) motor_driveR = -127;
if( motor_driveL >  127 ) motor_driveL =  127;
if( motor_driveL > -127 ) motor_driveL = -127;

// and finally set the motor control value
FwMotorSetR( motor_driveR );
FwMotorSetL( motor_driveL );

// Run at somewhere between 20 and 50mS
wait1Msec( FW_LOOP_SPEED );
}
}

{
// Start the flywheel control task

// Main user control loop
while(1)
{
// Different speeds set by buttons
if( vexRT Btn8L ] == 1 )
{
FwVelocitySetR( 144, 0.55 );
FwVelocitySetL( 144, 0.55 );
}
if( vexRT Btn8U ] == 1 )
{
FwVelocitySetR( 120, 0.38 );
FwVelocitySetL( 120, 0.38 );
}
if( vexRT Btn8R ] == 1 )
{
FwVelocitySetR( 50, 0.2 );
FwVelocitySetL( 50, 0.2 );
}
if( vexRT Btn8D ] == 1 )
{
FwVelocitySetR( 00, 0 );
FwVelocitySetL( 00, 0 );
}

// Don't hog the cpu :)
wait1Msec(10);
}
}``````

Typo

``````
if( motor_driveR >  127 ) motor_driveR =  127;
if( motor_driveR < -127 ) motor_driveR = -127;
if( motor_driveL >  127 ) motor_driveL =  127;
if( motor_driveL > -127 ) motor_driveL = -127;
``````

Should be

``````
if( motor_driveL < -127 ) motor_driveL = -127;

``````

Also, using the original version of the code that used a structure would have been beneficial, but no matter, this will work (I did not test anything else btw, may still have other bugs).

2 Likes

Fantastic!!! THANK YOU, THANK YOU, THANK YOU!

I spent all day looking for that silly little typo! Gah!
I can’t say it was time wasted though, I really learned a lot about coding in RobotC, and a little about the debug window, and a few other lessons.
I tried combining the code if a few different ways thinking it was the order or how or the tasks and functions were called.

I’ve got a feeling if I can distill a little of this new knowledge to the students it will make a real difference to their robot and their knowledge of RobotC and programming.

1 Like

Hi James,

One of my teams was trying to implement your TBH method tonight and we came across a small mistake (we think). Below is a snippet of your code:

We are running turbo motors and found it wasn’t working properly. We did a little searching and found that it appears you have the regular and turbo ticks per revolution to be backwards.

The regular speed speed should be 627.2 while the turbo should be 261.333.

We discovered this because our calculated velocity was not making sense, we traced it back to the above problem.

Anyways, we only wanted to mention it here in case anyone else is having issues with this. We are very very grateful for your tutorial. My programmer and myself learned A LOT of things tonight thanks to this thread.

I assume James most likely meant “T” to stand for “torque”, “S” to stand for “speed”, and “R” to stand for “turbo”.

Oh, that could very well be. We interpreted it as R=regular S=speed T=turbo. But what you are saying is logical as well.

Either way, we greatly appreciate James’ TBH tutorial.

A similar comment came up here and Jpearman mentioned that people had misinterpreted his code and added comments.

https://vexforum.com/showpost.php?p=393307&postcount=4

Yes, that’s correct.

In the days before turbo gears I always used T for torque and S for speed gearing. When turbo gears were introduces I needed to update the smart motor library and my warped mind decoded to use R for tuRbo and leave the others as they were before.

Unfortunately this is the second time my shorthand has been interpreted incorrectly, perhaps I had better update the TBH code with more verbose definitions.

2 Likes

I stumbled upon this question that instead of Take Back Half, why not Take Back Quarter?

I re-read the code and found this line:

``````
drive = 0.5* ( drive + drive_at_zero );

``````

Am I right to assume that the 0.5 in the line is the “half” in Take Back Half?

If that is the case, what is the effects of adjusting the “0.5”, to let’s say, “0.25”?

Would that be better to set encoder value to zero before use?