Note: Using a Line Sensor to Update Gyro Heading

February 28, 2013

Using Line Sensor to Update Gyro Heading

The programming challenge for the 2013 Sack Attack game was attempted by team 1508 LancerBots fully autonomous robot. It uses gyro information to generate a navigation solution and then automatically sequences through several way points to solve the route sequences. Unfortunately as mentioned in earlier posts, the gyro drift causes heading corruption which then causes sacks to by slightly missed. One possible solution I am proposing is to use line sensors to update the gyro.
Figure 1 shows the geometry and formulas required to accomplish the update for a special case when the line is parallel to the truth x axis.    The coordinates used by 1508 were specifically chosen so all lines were parallel to either the x or y  axis.  This simplifies the update equations.

Simply put… when a line sensor event occurs, note the position error  with respect to the line (using the Nav coordinates) and divide it by the range from the origin.    This ratio is the gyro drift error in radians.

Error Contributions:  

Errors in the update equation can stem from errors in  the Nav initial condition (<.25 in, 1 deg heading) , encoder scaling (typically <1%) , encoder resolution(< .03 in) , line location measurements (<.25 in), line tape width (<.5 in) and Navigation solution numerical errors (sampling, round off).

The dominant error is the encoder scaling which can add up when the encoder has moved several hundred inches.   Lines can also be use to update the Nav  position solution so that these  errors remain less than 1% distance  from the field origin and not the total distance traveled by an encoder.   This error translates to about .6 deg of heading error from a gyro update.    So we should not expect much better than that from our updates.

General Case: Lines skew to the coordinate axes

To complete this note,  lets consider a more general line that is described by

c = a*x + b*y

where a,b and c are constants and again x and y are truth coordinates.   Now we have three unknowns (x,y and delta) and three equations:

x’ = x*cos(delta) + y*sin(delta)

y’ = -x*sin(delta) + y*cos(delta)

c = a*x + b*y

First we solve for x and y from the first two equations which is a simple inverse:

x = x’*cos(delta) – y’*sin(delta)

y = x’*sin(delta) + y’*cos(delta)

Make the small angle substitution gives

x = x’ – y’*delta

y = x’*delta + y’

Now substitute x and y into the line equation

c = a*(x’ – y’*delta) + b*(x’*delta + y’)

Now solve for delta

c -a*x’ – b*y’ = (b*x’ – a*y’)* delta



delta = (c -a*x’ – b*y’ )/(b*x’ – a*y’)

Check the special case above :  y = c, a = 0, b = 1

delta = (y – y’)/(x’)  : checks ok


1508 LancerBots reach world top 40 in drive skills at Robodox skills challenge

February 26, 2013

Funny thing happened on the way to qualify for worlds in the programming challenge… the robot  won the Robodox driver challenge instead with our programmer Gevorg as the driver.  Go figure.   Lancerbots brought three robots to the competition  but only one of them was working so the programming robot did double duty as both driver and programming entry.   The drive skill score was 205 which puts it in the top 40 world rankings but you need to be in the top 30 to qualify.  Being in the top 5 of the California robots qualifies it for the California State Championships on March 16th in Santa Clara near San Jose.

I helped the kids construct a solid programming robot that was very close to being qualified for worlds…except for a pesky sack that caused the robot to not complete its route. (robot video)   The robot is a very efficent spatula robot that can pick up and dump a load of 16 sacks over its back.   It is designed with encoders  , line sensors and gyro sensors that are used to provide full navigation ( two axis position and speed as well as  heading).   The line sensors are used to update the navigation solution.

The field geometry is coded by 4 way points  which are at the intersection of  the crossing white lines used for line tracking.   The waypoints  are used to automatically steer toward using a waypoint tracker algorithm that nulls cross track distance and has heading inner loop.   It makes the programming simple and straight forward and works great so long as the gyro doesn’t drift too much.   The gyro is used to generate heading as a compass would.   We know it is not a great sensor to use as a compass because it does drift at about 6 deg/min which is enough to mess you up on the last legs of the route.   But we thought we would give it a try since it is a random phenomonon and sometimes it works great..if you are lucky.       We have some ideas to make the compass heading more reliable by using  a gyro rate deadzone and also blending the gyro with additional encoder data.

I think the robot is capable of competing at the world class level and probably could score 250 points (pending compass problem fixes and optimization).   The world record currently stands at 185 points.   Had it completed its route on Saturday, the score would have matched the current world record.  Woulda shoulda coulda…frustrating to put in hours of work and have a darn sack trip you up.

I will publish the software at a future date.   It uses four tasks: Navigation, System Control, Feedback Control and Task Control.   The User and Autonomous functions generate engage commands for automatic functions and provide  relavent parameters.

As an example:   Suppose we want to track a line crossing through a waypoint.   We simply set the waypoint number (a waypoint has a position and heading) , engage the tracker and use a “move until” distance function that stops the robot when it has reached the desired point along the track.   The lateral tracking is done automatically.   Any virtual line on the field can be tracked independent of the white lines given to the programmer to use.

Relavent posts:

Note: Using a line sensor to update gyro heading

Waypoint steering geometry for a mobile robot

Navigation update equations for a two wheeled robot

Waypoint steering geometry for a mobile robot

February 5, 2013

waypoint stearing dia

Fig 1 shows the geometry that I will use in the waypoint steering algorithms that will be discussed in future posts.  The field geometry is North (N) along the y-axis and East (E) along the x-axis.   The heading angle , psi is defined positive clockwise from North as a compass rose.

Waypoint:   The waypoint is defined with both location (X_p, Y_p) and direction psi_p.   The slope of the center track line m is  1/tan(psi_p).

Cross track and Along track Ranges:

The two parameters of interest are the along track range R_y and cross track range , R_x.   The steering is determined by R_x = R*sin(delta_psi) where delta_psi is the angle between a line drawn from the robot to the waypoint and the actual track line.

Derivation of R_x  and  R_y without sin and cos functions:

We know that the track can be expressed as a line that goes through (X_p,Y_p) and has a slope m.   So we can write the track line as  y= mx + y_0 form but we need y_0.    Plugging in for point p gives  y_0  = Y_p – m*X_p .

Hence the line eq  is   0 =  -y +m*x + Y_p – m*X_p.

It can be shown that the distance R_x  between a point  (x_r, y_r)  and a line  a*x + b*y  + c  = 0  is

|R_x|= | (a*x_r+b*y_r +c)|/sqrt(a^2 + b^2)    (see  wikipedia)

where a = m ,  b= -1 ,  c= Y_p – m*X_p ,    m  = 1/tan(psi_p)

This form can lead to division by zero when psi_p = 0 but can be avoided by using the tan(90 – psi_p)  = 1/ m  .  We  divide the starting equation

0 =  -y +m*x + Y_p – m*X_p

by m   to change its form and use it whenever m >1 or psi_p > 45 degrees.

0 = -y/m + x + Y_p/m – X_p.

Now we have new definitions of a,b,c

a = 1, b = -1 /m, c = Y_p/m – X_p  , 1/m = tan(90 – psi_p).

R_y derivation

We can use a similar method to find R_y.

|R_y| =  |(a’*x_r+b’*y_r +c’)|/sqrt(a’^2 + b’^2) )

where we use the perpendicular line to the track passing  through (X_r,Y_r) with slope  m’ =-1/m and the point as (X_p,Y_p).    This leads to

a’ = -1  , b’ = m’ , c’ = Y_r  – m’*X_r  , m’ = -1/m = -tan(psi_p)  .  We use this when psi_p  < 45 degrees and the modified form

a’ = m , b’ = 1 , c’ = -Y_r*m – X_r    for psi_p > 45 degrees.

Making R_x and R_y signed values

By convention, I want displacement to the right as positive R_x.  (X_r,Y_r) will be to the right of the track if  0< delta_psi  < 180 otherwise we change the sign of R_x.

Similarly, we want the along track distance, R_y,  to be positive if we are heading in the direction of the waypoint along the track and we have not reached the waypoint. This occurs when  0<90 – delta_psi < 180 otherwise R_y is negative.

Navigation update equations for a two wheeled robot

January 31, 2013

Navigation update equations for a two wheeled robot

These equations assume that the robot is traveling in a circular path with constant turn rate. This is typical of a robot that has set its motor speeds constant but different for left and right speeds. The nonlinear form involves sin and cos of both the present and last heading and involves division by the change in heading. It is more accurate but can lead to division by zero if the robot is traveling in a straight line.. i. e. it has infinite turn radius. The approximate form which is valid for small heading changes between updates is more robust since it doesn’ t require division by the heading change.

If a gyro is available with heading output then you don’t need to compute heading, otherwise, heading is derived from the difference in encoders.   Usually, the navigation equations are run periodically at around 20 to 50 hz.    Rates are derived by dividing the delta changes in position and heading by the update time.  Velocities can be noisy when the update times are short so often an exponential filter is used to smooth the outputs before use in control laws.

In the figure, psi_1 and psi_2 represent last and present heading values respectively.   These are defined positive counter clockwise.   If your coordinate system is N, E and  positive heading clockwise  from N then you would modify the equations by letting delta E = delta x , delta N = delta y and replace psi_ccw with 90 -psi_cw since we are now defining psi_cw positive to the right of the y axis instead of the x axis.  So  the NE coordinates would look like this:

N = N + (sl + sr)/2 *(cos(psi_cw) – delta_psi*(sin(psi_cw))

E = E  + (sl + sr)/2*(sin(psi_cw)   + delta_psi*(cos(psi_cw))

Proposed method of incorporating PTC resistance into my Vex motor models

November 24, 2012

rev 11/25/2012

The motor model uses two resistances: motor resistance Rm and  series resistance Rs.   These are used to compute time constants and current contstants for generating waveforms for the Hbridge ON phase and OFF phases.

tau_ON = L/(Rm + Rs)

tau_OFF = L/(Rm)

Also the steady state current constants are affected by resistances:

i_ss_ON = (V_battery – V_bemf)/(Rm +Rs)

i_ss_OFF = -(V_diode + Vbemf)/Rm

These constants basically assume that the PTC resistances are incorporated into Rs during the ON phase and negligible for the OFF phase.   Typically, Rs that best fits test data is .3 ohms according to Jpearman.   The Rm  is 1.5 ohms for a 393 and 2.5 for a 269 motor.

Incorporating the PTC requires an understanding of the Vex power distribution schematic shown below:(Click to expand)

The two PTC resistances involved are the Cortex Rptc_c and the motor PTC resistance Rptcm.  They affect the controller voltage and the time constants of the H-bridge.

Voltage Drop: Rptc_c causes a voltage drop between V_battery measuring point and the controller.  The voltage drop is Rptc_c*i_1_5 where i_1_5 is the sum of the current draws from ports 1-5 on the cortex.  These currents have been assumed to be the average currents from each of the motors in the PTC monitor software.   This however is very conservative since the current draw from the battery is zero for a motor during the OFF phase of the PWM controller cycle.      For example, when the motor current of a 393 is at its safe current of .9 amps, the ON phase current is about 0.33 amps or 37% of i_m.

The current model  computes the average current per PWM cycle as

i_avg = i_ss_ON*duty_ON + i_ss_OFF*duty_OFF

where  i_ss_ON*duty_ON  is the average current over a PWM cycle caused by the ON PWM pulse.  This should be used in the computation of the cortex currents rather than the total motor current i_avg.


Vc = V_battery – Rptc_c*i_1_5

should be used as the input voltage to the current model rather than V_battery which we are using now.

Rptc_c for the HR16-400 is only 18 mohms.  Typically the maximum current for a cortex bank is from 3 motors at stall or 4.8*3 = 14.4 amps.   This can lead to a voltage drop of  .26 volts.       One might argue that Rptc_c effects can be ignored.

Time Constants: By inspection from the figure we can see that the series resistances in the controller-motor loop in series with the motor resistance Rm are slightly different for the ON and OFF phases.

R_s_ON = Rptcm + 2*Rwire +  2*Rfet.

R_s_OFF = Rptcm + 2*Rwire + Rfet.   (it has one less FET in its path)

The time constants are :

tau_OFF = L/(Rm + R_s_OFF)

tau_ON = L/(Rm + R_s_ON)

where :

Rwire is the wire resistance down stream of the controller.  The upstream wire resistance is assumed negligible.    1 foot of wire is assumed for each power and ground leg and at 10 mohms per ft, Rwire = 10 mohms.

Rfet is the ON source-drain resistance of the MOSFET.  This is about 20 mohms for both the high and low side MOSFETs.

Rptcm is the motor PTC reference resistance at 25 deg C.   For the 393 Rptcm = 140 mohms and for the 269 Rptcm = 110 mohms.

Plugging in to the above formulas for a 393 motor gives

R_s_OFF = 140 + 20 + 40 = 200 mohms (.2 ohms)

R_s_ON =  140 + 20 + 20 = 180 mohms (.18 ohms)

Since there is only 20 mohms difference and these are added to Rm which is 1500 mohms I would recommend using one time constant:

tau = tau_ON = tau_OFF = L/(Rm + R_s)   where R_s = 200 mohms.

Using a similar argument for the 269 we would have R_s = 170 mohms

Current constants:

The current constants are modified similar to the time constants:

i_ss_ON = (V_c – V_bemf)/(Rm +Rs_on)  …. using V_c rather than V_battery

i_ss_OFF = -(V_diode + Vbemf)/(Rm + Rs_off)

We can also use the approximation Rs_on = Rs_off = Rs  as we did for the time constants.


Having said all this , the differences discussed may or may not be noticible in the results and I wouldn’t be too uncomfortable keeping things the way they are since much of the testing done by Jpearman has shown adequate results with the present model.
Relevant links:

smart_PTC_monitor beta code release

August 31, 2012

Ok, I’ve got the RobotC code used in my smart_PTC_monitor developed to a point where I would let others play with it.   I originally used a single timed iterative loop that ran in the same task as  autonomous and user modes.  I decided to create a separate task that simplifies the user interface so normal programming can occur without any of my constraints.  When done, the user functions are easily inserted into my template to seamlessly add the PTC monitor protection capability.    The program is self documented with liberal commenting but I am sure people will have a lot of questions.   I am putting it out without fully testing all the functions to get early user feedback and if it proves to have utility, I will put out a more mature version that includes user feedback.

Please post your comments to the Vex forum thread where the program is introduced.   Thanks




Below is a copy of the introduction comments in the program:

//*************************************************************************************************************** // smart_PTC_monitor_template.beta.8.27.2012.c   Rev 0

// This program is an open source beta version of a competition template for Sack Attack.  It adds a task // called smart_PTC_monitor which uses current and PTC temperature models to manage a current_limiter function // which can keep hardware PTC fuses in the motors,cortex and power expander from ever tripping.

// Program Author: Chris Siegert aka Vamfun on the Vex forums. // see my blog:  for model details and vex forum threads on subject // email: // Mentor for team 599 Robodox and team 1508 Lancer Bots

// Teams are authorized to freely use this program , however, it is requested that improvements or additions be // shared with the Vex community via the vex forum.  Please acknowledge my work when appropriate.  Thanks

// Scope and limitations: // Program assumes that servo currents are zero.  If servo position is known, then currents can be modeled. // Testing has been limited to 393 motors and 3wire motors, however, the program handles  269 motors. // Periodic updates to the models will be made and posted on the Vex forum as more testing is done. // Program handles the Power Expander PTC when the ports are entered with provided #defines // All other motor ports are automatically configured to calculate the correct currents for each PTC

// Basic Instructions: // Write your program as you normally would.  When done, put your autonomous code in place of my_autonomous() // and your user code in place of my_user().  Then do a find and replace to change all the “motor” functions to // “motor_input” i.e.  motor[Left_motor] –> motor_input[Left_motor].

// Put all your encoder speed calculations into  void compute_speeds() function and assign speeds to M[port].speed variable. // You can read these speeds from the global array whenever you need them.  This loop runs at about 100ms update rate.

// Use a #define or switch to create a // PTC_monitor_engage boolean and set true to engage the monitor.

// Tell the program which ports are used by the Power Expander as shown in the example below: // #define PE_CH1  UNUSED // if used put port number …. i.e. PE_CH1  port1  else put the #define UNUSED

// #define PE_CH2  Right_drive_motor  //use setup names

// #define PE_CH3  port3  //use port name

// #define PE_CH4  1  // use integer = to port number -1.  This is motor port2

// Initialize the PTC temperatures using these #defines: // #define T_0_DEG_F  (72.) //Set robot ambient temperature here.  If you are in a hot gym…this might be 85 deg

// #define T_M  (100.) //100 deg C =Setpoint temperature for the PTC current monitor.

// Motor currents and PTC temperature data are held in a global state matrix M[] which is a structured array that contains // the motor PTC/current data in the first 10 elements (index 0 to 9) and the non motor PTC’s data in the next three elements. // M[10] , M[11] and M[12] pertain to the cortex1_5,cortex6_10 and power expander PTC states respectively.

// Program uses two update rates: The current_limiter updates at PTC_TASK_DELAY_MS which is 15ms // and the temperature calculations are set to run at about 6 times {PTC_TASK_DELAY_MS + subtasks delays) ~= 100ms. //****************************************************************************************************************

Smart software monitor keeps PTC fuses from tripping

August 18, 2012

They say “knowledge is power”.  Well if you give a RobotC program a little extra knowledge about the state of the currents in the motors and the constants that convert currents into PTC temperatures you can make a very smart monitor.  Smart meaning, PTC’s are prevented from ever tripping while maintaining near optimal current capability.

Monitor tutorial and preliminary tests are described in my latest video(click to watch).

Keeping PTC’s from tripping by limiting current is not a new concept and both teams that I have worked with have used a simple version of a monitor that usually operates during Autonomous phase of a match where the robot might accidentally run into trouble and stall motors.   If the stall tripped a PTC, the robot would remain idle for much if not all of the driver segment of the match.  We have used a simple compare on the drive train encoder speed to shut off the move functions when the measured speed did not match a expected speed for some predetermined time.    This works pretty good for that short controlled scenario.   This type of simple monitor doesn’t work too well during driver mode since the speeds vs command don’t reflect the actual temperature state of the PTC which is the only parameter that determines whether or not it trips.    If the PTC has heated up partially an is near tripping, speed cannot be used to effectively sense this so my teams usually inhibit the simple monitors during the driver phase.

With the “noble goal” of making  monitor that “knows the state of the PTC” , I and others have been developing better models of the Vex current (given command and speed) and then using this current in a differential equation that models the thermodynamic response of a PTC to a current input.   These models have been discussed in the Vex forums and are at the point that they can be used to improve existing PTC trip protection software.  I have incorporated these models into a Smart PTC current limiter which  controls a current limiting function that is inserted between the user command and the standard motor[port] function.

The smart monitor performs the following functions:

1) reads the RobotC configuration matrix  and allocates the proper current models based upon the motors being used to feed the 1o motor  and 3 CORTEX/POWER EXPANDER PTC models.

2) initializes the safe currents for each of the motors and for each of the higher level CORTEX/PE PTC’s based upon #defines constants.

3)PTC temperatures are computed and compared against a monitor threshold temperature T_m

4) PTC Temperature Limiting:  The monitor acts as a PTC thermostat  which has a reference setting called T_m.  Under normal operating conditions, the PTC monitor logic is false and the current is limited to the maximum possible.  If the PTC models sense that the temperature T has exceeded the set point T_m, (which is slightly below the critical temperature of the hardware PTC) ,then the monitor logic trips and the current is limited to a “safe_current” .  The safe_current  is just below the PTC hold current where a PTC will never trip.  The PTC temperature stabilizes and the motor runs with limited current until the estimated PTC temperature drops to about 80% of the critical temperature.  At this level, the PTC monitor trip logic is reset and full current capability is restored.

Models used in the monitor are described a some of my recent posts.




RobotC  beta code here.