I’d like to be able to tell the exact angle that the robot is facing at a given time. So even if the robot is moving and rotating at the same time, I should be able to tell which way the robot is facing.
We purchased a gyroscope but the error accumulates very quickly in practice and we’re left with unusable values.
How would I go about making the values more accurate? I’ve read about Kalman filters which would use an accelerometer to correct the error. Would this work in practice with a Vex accelerometer and could anyone help me out with the (RobotC) programming for this? I’m a decent programmer but the kalman filter is a bit over my head.
I use the following code for the gyro, it still needs some refinement as the original was written several months ago but I find it adequate for a 3 minute match. Not sure that a kalman filter is really necessary, make sure the gyro has a solid mount, use a short cable to the cortex and keep sources of interference away from it (ie. don’t run near motor wires). The robot should be stationary when the gyro is initialized. This code probably needs ROBOTC 3.50 or later.
Init the gyro as follows, GyroInput is the analog sensor port.
// Gyro init
GyroInit( GyroInput );
Call GyroGetAngle() to get the current gyro value in degrees. Here was some code where I used it in some field centric drive code using mecanum wheels.
void
DriveSystemMecanumTask()
{
int forward, right, turn;
int temp;
float theta;
// Get joystick values
DriveSystemGetControlValues( &forward, &right, &turn );
// Field centric control
if( theDrive.type == Mecanum4MotorFieldCentric )
{
// Get gyro angle in radians
theta = degreesToRadians( GyroGetAngle() );
// rotate coordinate system
temp = forward * cos(theta) - right * sin(theta);
right = forward * sin(theta) + right * cos(theta);
forward = temp;
}
// Send to drive
DriveSystemMecanumDrive( forward, turn, right );
}
Here’s the library.
/*-----------------------------------------------------------------------------*/
/* */
/* */
/*-----------------------------------------------------------------------------*/
/* */
/* Copyright (c) James Pearman */
/* 2012 */
/* All Rights Reserved */
/* */
/*-----------------------------------------------------------------------------*/
/* */
/* Module: GyroLib.c */
/* Author: James Pearman */
/* Created: 2 Oct 2012 */
/* */
/* Revisions: V0.1 */
/* */
/*-----------------------------------------------------------------------------*/
/* */
/* Description: */
/* */
/* Make using the Gyro easier */
/* */
/*-----------------------------------------------------------------------------*/
/* */
// Stop recursive includes
#ifndef __GYROLIB__
#define __GYROLIB__
// Structure to hold global info for the gyro
typedef struct {
tSensors port;
bool valid;
float angle;
float abs_angle;
} gyroData;
static gyroData theGyro = {in1, false, 0.0, 0.0};
/*-----------------------------------------------------------------------------*/
/* */
/* Small debugging function */
/* */
/*-----------------------------------------------------------------------------*/
void
GyroDebug( int displayLine )
{
string str;
if( theGyro.valid )
{
// display current value
sprintf(str,"Gyro %5.1f ", theGyro.angle );
displayLCDString(displayLine, 0, str);
}
else
displayLCDString(displayLine, 0, "Init Gyro.." );
}
/*-----------------------------------------------------------------------------*/
/* */
/* Task that polls the Gyro and calculates the angle of rotation */
/* */
/*-----------------------------------------------------------------------------*/
task GyroTask()
{
int gyro_value;
int gyro_error = 0;
int lastDriftGyro = 0;
float angle;
float old_angle, delta_angle;
long nSysTimeOffset;
// Gyro readings invalid
theGyro.valid = false;
// Cause the gyro to reinitialize (a theory anyway)
SensorType[theGyro.port] = sensorNone;
// Wait 1/2 sec
wait10Msec(50);
// Gyro should be motionless here
SensorType[theGyro.port] = sensorGyro;
// Wait 1/2 sec
wait10Msec(50);
// What is the current system timer
nSysTimeOffset = nSysTime;
// loop forever
while(true)
{
// get current gyro value (deg * 10)
gyro_value = SensorValue[theGyro.port];
// Filter drift when not moving
if( (nSysTime - nSysTimeOffset) > 250 )
{
if( abs( gyro_value - lastDriftGyro ) < 3 )
gyro_error += (lastDriftGyro - gyro_value);
lastDriftGyro = gyro_value;
nSysTimeOffset = nSysTime;
}
// Create float angle, remove offset
angle = (gyro_value + gyro_error) / 10.0;
// normalize into the range 0 - 360
if( angle < 0 )
angle += 360;
// store in struct for others
theGyro.angle = angle;
// work out change from last time
delta_angle = angle - old_angle;
old_angle = angle;
// fix rollover
if(delta_angle > 180)
delta_angle -= 360;
if(delta_angle < -180)
delta_angle += 360;
// store absolute angle
theGyro.abs_angle = theGyro.abs_angle + delta_angle;
// We can use the angle
theGyro.valid = true;
// Delay
wait1Msec( 20 );
}
}
/*-----------------------------------------------------------------------------*/
/* */
/* Initialize the Gyro on the given port */
/* */
/*-----------------------------------------------------------------------------*/
void
GyroInit( tSensors port )
{
theGyro.port = port;
StartTask( GyroTask );
}
/*-----------------------------------------------------------------------------*/
/* */
/* Cause the gyro to be reinitialized by stopping and then restarting the */
/* polling task */
/* */
/*-----------------------------------------------------------------------------*/
void
GyroReinit()
{
StopTask( GyroTask );
StartTask( GyroTask );
}
/*-----------------------------------------------------------------------------*/
/* */
/* Functions to get the public parameters */
/* */
/*-----------------------------------------------------------------------------*/
float
GyroGetAngle()
{
return( theGyro.angle );
}
float
GyroGetAbsAngle()
{
return( theGyro.abs_angle );
}
bool
GyroGetValid()
{
return( theGyro.valid );
}
#endif //__GYROLIB__
@jpearman:
Just wanted to make sure I’m reading your code correctly.
The idea you’re working with is that the gyro’s drift is slow. Thus, if the change in gyro readings is less than 3 of some unit (degrees, I think? Or is that 10ths of a degree?) every quarter-second, ignore that change, as it must be drift.
I’m kinda curious to see how others are compensating for drift. I had written something similar to your code when we got our first gyro, but was wondering if there was a different way…
Are others still experiencing drift with the newer versions of ROBOTC? I was working with our Gyroscopic Sensor earlier with v3.54, and I had absolutely no drift.
I honestly don’t know at the moment, I bounce around between three or four different versions of ROBOTC depending on whether I’m writing code for myself, answering questions or beta testing new versions, it’s really hard to keep track of everything. The code I posted was probably written for V3.04 last year and then revised when V3.51 was released, I do seem to remember removing the drift code at one point but decided it still worked better with it left in. I will take a look later but I think that the last time I used it drift for steady state (no movement) was pretty good but I was still accumulating an error after driving the robot for a while. The team had a joystick button that would reinitialize the gyro if the error had become large during driving but then decided that they did not want to use it during the driver control period anyway so it was turned off.
Edit:
Ok, so I let the gyro sit for 10 minutes with no movement using V3.51 and there was absolutely no drift. The drift monitoring code won’t hurt anything, it will just leave the detected error at zero so there’s no downside to leaving it in other than very slow movement (0.8 deg/sec) would be ignored.
I used this compass in a (non VRC) robot competition, recently. This worked fairly nicely, but I found it was only accurate +/- 2 to 3 degrees. (It would flutter back and forth a bit.) This wasn’t really a problem for me, but could be depending on how much you need a stable reading. If you do decide to go the compass route, make sure you get one with the accuracy you want.
Of course, if you’re in VRC (non-college) you can’t use a compass anyway.
This is a device I’m currently using in a non-robotics application (digital camera metadata) http://tech.yostengineering.com/3-space-sensor/product-family/embedded
Combo compass, gyro and accelerometer with on board processing. It barely works for my application and tends to be influenced by adjacent metal structure over which I have no control. Not legal for High School and would be expensive if sold as an accessory.
I would suggest looking into Complementary filtering which can be just as effective/efficient as Kalman filtering yet MUCH less computationally intensive and MUCH less complicated…
