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@42:96671b71aac5, 2019-07-27 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Sat Jul 27 20:58:46 2019 +0000
- Revision:
- 42:96671b71aac5
- Parent:
- 40:6fa672be85ec
Calibrate L3G using a buffer of 1000 zero-rate readings. Measured a drift of -1.850 degrees over 3 minutes (-0.01 degree per second).
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 40:6fa672be85ec | 1 | // Code for reading from the L3GD20H gyro with I2C. |
DavidEGrayson | 40:6fa672be85ec | 2 | |
DavidEGrayson | 40:6fa672be85ec | 3 | #include <mbed.h> |
DavidEGrayson | 40:6fa672be85ec | 4 | #include "l3g.h" |
DavidEGrayson | 40:6fa672be85ec | 5 | #include "pc_serial.h" |
DavidEGrayson | 40:6fa672be85ec | 6 | |
DavidEGrayson | 40:6fa672be85ec | 7 | I2C i2c(p9, p10); |
DavidEGrayson | 40:6fa672be85ec | 8 | |
DavidEGrayson | 42:96671b71aac5 | 9 | const int address = 0xD6; |
DavidEGrayson | 40:6fa672be85ec | 10 | |
DavidEGrayson | 40:6fa672be85ec | 11 | int32_t l3gInit() |
DavidEGrayson | 40:6fa672be85ec | 12 | { |
DavidEGrayson | 40:6fa672be85ec | 13 | int whoami = l3gReadReg(0x0F); |
DavidEGrayson | 40:6fa672be85ec | 14 | if (whoami != 0xD7) |
DavidEGrayson | 40:6fa672be85ec | 15 | { |
DavidEGrayson | 40:6fa672be85ec | 16 | pc.printf("l3g whoami error: %d\n", whoami); |
DavidEGrayson | 40:6fa672be85ec | 17 | return -1; // wrong ID or no response |
DavidEGrayson | 40:6fa672be85ec | 18 | } |
DavidEGrayson | 40:6fa672be85ec | 19 | |
DavidEGrayson | 40:6fa672be85ec | 20 | // CTRL1: 800 Hz output data rate, |
DavidEGrayson | 40:6fa672be85ec | 21 | // low-pass filter cutoff 100 Hz |
DavidEGrayson | 40:6fa672be85ec | 22 | l3gWriteReg(0x20, 0xFF); |
DavidEGrayson | 40:6fa672be85ec | 23 | |
DavidEGrayson | 40:6fa672be85ec | 24 | // CTRL4: 2000 dps full scale |
DavidEGrayson | 40:6fa672be85ec | 25 | l3gWriteReg(0x23, 0x20); |
DavidEGrayson | 40:6fa672be85ec | 26 | |
DavidEGrayson | 40:6fa672be85ec | 27 | // CTRL5: High-pass filter disabled |
DavidEGrayson | 40:6fa672be85ec | 28 | l3gWriteReg(0x24, 0x00); |
DavidEGrayson | 40:6fa672be85ec | 29 | |
DavidEGrayson | 40:6fa672be85ec | 30 | return 0; // success |
DavidEGrayson | 40:6fa672be85ec | 31 | } |
DavidEGrayson | 40:6fa672be85ec | 32 | |
DavidEGrayson | 40:6fa672be85ec | 33 | int32_t l3gReadReg(char reg) |
DavidEGrayson | 40:6fa672be85ec | 34 | { |
DavidEGrayson | 40:6fa672be85ec | 35 | int result = i2c.write(address, ®, 1); |
DavidEGrayson | 40:6fa672be85ec | 36 | if (result != 0) |
DavidEGrayson | 40:6fa672be85ec | 37 | { |
DavidEGrayson | 40:6fa672be85ec | 38 | return -1; // error |
DavidEGrayson | 40:6fa672be85ec | 39 | } |
DavidEGrayson | 40:6fa672be85ec | 40 | |
DavidEGrayson | 40:6fa672be85ec | 41 | char data; |
DavidEGrayson | 40:6fa672be85ec | 42 | result = i2c.read(address, &data, 1); |
DavidEGrayson | 40:6fa672be85ec | 43 | if (result != 0) |
DavidEGrayson | 40:6fa672be85ec | 44 | { |
DavidEGrayson | 40:6fa672be85ec | 45 | return -2; // error |
DavidEGrayson | 40:6fa672be85ec | 46 | } |
DavidEGrayson | 40:6fa672be85ec | 47 | |
DavidEGrayson | 40:6fa672be85ec | 48 | return data; |
DavidEGrayson | 40:6fa672be85ec | 49 | } |
DavidEGrayson | 40:6fa672be85ec | 50 | |
DavidEGrayson | 40:6fa672be85ec | 51 | int32_t l3gWriteReg(char reg, char value) |
DavidEGrayson | 40:6fa672be85ec | 52 | { |
DavidEGrayson | 40:6fa672be85ec | 53 | char data[2]; |
DavidEGrayson | 40:6fa672be85ec | 54 | data[0] = reg; |
DavidEGrayson | 40:6fa672be85ec | 55 | data[1] = value; |
DavidEGrayson | 40:6fa672be85ec | 56 | int result = i2c.write(address, data, 2); |
DavidEGrayson | 40:6fa672be85ec | 57 | if (result != 0) |
DavidEGrayson | 40:6fa672be85ec | 58 | { |
DavidEGrayson | 40:6fa672be85ec | 59 | return -1; // error |
DavidEGrayson | 40:6fa672be85ec | 60 | } |
DavidEGrayson | 40:6fa672be85ec | 61 | |
DavidEGrayson | 40:6fa672be85ec | 62 | return 0; |
DavidEGrayson | 40:6fa672be85ec | 63 | } |
DavidEGrayson | 40:6fa672be85ec | 64 | |
DavidEGrayson | 40:6fa672be85ec | 65 | int32_t l3gZAvailable() |
DavidEGrayson | 40:6fa672be85ec | 66 | { |
DavidEGrayson | 40:6fa672be85ec | 67 | // Read STATUS |
DavidEGrayson | 40:6fa672be85ec | 68 | int32_t result = l3gReadReg(0x27); |
DavidEGrayson | 40:6fa672be85ec | 69 | if (result < 0) |
DavidEGrayson | 40:6fa672be85ec | 70 | { |
DavidEGrayson | 40:6fa672be85ec | 71 | return result; |
DavidEGrayson | 40:6fa672be85ec | 72 | } |
DavidEGrayson | 40:6fa672be85ec | 73 | |
DavidEGrayson | 40:6fa672be85ec | 74 | // Read the ZDA bit. |
DavidEGrayson | 40:6fa672be85ec | 75 | return result >> 2 & 1; |
DavidEGrayson | 40:6fa672be85ec | 76 | } |
DavidEGrayson | 40:6fa672be85ec | 77 | |
DavidEGrayson | 40:6fa672be85ec | 78 | int32_t l3gZRead() |
DavidEGrayson | 40:6fa672be85ec | 79 | { |
DavidEGrayson | 40:6fa672be85ec | 80 | char reg = 0x80 | 0x2C; // OUT_Z_L, with slave address updating |
DavidEGrayson | 40:6fa672be85ec | 81 | int result = i2c.write(address, ®, 1); |
DavidEGrayson | 40:6fa672be85ec | 82 | if (result != 0) |
DavidEGrayson | 40:6fa672be85ec | 83 | { |
DavidEGrayson | 40:6fa672be85ec | 84 | return -500001; |
DavidEGrayson | 40:6fa672be85ec | 85 | } |
DavidEGrayson | 40:6fa672be85ec | 86 | |
DavidEGrayson | 40:6fa672be85ec | 87 | char data[2]; |
DavidEGrayson | 40:6fa672be85ec | 88 | result = i2c.read(address, data, 2); |
DavidEGrayson | 40:6fa672be85ec | 89 | if(result != 0) |
DavidEGrayson | 40:6fa672be85ec | 90 | { |
DavidEGrayson | 40:6fa672be85ec | 91 | return -500002; |
DavidEGrayson | 40:6fa672be85ec | 92 | } |
DavidEGrayson | 40:6fa672be85ec | 93 | |
DavidEGrayson | 40:6fa672be85ec | 94 | return (int16_t)(data[1] << 8 | data[0]); |
DavidEGrayson | 42:96671b71aac5 | 95 | } |
DavidEGrayson | 42:96671b71aac5 | 96 | |
DavidEGrayson | 42:96671b71aac5 | 97 | // The stuff below doesn't actually have anything to do with the L3G. |
DavidEGrayson | 42:96671b71aac5 | 98 | |
DavidEGrayson | 42:96671b71aac5 | 99 | #define L3G_CAL_COUNT_MAX 1000 |
DavidEGrayson | 42:96671b71aac5 | 100 | int8_t l3gCalBuffer[L3G_CAL_COUNT_MAX]; |
DavidEGrayson | 42:96671b71aac5 | 101 | uint32_t l3gCalCount = 0; |
DavidEGrayson | 42:96671b71aac5 | 102 | uint32_t l3gCalReplayIndex = 0; |
DavidEGrayson | 42:96671b71aac5 | 103 | |
DavidEGrayson | 42:96671b71aac5 | 104 | int32_t l3gCalibrate() |
DavidEGrayson | 42:96671b71aac5 | 105 | { |
DavidEGrayson | 42:96671b71aac5 | 106 | int32_t reading = l3gZRead(); |
DavidEGrayson | 42:96671b71aac5 | 107 | |
DavidEGrayson | 42:96671b71aac5 | 108 | if (l3gCalCount < L3G_CAL_COUNT_MAX) |
DavidEGrayson | 42:96671b71aac5 | 109 | { |
DavidEGrayson | 42:96671b71aac5 | 110 | int8_t c; |
DavidEGrayson | 42:96671b71aac5 | 111 | if (reading > 127) { c = 127; } |
DavidEGrayson | 42:96671b71aac5 | 112 | else if (reading < -128) { c = -128; } |
DavidEGrayson | 42:96671b71aac5 | 113 | else { c = reading; } |
DavidEGrayson | 42:96671b71aac5 | 114 | l3gCalBuffer[l3gCalCount++] = c; |
DavidEGrayson | 42:96671b71aac5 | 115 | } |
DavidEGrayson | 42:96671b71aac5 | 116 | |
DavidEGrayson | 42:96671b71aac5 | 117 | return reading; |
DavidEGrayson | 42:96671b71aac5 | 118 | } |
DavidEGrayson | 42:96671b71aac5 | 119 | |
DavidEGrayson | 42:96671b71aac5 | 120 | bool l3gCalibrateDone() |
DavidEGrayson | 42:96671b71aac5 | 121 | { |
DavidEGrayson | 42:96671b71aac5 | 122 | return l3gCalCount >= L3G_CAL_COUNT_MAX; |
DavidEGrayson | 42:96671b71aac5 | 123 | } |
DavidEGrayson | 42:96671b71aac5 | 124 | |
DavidEGrayson | 42:96671b71aac5 | 125 | void l3gCalibrateReset() |
DavidEGrayson | 42:96671b71aac5 | 126 | { |
DavidEGrayson | 42:96671b71aac5 | 127 | l3gCalCount = 0; |
DavidEGrayson | 42:96671b71aac5 | 128 | } |
DavidEGrayson | 42:96671b71aac5 | 129 | |
DavidEGrayson | 42:96671b71aac5 | 130 | int32_t l3gZReadCalibrated() |
DavidEGrayson | 42:96671b71aac5 | 131 | { |
DavidEGrayson | 42:96671b71aac5 | 132 | int8_t zeroRate = 0; |
DavidEGrayson | 42:96671b71aac5 | 133 | if (l3gCalCount) |
DavidEGrayson | 42:96671b71aac5 | 134 | { |
DavidEGrayson | 42:96671b71aac5 | 135 | if (l3gCalReplayIndex >= l3gCalCount) { l3gCalReplayIndex = 0; } |
DavidEGrayson | 42:96671b71aac5 | 136 | zeroRate = l3gCalBuffer[l3gCalReplayIndex++]; |
DavidEGrayson | 42:96671b71aac5 | 137 | } |
DavidEGrayson | 42:96671b71aac5 | 138 | |
DavidEGrayson | 42:96671b71aac5 | 139 | return l3gZRead() - zeroRate; |
DavidEGrayson | 40:6fa672be85ec | 140 | } |