Note: Vex Gyro (revised 3/2/2013)

Revised based upon more accurate information for Vex people.

I bought a Vex Gyro at Worlds and decided to try it out with RobotC.

The gyro chip is a MEMS ST  LY3100ALH  1000 deg/sec max rate gyro.   The Vex people amplified the output by a factor of 1.511 so that the nominal 3 V max is 4.533 v.   Don’t know why they didn’t amplify it to 5v … but it results in a gyro scale factor of 600 deg/s/volt.   The cortex equation that converts A2D to dps can be derived as follows:

Vgyro = (dps*.0011 + 1.5) * 1.511 = dps* .00166 + 2.267 volts

A2D = Vgyro*2/3*4095/3.3 = Vgyro*827.27 counts

or  A2D = (dps*.00166 + 2.267)*827.27

or A2D = 1.374 * dps + 1874

The inverse gives

dps = (A2D-1874)*.7277

Setting the Gyro Deadzone:

The noise density is .016 deg/s/√(HZ) and the bandwidth is 140 hz so the noise output   = .016*√140 = .19 dps rms.    The A/D sampling resolution is  1 bit  from above is  .728 dps.  The standard deviation of the sampling noise is typically about .289 LSB  or .21 dps.   RSS of these two sources is  .283 dps.   Typically we set the deadzone at the  2 sigma level of the total noise sources or .57 deg/s.  This  is less than 1 count on the A/D output so the dz should be placed at 1 counts  to keep the gyro integration from drifting.   I use 2 counts to add some margin.   Without this deadzone you would expect to see a random walk drift on the integration of the gyro angle when sitting still.

3.2.2013  RobotC 3.08 revisions

“Fix drift in gyro value calculation. Make “nGyroJitterRange” and “bHasSensorBiasSettings” #define parameters in “loadBuildOptions.h”. Change bias setting for gyro from 130 to 131. Fix some small bugs in gyro bias calculation. Introduce a “gyro jitter range” setting that ignores very small changes for gyro steady state position. Suppose gyro bias in 1840; they the ‘ignore jitter’ would treat values in 1837 to 1843 as if they were the steady state value.”

Ok..so it looks like the 3.08 RobotC dead zone is +-3 counts…. or about +-2.2 deg/sec.  My 3.58 RobotC now shows the dead zone at 4 counts now or 2.8 deg/sec.

Recent testing of programming robot have shown drift rates of .6 deg/min even with the jitter range that RobotC claims to use.

Corrected 1/4/2012

When constantly turning with rates that exceed the deadzone,  the rms rate noise contributes about

 sqrt(Time*dt) *.283 degs rms

angle drift where dt is the gyro integration step period.     If one updates at 100 hz then the rms drift is sqrt(Time)*.0283  So with a 1 minute programming challenge time we would expect around  .22 deg rms of drift.   Which is not too bad.

Bias Calibration Time

How long should we average the bias before starting the integration to get angle.   Using an average to estimate of the bias sees the standard error drop by 1/√N where N is the number of samples.   We want enough samples to drive the standard error down to where it doesn’t contribute significantly to the total drift rate after 60 second programming challenge.   If we pick a 10% increase in the total drift error then we can tolerate 1 deg error/60 sec.  From this we can calculate N

1/60 = 1/√N *.283

so N = 288

So we can generally sample at 1 per ms so a 1 second averaging time should be sufficient.

Vex Pendulum Robot:

I wanted to use this with the accelerometer to reconstruct the Vex bbot inverse pendulum robot… however it will be still noisy in tilt angle due to the low resolution of the gyro.   My last bbot used a 400 dps max  with a 10 bit A/D so with the Cortex 12 bit A/D and 1000 deg/sec max we would not see any improvement.  I was hoping for a variable scaling gyro that would allow me to use 100 to 200 deg/s maximum for this application.

Test Results:

The RobotC version 2.32 gyro type did not work with this gyro yet.   I wrote a  RobotC gyro test software that used the A/D to measure the voltage and then process it with my own task process_gyro(). Running this process calculates an unbiased rate gyro output and gyro angle from integrated rate.     The gyro worked pretty well with a 500 hz integration rate and a 1 second initialization time.   Typically a rotation to 180 deg and back was within 5 degs.  There is noticable cross axis sensitivity and also sensitivity to EMI noise so you don’t want to mount this on your processor.

For those interested here is the program:

#pragma config(Sensor, in1,    gyro,                sensorPotentiometer)
//*!!Code automatically generated by ‘ROBOTC’ configuration wizard               !!*//
//Program author:  Chris Siegert
//Vamfun@yahoo.com
//Mentor Vex Team 599 and 1508a
//Global Variables
//The gyro board scales the 3 volt chip to 4.533v with gain factor 1.511
//this gives 600 dps/volt scale at board
const int gyro_init_time_ms = 1000; //ms
const float sf_gyro = 600.*5./4095.;  //units dps_per_cnt = 600 dps/volt*5volt/4095cnts
const int gyro_dz = 2 ;// dead zone in a/d counts (approx .67 deg_per_sec per count)
const int gyro_int_step_size = 2; // integer ms step size 500 hz nominal
//Chip Vdd = 3v  and Nominal gyro voltage output = 1.5 v -> 4095*/3.3*2/3*1.5*1.511. = 1874 cnts
int gyro_bias = 1874;  //nominally 1874 cnts
int gyro_angle = 0; //gyro angle degs  1 deg resolution
int rate_cnts=0; //unbiased gyro rate a/d counts
int gyro_rate_x10 = 0;//unbiased gyro rate deg/sec*10

//Tasks
task Process_gyro()
{
float cum = 0;
//………………. initialize gyro …………………………….
time1[T1]= 0;
int count = 0;
while(time1[T1]< gyro_init_time_ms)
{
count++ ;
wait1Msec(1);// this gives about 1 count per ms to average
cum = cum + (float)(SensorValue[gyro]);

}
gyro_bias =  cum/count;
cum = 0;

//………………. integrate gyro……………………………….
while(true)
{
int dt = time1[T1];
rate_cnts = SensorValue[gyro]-gyro_bias;
gyro_rate_x10 =10*sf_gyro*(float)rate_cnts;

if(dt >=gyro_int_step_size)
{
float error = SensorValue[gyro]-gyro_bias;

if(error>gyro_dz || error< -gyro_dz)
{
cum = cum + dt*error*sf_gyro/1000.;  //integrate if |error| > dz
}
ClearTimer(T1); //reset timer after use rate pulse
}
gyro_angle= cum;//scale to 1 deg/sec resolution
}
}
task main()
{

StartTask (Process_gyro);

}

Advertisements

11 Responses to Note: Vex Gyro (revised 3/2/2013)

  1. Victor says:

    Hi,Thanks for this useful code I don´t understand it to full extend but my question here is how can I use it with a function that ocuppies the value of the gyroscope, I mean wich of the global variables you declared can I use to compensate the error of the gyro in an example code where I wan’t to turn “n” degrees.

  2. vamfun says:

    Your question is a bit unclear to me…
    The global varaibles of interest are

    gyro_rate_x10 (gyro rate output uncompensated for gyro bias)
    error (gyro rate output compensated for gyro bias)
    gyro_angle (integrated gyro rate in degs)

    Example:
    If the gyro is measuring turn rate and you are steering to a heading reference then you would feedback a heading_error to the motors.

    heading _error = reference – gyro_angle
    motor_command = Kp * (heading_error)

    where Kp is the position gain of a PID loop.

    If I misunderstood the quesion…please elaborate more.

  3. Victor says:

    Well my question mainly is the proper use of your code so the drift of the gyro doesn’t affect too much a path I’d like the robot to follow for a minute.

    I’d like to make a function where I can monitor the value of the gyroscope with the drift compensation and use it to compare it in an if statemente with something like this:

    desired_position= actual_position_gyro-desired_degrees
    while(gyro_value!=desired_position){
    if(compensated_gyro_valuedesired_position)
    motor[right]=80;
    motor[left]=-80;
    }

    I wanted to use it as a condition so the robot turns until it get to the position it will get when turning the desired degrees comparing it with the gyro compensation.

    I got confused with the PID loop, are you referring that “motor_command=Kp*heading_error”, will change the speed of the motors?

  4. Victor says:

    Excuse me, what I meant to say in the second paragraph is I’d like to amke a function where I can use a monitored value (in this case the gyro compensation)

  5. Victor says:

    desired_position= actual_position_gyro-desired_degrees
    while(compensated_gyro!=desired_position){
    if(compensated_gyro<desired_position)
    motor[right]= 80;
    motor[left]= -80;
    }
    motor[right]=motor[left]=0;

    • vamfun says:

      Victor .. There is still some confusion. I don’t know what you mean by compensated_gyro. There is only acutal_gyro_angle which is the integration of the unbiased rate coming from the rate gyro. If this is what you mean by compensated_gyro then there is no actual_position_gyro. Also your code will only work for turns in one direction.

      Here is some pseudo code that will do what you want similar to the example PID loop I gave you before but using fixed motor commands. Assumes that positive heading is to the right and that positive motor commands cause the robot to move forward. This loop stops when the heading error is less than 2 deg. This keeps the motors from limit cycling back and forth when the error gets toward zero.

      while(true)
      {
      heading_error = reference - heading_angle;
      If ( heading_error > 2)
      {
      motor[right] = -80; // turn right
      motor[left] = 80 ;
      }
      else if (heading_error < -2)
      {
      motor[right] = 80 ; // turn left
      motor[left] = -80 ;
      }
      else
      {
      motor[right] = motor[left] = 0; //stop turning
      break;
      }
      }//end while loop

      If you would like to learn a little more about PID loops go to the Freerange PID Tutorial link I posted under RobotC links. Its well written and easy to follow.

      Here is the PID version (position only) of the above loop:
      Kp = 2;
      while(true)
      {
      heading_error = reference - heading_angle;
      If ( abs(heading_error) > 2)

      {
      motor[right] = - Kp*heading_error ;
      motor[left] = Kp*heading_error ;
      }
      else
      {
      motor[right] = motor[left] = 0; //stop turning
      break;
      }
      }//end while loop

      • Victor says:

        Sorry for my last post, I didn’t have time to read it when you edited it, thanks for this last explanation, I got a better idea on how to use the code you wrote on a function, I’ll definitely have a read on the PID link you suggest me, thanks for your time on explaining me on how I could use it, I’ll try to implement it and post any feedback results that could be useful, thanks again 🙂

  6. Victor says:

    Ok I think I have a bit more clear the idea, so if I understand correctly in : “heading_error =reference-heading_angle;”
    “reference” is the current position of the gyro wich is the variable gyro_angle, and if the error is great than 2 the motors will run at 80, after that I got a little confused with the next statement since It misses an operator “else if (heading_error 2)”, when are the motors going to be proportional to the error.
    Another thing I’d like to ask is what default speed should be good to start a turn? Or with the proportional speed (KP*heading_error) and gyro_angle I should’nt worry about the default speed to have a more precise turn.

  7. vamfun says:

    You said…“reference” is the current position of the gyro wich is the variable gyro_angle”
    No sir….
    reference is a fixed heading angle that you desire to go to.
    heading_angle is the global variable which is the current heading…ie the variable gyro_angle defined in my gyro code.

    Re your confusion..try to reread the comment… I corrected some errors and you might have read the first draft.

    If you are using Vex motors then the speed of 80 will give you almost maximum turn rate and you will likely over shoot if you are using the fixed motor command loop. Your overshoots will be less with the proportional Kp loop since as the error decreases so does the motor command hence the heading will approach the reference almost asymtotically.

  8. Iron Devils says:

    Hi, I know this is a very old form, but I recently purchased a vex pro yaw rate gyro and have been having some trouble with it. It is extremely irritating that they scaled the gyro output and did not release any sort of data sheet to document it. From your math above, it looks like you are saying that the scaled V/degree/second should be 0.00166. Is this correct? Also what is A2D?

  9. […] This vamfun blog post from 2013 has some specific calculations that the author did concerning drift that more advanced readers may find helpful. I admit that since I have not dived that deeply into trying to calculate anything specific in this realm, I cannot attest to its helpfulness in 2017. […]

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

%d bloggers like this: