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
l3g.cpp
- Committer:
- DavidEGrayson
- Date:
- 2019-08-13
- Revision:
- 48:597738b77f77
- Parent:
- 42:96671b71aac5
File content as of revision 48:597738b77f77:
// Code for reading from the L3GD20H gyro with I2C. #include <mbed.h> #include "l3g.h" #include "pc_serial.h" I2C i2c(p9, p10); const int address = 0xD6; int32_t l3gInit() { int whoami = l3gReadReg(0x0F); if (whoami != 0xD7) { pc.printf("l3g whoami error: %d\n", whoami); return -1; // wrong ID or no response } // CTRL1: 800 Hz output data rate, // low-pass filter cutoff 100 Hz l3gWriteReg(0x20, 0xFF); // CTRL4: 2000 dps full scale l3gWriteReg(0x23, 0x20); // CTRL5: High-pass filter disabled l3gWriteReg(0x24, 0x00); return 0; // success } int32_t l3gReadReg(char reg) { int result = i2c.write(address, ®, 1); if (result != 0) { return -1; // error } char data; result = i2c.read(address, &data, 1); if (result != 0) { return -2; // error } return data; } int32_t l3gWriteReg(char reg, char value) { char data[2]; data[0] = reg; data[1] = value; int result = i2c.write(address, data, 2); if (result != 0) { return -1; // error } return 0; } int32_t l3gZAvailable() { // Read STATUS int32_t result = l3gReadReg(0x27); if (result < 0) { return result; } // Read the ZDA bit. return result >> 2 & 1; } int32_t l3gZRead() { char reg = 0x80 | 0x2C; // OUT_Z_L, with slave address updating int result = i2c.write(address, ®, 1); if (result != 0) { return -500001; } char data[2]; result = i2c.read(address, data, 2); if(result != 0) { return -500002; } 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; }