Waiting 10 ms is far too long. Try waiting for 1 ms instead and see if that helps you.
To be clear about my implementation, I’m actually not doing any filtering on gyro noise while a robot is moving. My code just has an effective model for calibration and preventing gyro drift (with a configurable parameter). Here’s a summarized version of my approach while working with the gyro sensor which should explain how everything works.
There’s a few things to know about the chip and the breakout board that VEX made for the gyro sensor before you can jump in and build a filter for it. Have a look at the datasheet for this chip, it’s important for understanding the sensor scaling: https://content.vexrobotics.com/docs/276-2333-Datasheet-1011.pdf
1. The analog inputs on the Cortex do their ADC on a 5v scale but according to the datasheet of the gyro chip the sensor itself is 3.3v scale. When I was doing the initial work on my filter implementation I determined that there must be some sort of boost circuit on the VEX gyro sensor itself that boosts the 3.3v signal up to 5v for use with the Cortex. Becuase of this boost, you need to scale all your measurements up by a factor of 5v / 3.3v which is about 1.515.
2. There’s two key values we need to look at on the gyro datasheet in order to be able to measure angular rate: the zero rate voltage (that is, what voltage it spits out when there’s no movement) and the sensitivity (that is, the change in volts per degree per second of rotation of the sensor). Remember, these values need to be scaled by our “boost factor” of 1.515. The boosted values should be a zero rate voltage of 1.5v * 1.515 = 2.2725v and a sensitivity (volts per degree per second) of 0.0011v/dps * 1.515 = 0.001667v/dps.
3. We need to convert those voltages from above into ADC values for the Cortex. This is a simple linear interpolation from 0v-5v to 0-4095. So the zero rate value is 2.2725v / 5v * 4095 = about 1861, and the sensitivity in “ticks” per degrees per second is 0.001667(v/dps) / 5v * 4095 = about 1.36527 ticks/dps. I assume you’re able to piece together the rest, but basically a sensor value of 1900 based on these calculations means that the sensor is rotating at a rate of (1900-1861)/1.36527 = about 28.566 degrees per second. You’d then be able to integrate this over time in a loop and get an angle measurement.
4. Now if we measure our raw analog sensor values from the gyro at zero rate, we can find some noise and possibly a different zero rate value that what we calculated. This is why we need to calibrate the sensor. The way I do calibration is by measuring the sensor at rest for a few seconds to get the average value, and set the zero rate value to that average value. We assume with good faith that our sensitivity is constant, because calibrating that would be a difficult task.
5. There’s still the issue of gyro drift. There’s a ton of ways to deal with this, and I happened to choose one that I thought might work well when working on this for my World Championship bots from 2015 and stuck with it, but don’t assume that this is the optimal approach for this sensor, I assure you that it probably isn’t. My approach uses an assumption that the noise is gaussian (that is, you can model the noise distribution with a gaussian, or “normal” model). This assumption allows us to build a model that we can use to determine (with a configurable level of confidence) that any given measurement is noise and should be determined as no movement, or if it is actually movement that should be measured. While calibrating the sensor, we can use the data points that we collected to estimate the standard deviation of the zero-rate measurements by taking the average of their distances from the zero-rate average value that we computed before (read this sentence a few times if it doesn’t make sense right away). For those who have a bit of a statistics background, you know that gaussian distributions can build confidence intervals by using the standard deviation. We can configure a confidence band of n standard deviations around the zero rate average we computed and saved in our calibration routine. Anything in this band is assumed to be noisy zero rate readings, so we can just set them all to the zero rate value, thus reducing or removing gyro drift while not moving.
For the sake of not doubling the size of this post, if you want to see how any of this works in code, go check out my gyro implementation here: https://bitbucket.org/VRCNERD/nerd_robotc/src/79c4c0cec371e822abdc4d1cfef75307a2bd1376/libGyro/NERD_Gyro.c?at=master&fileviewer=file-view-default
If you’re just interested in using my implementation, I can make a post that demonstrates what I believe to be the proper way to measure the gyro data using my lib. I’d suggest putting the measurements in a task that runs as fast as possible basically.
1 Like
Okay, so I think I’m understanding what you are talking about. That really helped me understand what your code is doing and the logic behind it. I’m going to work on something and then report back the results.
Okay, so it’s been quite a while, but I have been trying a bunch of different things. The code I’m using for the filtering isn’t much different from what I had previously other than now I’m only waiting for 1 mSec between measurements. It still only reads about 65 ish degrees when it has turned 90 in reality. I was doing some further experimentation and found that the error is linear. What I mean is that if I turn it 90 degrees and find what percentage of 90 that reading is, then I do that for 180 degrees, and then for 270 degrees, that constant is always very close. As a result, I’m thinking of setting my the gyro value that I will use to the gyroVal * experimentally_determined_constant. Does this sound like something that is okay to do, or is this fundamentally flawed?
Nope, this is common. If you are using RobotC, you can use SensorScale[gyroName] = x; to adjust the scale to match sensor values to actual values. Multiplying your values by a constant is the same.
Okay thanks. I wasn’t sure if that was okay because here we are obtaining the values by reading and interpreting the raw values. Now that I think about it though, it is probably something to do with the variances in manufacturing of these sensors. Thanks a ton!
You’re exactly right; it has to do with the sensors as built, not with how you’re reading them. All sensors have some amount of variation to the stimuli they are designed to measure. Sometimes the manufacturer provides compensation or calibration to get the measurement into a standard range. (temperature sensing diodes, resistive heaters for temperature stabilization and laser-trimmed resistors and capacitors are all typical ways that sensors response is standardized. With this particular sensor board, they didn’t do any on-board compensation or calibration; they’re counting on the user to do that. The variance for this sensor is linear( or close to, within 1 bit of measurement) so that a simple scaling of the output is all you need. What you’ve worked out on your own is pretty much what you need to do.
For a nice description of what’s needed, I usually advise people to read this article:
In particular, read the section labeled “Scaling.”
1 Like
Thanks for that. I read the whole article and it was very useful. Thank you to Renegade Robotics for writing that as well.
I have updated my program, and it seems pretty accurate now. However, I now get extensive drift again. I’m not sure if that has to do with this constant that I’m multiplying by slowly increasing in error as I turn, or if it’s something else. I will do further experimentation and then post my code. Thanks to everyone that has helped me in this new venture. I couldn’t have done it without you all.
I have done lots of testing and now the code is experiencing serious drift. Here is the code that is keeping track of the actual gyro value:
task gyroValCalc {
prevTime = time1[T1];
wait1Msec(1);
currentTime = time1[T1];
gyroValDiff = 0;
gyroRate = gyro_get_rate(mainGyro);
timeDiff = currentTime - prevTime;
timeDiffSeconds = timeDiff / 1000;
while(true) {
//writeDebugStreamLine("%d", gyro_get_rate(mainGyro) );
gyroscopeValue = SensorValue[in1];
currentTime = time1[T1];
gyroRate = gyro_get_rate(mainGyro);
timeDiff = currentTime - prevTime;
timeDiffSeconds = timeDiff / 1000;
gyroValDiff = gyroRate * timeDiffSeconds;
gyroValue += gyroValDiff;
finalGyroValue = gyroValue * mainGyro.gyro_offset_constant;
if(finalGyroValue < 0) {
finalGyroValue = 360 + finalGyroValue; //add because gyroVal is currently negative to reverse that value
gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
else if(finalGyroValue >= 360) {
finalGyroValue = 360 - finalGyroValue;
gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
prevTime = time1[T1];
wait1Msec(1);
}
Everything else is identical to the way that @jmmckinney had it in his online repository. Any suggestions on how to fix this? As far as I can tell, the constant I have is pretty accurate.
This is somewhat difficult to follow without the context of the rest of your code. What are the types for your variables in this task? Do you make sure to run the calibrate routine before you use the sensor? There are pieces of code missing that could be contributing to your issue that would be helpful to see in order to help diagnose the issue.
Also, have you tried testing your sensor on someone else’s code, or have you tried testing your code with another sensor? That could help determine if the issue is programmer error or a bug vs an issue with the hardware itself.
Sorry about that. I do run the calibration routine and it works as expected - I can see the values in the debugger after its completed calibration.
Here’s the complete code:
“GyroFiltering.h”:
#ifndef NERD_GYRO
#define NERD_GYRO
float gyroValue = 0;
float finalGyroValue = 0; //value of the gyro after being multiplied by the constant
int gyroscopeValue;
struct gyro_config{
float std_deviation;
float avg;
float volts_per_degree_per_second;
char gyro_flipped;
};
typedef struct {
struct gyro_config config;
int port_number;
float gyro_offset_constant;
} Gyro;
Gyro mainGyro;
//ignore data within n standard deviations of no motion average
#define GYRO_STD_DEVS 10
#define GYRO_OVERSAMPLE 2
//points or time in mSec that the gyro calibrates for
#define GYRO_CALIBRATION_POINTS 1500
float calibration_buffer [GYRO_CALIBRATION_POINTS];
float gyro_get_rate (Gyro gyro);
/**
* generate calibration data for the gyro by collecting
* zero movement data for reference when reading data later
*
* @param gyro instance of gyro structure
*/
void gyro_calibrate (Gyro gyro){
float raw_average = 0.0;
float std_deviation = 0.0;
//calculate average gyro reading with no motion
for(int i = 0; i < GYRO_CALIBRATION_POINTS; ++i){
float raw = SensorValue [in1];
raw_average += raw;
calibration_buffer * = raw;
delay (1);
}
raw_average /= GYRO_CALIBRATION_POINTS;
gyro.config.avg = raw_average;
//calcuate the standard devation, or the average distance
//from the average on the data read
for (int i = 0; i < GYRO_CALIBRATION_POINTS; ++i)
std_deviation += fabs (raw_average - calibration_buffer *);
std_deviation /= (float) GYRO_CALIBRATION_POINTS;
gyro.config.std_deviation = std_deviation;
/*
* Datasheet from VEX indicates that the sensitivity of the gyro is 1.1mV/dps
* and the cortex ADC for raw analog reads ranges from 0-4095 for 0v-5v
* readings. The gyro is scaled from the nominal 2.7v-3.6v operating range
* that the actual chip has to work on the cortex's 5v logic voltage. The scale multiplier
* value is in the ballpark of 1.515.
*/
gyro.config.volts_per_degree_per_second = 0.0011 * 1.515;
writeDebugStreamLine("Calibration complete");
}
/**
* initialize gyro and run the calibration subroutine
*
* @param gyro instance of gyro structure
* @param port_number the port number of the gyro
*/
void gyro_init (Gyro gyro, int port_number, char gyro_flipped, float gyro_offset_constant) {
gyro.port_number = port_number;
gyro.config.gyro_flipped = gyro_flipped;
gyro_calibrate (gyro);
gyro.gyro_offset_constant = gyro_offset_constant;
}
/**
* calculate filtered gyro rate data, ignoring anything within
* GYRO_STD_DEVS standard deviations of the average gyro
* rate value at zero motion
*`
* @param gyro instance of gyro structure
*
* @return gyro rate, in degrees per second
*/
float gyro_read = 0.0;
int sample_sum = 0;
float gyro_get_rate (Gyro gyro){
sample_sum = 0;
gyro_read = 0.0;
#if defined (GYRO_OVERSAMPLE)
if (GYRO_OVERSAMPLE > 0) {
int n_samples = pow (4, GYRO_OVERSAMPLE);
for (int i = 0; i < n_samples; ++i)
sample_sum += SensorValue[gyroscope];
gyro_read = (float) sample_sum / (float) n_samples;
}
else
gyro_read = SensorValue[gyroscope];
#else
gyro_read = SensorValue[gyroscope];
#endif
//Difference from zero-rate value or the average calibration read
float difference = gyro_read - gyro.config.avg;
//Difference fro zero-rate value, in volts
float gyro_voltage = difference * 5.0 / 4095.0;
if (fabs (difference) > GYRO_STD_DEVS * gyro.config.std_deviation)
if (gyro.config.gyro_flipped)
return -1 * gyro_voltage / gyro.config.volts_per_degree_per_second;
else
return gyro_voltage / gyro.config.volts_per_degree_per_second;
return 0;
}
#endif
/**
*This task will keep track of the gyroscope value using gyro_get_rate()
*/
float prevTime;
float currentTime;
float gyroValDiff;
float gyroRate;
float timeDiff;
float timeDiffSeconds;
task gyroValCalc {
prevTime = time1[T1];
wait1Msec(1);
currentTime = time1[T1];
gyroValDiff = 0;
gyroRate = gyro_get_rate(mainGyro);
timeDiff = currentTime - prevTime;
timeDiffSeconds = timeDiff / 1000;
while(true) {
//writeDebugStreamLine("%d", gyro_get_rate(mainGyro) );
gyroscopeValue = SensorValue[in1];
currentTime = time1[T1];
gyroRate = gyro_get_rate(mainGyro);
timeDiff = currentTime - prevTime;
timeDiffSeconds = timeDiff / 1000;
gyroValDiff = gyroRate * timeDiffSeconds;
gyroValue += gyroValDiff;
finalGyroValue = gyroValue * mainGyro.gyro_offset_constant;
if(finalGyroValue < 0) {
finalGyroValue = 360 + finalGyroValue; //add because gyroVal is currently negative to reverse that value
gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
else if(finalGyroValue >= 360) {
finalGyroValue = 360 - finalGyroValue;
gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
prevTime = time1[T1];
wait1Msec(1);
//writeDebugStreamLine("%d", sample_sum);
}
}
I include the preceding file in the following file - “Gyroscope Filtering.c”:
#pragma config(Sensor, in1, gyroscope, sensorAnalog)
#pragma config(Sensor, dgtl9, rightEnc, sensorQuadEncoder)
#pragma config(Sensor, dgtl11, leftEnc, sensorQuadEncoder)
#pragma config(Motor, port1, BackRight, tmotorVex393HighSpeed_HBridge, openLoop, reversed)
#pragma config(Motor, port2, FrontRight, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port9, FrontLeft, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port10, BackLeft, tmotorVex393HighSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "GyroFiltering.h"
task main()
{
clearTimer(T1);
clearDebugStream();
//writeDebugStreamLine("Gyro is ready");
gyro_init(mainGyro, in1,'t', 1.35480979);
//writeDebugStreamLine("%d", mainGyro.port_number);
//writeDebugStreamLine("%d", mainGyro.config.std_deviation);
//writeDebugStreamLine("%d", SensorValue[gyroscope]);
startTask(gyroValCalc);
//drive('f', 15);
//turnToTarget(90);
while(true) {
motor[FrontRight] = motor[BackRight] = vexRT[Ch2];
motor[FrontLeft] = motor[BackLeft] = vexRT[Ch3];
wait1Msec(20);
/*if(vexRT[Btn8U] == 1) {
writeDebugStreamLine("%f", (180 / gyroValue));
while(vexRT[Btn8U] == 1) {
}
}*/
}
}
As you can see,
mainGyro
is initialized and calibrated before anything begins.
I have not attempted to use this sensor with anyone else’s code, nor have I tried to use a different gyro with this code, but I believe that this sensor is fine because it worked fine when using RobotC’s inbuilt gyro code when I set it up as a gyroSensor. However, I will try using a different sensor later today. **
I tried using the same code with another gyroscope. Almost the same results, so it’s not a problem with the gyro.
I just tried running you code, I’m finding no drift really at all if what you mean by drift is change of gyro angle when it’s not moving.
I added this to the code to debug
char str[32];
sprintf(str,"%.3f %.3f", gyroValue, finalGyroValue );
bLCDBacklight = true;
displayLCDString(0, 1, str);
and changed this code as it didn’t make much sense to me to multiply gyroValue and then divide it again.
finalGyroValue = gyroValue * mainGyro.gyro_offset_constant;
if(finalGyroValue < 0) {
finalGyroValue = 360 + finalGyroValue; //add because gyroVal is currently negative to reverse that value
// gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
else if(finalGyroValue >= 360) {
finalGyroValue = 360 - finalGyroValue;
// gyroValue = finalGyroValue / mainGyro.gyro_offset_constant;
}
and I also had to change the value of mainGyro.gyro_offset_constant because it was creating an incorrect value of finalGyroValue for me, I found that gyroValue was almost correct without any correction. I just plugged in a value of 1.04 without really figuring out where your value came from.
So after doing that, this is what I see 10 minutes after calibration
drifted less than 1 deg
and if I rotate 90 degrees
seems pretty good, value on the right is finalGyroValue which I assume is your output.
1 Like
Here’s my setup, I mount the gyro on square steel block to help get accurate 90 deg turns.
Thank you so much for taking your time to actually test my code.
I probably should have specified though, because when I was saying drift I meant over time of driving it around. When I leave it there, the drift is minimal even over long periods of time. The problem is when I drive it around and turn it in circles several times. That’s what I’m trying to reduce because I’m working on position tracking code. This needs to be very accurate for my purposes.
One of my ideas is that since the constant I multiply by isn’t perfect, it might read 179.5 degrees on a 180 degree turn. Then, if you turn the robot completely around 10 times, it would be 5 degrees off. Keep going and the error would keep increasing. My plan is to try to perfect the constant, but if anyone else has any ideas, I would be very grateful.
For this gyro that seems pretty good to me.
How/where is the gyro mounted on the robot ?
For platform tracking/positional awareness determination, you would normally either use more than one sensor system or perform a synchronization maneuver to synchronize and update your directional heading over time. It would be very unusual to use a single sensor system for that purpose for exactly what you’re seeing here: errors accumulate. For VEX you can sometimes use line sensing or ultrasonic sensors as another sensor system and perform sensor fusion to improve your positional awareness. However, aligning yourself with a synchronization maneuver is easier. One very simple such maneuver in VEX is driving in to a wall to absolutely square the sensor with the world. Then re-zero.
This sensor may never provide the accuracy you’d want for absolute positioning over two minutes while driving around in normal field conditions. This is not a complaint about the sensor; it really does a fine job. You just have to use it cleverly to wring a lot out of it.
It might be worth finding out how close you can get the constant if you believe it is contributing reporting error. That’s easily done. Zero the gyro. Pick up the robot and while holding it gently spin in place. Do this for at least five rotations without bumping or shaking the gyro. Ten would be even better. Gently put the robot down and square it up with its original position. Take the gyro reading. Use the difference in readings to determine whether your constant is correct. If it isn’t, calculate the updated constant using the data you just created.
Note that if you are using the standard gyro stuff in RobotC (I know you aren’t; other people will be) that it is helpful to set SensorFullScale to allow wrapping 12 or so times without sensor rollover.
You shouldn’t drive the robot in a circle to do this; the point is to measure the sensor’s sensitivity to rotation, not how it is affected by mechanical and electrical noise when driving.
And on that point, sometimes it is mechanical and electrical noise which are the greatest contributors to inaccuracy. That’s why @jpearman is asking how you have the sensor mounted. You can work on improving that and get lots of gain in accuracy.
Following that, the biggest gains come from occasionally re-zeroing when you know which way you’re pointing. Then, even a large measurement error can give acceptable results.
1 Like
The gyro is mounted on the most stable part of the robot in my opinion, on rubber links. As far as vibration is concerned, I think that place receives the least amount of excess vibration on the whole robot.
Completely out of curiosity, in industry level work, are the gyros that are used more accurate or is the software they use more advanced, or both?
This is a great idea. I didn’t think about combining multiple sensors to measure rotation. I’m going to try to combine the encoder and gyro to see what I can get.
One more thing that I would like to ask, and I apologize for having so many questions, is some idea on how to handle collisions. Since the gyro reads vibration as movement, when the robot bangs into something, the gyro will read that as a large turn. Even if I add a lot of mass to one point of the robot and put the gyro there, collisions are extremely intense and will inevitable cause a lot of error. The only thing I can think of is observe the pattern of values the gyro gives when I collide the robot with something. Most likely it will be a sudden spike for a very short amount of time, and then when the robot detects something like that, ignore all that data and assume it was nothing.
Mounting away from motors seems to be the most helpful thing, and sometimes to do that you have to mount in odd locations. In addition to rubber links, we’ve used both rubber band slings and anti-slip mat sandwiches to mount them. Both provide good vibration isolation. Maybe more than the rubber links.
Be sure the connector wire isn’t able to transmit vibration into the sensor board; we’ve seen that be a source of vibration transmission a couple of times.
It’s a mixed bag. Gyros like this are used a lot, usually integrated into a small driver board which provides startup, calibration, and filtering. Pretty much the things you’re working on right now, it’s just that you’re doing it on the main processor. Often there will be two sensors of the same type providing input to the driver software, and it will then combine the inputs to derive a single integrated output. Again, exactly the kind of thing you can do if you use more than one VEX gyro.
There are, of course, many sensor packages available that have better accuracy and precision than the VEX gyro, but there’s no magic to them–mostly just good electronics design to eliminate noise and thermal effects. Then they incorporate small processors inside the sensor enclosure to perform filtering and sometimes multi sensor fusion. So instead of a small sensor board, they’re closed modules.
So, if you were designing a new system, you’d use one of those sensors. But lots of inexpensive devices use something exactly like the VEX gyro.
And for some purposes involving safety of human lives, the sensors are way more expensive, and way more reliable. The first programmable gyros I used in that kind of scenario cost more than a new car. They don’t cost that much any more, but $5000 is typical for a man-safety gyro. Space rated gyros can cost a good bit more. Anything, really.
That can work well.
The impulse from a collision is very short-lived. I haven’t ever seen this be a problem with the VEX gyro. The sensor design takes some of that in to account, so it’s not clear to me you have to adjust for it. You should see whether you can trigger an erroneous turn this way. If you can, simple filtering will make it go away.
1 Like