I again was reading through this thread -
I probably missed something, but has anybody got a code (C-like) in order to
- calibrate the Gyro at the start of the program (until offset doesn't change), using a float GyroOffset
- using that float GyroOffset for calculating the Gyroheading out of SensorHTGyro(port, GyroOffset) ?
I tried it several times by my own, but I slowly dispair, I don't get it managed... :(
(well, it drifting just very slowly, but it's still drifting! :evil: )
(and yes, a voltage correction is still missing!)
Code: Select all
//--------------------------------------------------
// IO functions, sensors
//--------------------------------------------------
#define Menc(a) MotorRotationCount(a)
#define Beep(f,d) PlayTone(f,d)
#define printf1( _x, _y, _format1, _value1) { \
string sval1 = FormatNum(_format1, _value1); \
TextOut(_x, _y, sval1); \
}
const char LCDline[]={56,48,40,32,24,16,8,0};
const string clrln=" ";
inline bool btnhit(){
return ( ButtonPressed(BTN1, false) || ButtonPressed(BTN2, false)
|| ButtonPressed(BTN3, false) || ButtonPressed(BTN4, false));
}
void PressToContinue(char Button) {
string msg;
if (Button==BTNCENTER) msg="press BtnCntr...";
else
if (Button==BTNEXIT) msg="press BtnExit...";
else
if (Button==BTNRIGHT) msg="press BtnRight...";
else
if (Button==BTNLEFT) msg="press BtnLeft...";
printf1(0,LCDline[7],"%s", msg);
while (!ButtonPressed(Button, false)); while (btnhit());
}
//--------------------------------------------------
// math
//--------------------------------------------------
#define min(a,b) (a<b?a:b)
#define max(a,b) (a>b?a:b)
inline long round(float f)
{
if (f>=0) return (f + 0.5);
else return (f - 0.5);
}
inline long ArrayMedianI(int src[], int len)
{
int temp[];
ArraySort(temp, src, NA, NA)
return temp[(len-1)/2];
}
inline void ArrayPushI(long &src[], long _new, int len)
{
for (int i=len; i>0; --i) {src[i]=src[i-1];} // shift up
src[0]=_new;
}
//--------------------------------------------------
// sensors
//--------------------------------------------------
#define GYRO S2
#define COMPASS S3
long GyroHeading, GyroOffset=5.0, GyroValue, GyroIntegral,
CompHeading, CompOffset,
MencHeading ;
//--------------------------------------------------
// sensor calibration
//--------------------------------------------------
void GyroOffsetCalibration() {
int temp;
temp=0; // 1st approximation of Gyro Offset
for (int t=1; t<=200; ++t) {
temp=SensorHTGyro(GYRO, GyroOffset);
Wait(20);
temp+=temp;
}
GyroOffset=GyroOffset+round(temp/200);
Beep(400,100);
}
//------------------------------------------------------------------------------
task GetRotation(){
long gyrodata[3], temp, d=1, GyroCalib=10400;
CompHeading=CompOffset=SensorHTCompass(COMPASS);
GyroOffsetCalibration(); // 1st approximation of Gyro Offset
GyroOffsetCalibration(); // 2nd approximation of Gyro Offset
temp=0;
GyroIntegral=0;
while (d) { // wait until GyroIntegral doesn't change
GyroValue= SensorHTGyro(GYRO, GyroOffset);
GyroIntegral+=GyroValue;
d=abs(temp-GyroIntegral);
temp=GyroIntegral;
Wait(1000);
}
Beep(200,100);
while(1) {
ArrayPushI(gyrodata, SensorHTGyro(GYRO, GyroOffset), 3);
GyroValue= ArrayMedianI(gyrodata, 3);
GyroIntegral+=GyroValue;
GyroHeading=round(GyroIntegral*360/GyroCalib);
if (ButtonPressed(BTNLEFT, false)) { // press BTNLEFT:
GyroIntegral=0; // reset GyroIntegral
while (!btnhit());
// now turn robot 360°
} // then press BTNRIGHT:
if (ButtonPressed(BTNRIGHT, false)) { // reset GyroCalib after 360° turn
GyroCalib=abs(GyroIntegral);
while (!btnhit());
GyroOffsetCalibration();
}
while (GyroHeading>360) GyroHeading-=360;
while (GyroHeading< 0) GyroHeading+=360;
CompHeading=SensorHTCompass(COMPASS);
Wait(20);
}
}
task DisplayValues() {
while (1) {
printf1(0, 56, "mot_B%6d", Menc(OUT_B));
printf1(0, 48, "mot_C%6d", Menc(OUT_C));
printf1(0, 40, "GValu%6d", GyroValue);
printf1(0, 32, "GIntg%6d", GyroIntegral);
printf1(0, 24, "GOffs%6d", GyroOffset);
printf1(0, 16, "MotoH%6d", MencHeading);
printf1(0, 8, "CompH%6d", CompHeading);
printf1(0, 0, "GyroH%6d", GyroHeading);
Wait(10);
}
}
task main(){
SetSensorHTGyro(GYRO);
SetSensorLowspeed(COMPASS);
start DisplayValues;
start GetRotation;
while(1);
}
