David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
Diff: l3g.cpp
- Revision:
- 42:96671b71aac5
- Parent:
- 40:6fa672be85ec
diff -r 3ead1dd2cc3a -r 96671b71aac5 l3g.cpp --- a/l3g.cpp Thu Jul 25 03:20:41 2019 +0000 +++ b/l3g.cpp Sat Jul 27 20:58:46 2019 +0000 @@ -6,7 +6,7 @@ I2C i2c(p9, p10); -int address = 0xD6; +const int address = 0xD6; int32_t l3gInit() { @@ -92,4 +92,49 @@ } return (int16_t)(data[1] << 8 | data[0]); +} + +// The stuff below doesn't actually have anything to do with the L3G. + +#define L3G_CAL_COUNT_MAX 1000 +int8_t l3gCalBuffer[L3G_CAL_COUNT_MAX]; +uint32_t l3gCalCount = 0; +uint32_t l3gCalReplayIndex = 0; + +int32_t l3gCalibrate() +{ + int32_t reading = l3gZRead(); + + if (l3gCalCount < L3G_CAL_COUNT_MAX) + { + int8_t c; + if (reading > 127) { c = 127; } + else if (reading < -128) { c = -128; } + else { c = reading; } + l3gCalBuffer[l3gCalCount++] = c; + } + + return reading; +} + +bool l3gCalibrateDone() +{ + return l3gCalCount >= L3G_CAL_COUNT_MAX; +} + +void l3gCalibrateReset() +{ + l3gCalCount = 0; +} + +int32_t l3gZReadCalibrated() +{ + int8_t zeroRate = 0; + if (l3gCalCount) + { + if (l3gCalReplayIndex >= l3gCalCount) { l3gCalReplayIndex = 0; } + zeroRate = l3gCalBuffer[l3gCalReplayIndex++]; + } + + return l3gZRead() - zeroRate; } \ No newline at end of file