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

t**ask 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);

}

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.

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.

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?

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)

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;

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

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 🙂

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.

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.

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?

[…] 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. […]