David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
l3g.cpp@52:05a8e919ddb0, 2015-04-15 (annotated)
- Committer:
- DavidEGrayson
- Date:
- Wed Apr 15 23:56:52 2015 +0000
- Revision:
- 52:05a8e919ddb0
- Parent:
- 44:edcacba44760
It learned the course, holy shit!
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DavidEGrayson | 44:edcacba44760 | 1 | // Code for reading from the L3GD20H gyro with I2C. |
DavidEGrayson | 44:edcacba44760 | 2 | |
DavidEGrayson | 44:edcacba44760 | 3 | #include <mbed.h> |
DavidEGrayson | 44:edcacba44760 | 4 | #include "l3g.h" |
DavidEGrayson | 44:edcacba44760 | 5 | #include "pc_serial.h" |
DavidEGrayson | 44:edcacba44760 | 6 | |
DavidEGrayson | 44:edcacba44760 | 7 | I2C i2c(p9, p10); |
DavidEGrayson | 44:edcacba44760 | 8 | |
DavidEGrayson | 44:edcacba44760 | 9 | int address = 0xD6; |
DavidEGrayson | 44:edcacba44760 | 10 | |
DavidEGrayson | 44:edcacba44760 | 11 | int32_t l3gInit() |
DavidEGrayson | 44:edcacba44760 | 12 | { |
DavidEGrayson | 44:edcacba44760 | 13 | int whoami = l3gReadReg(0x0F); |
DavidEGrayson | 44:edcacba44760 | 14 | if (whoami != 0xD7) |
DavidEGrayson | 44:edcacba44760 | 15 | { |
DavidEGrayson | 44:edcacba44760 | 16 | pc.printf("l3g whoami error: %d\n", whoami); |
DavidEGrayson | 44:edcacba44760 | 17 | return -1; // wrong ID or no response |
DavidEGrayson | 44:edcacba44760 | 18 | } |
DavidEGrayson | 44:edcacba44760 | 19 | |
DavidEGrayson | 44:edcacba44760 | 20 | // CTRL1: 800 Hz output data rate, |
DavidEGrayson | 44:edcacba44760 | 21 | // low-pass filter cutoff 100 Hz |
DavidEGrayson | 44:edcacba44760 | 22 | l3gWriteReg(0x20, 0xFF); |
DavidEGrayson | 44:edcacba44760 | 23 | |
DavidEGrayson | 44:edcacba44760 | 24 | // CTRL4: 2000 dps full scale |
DavidEGrayson | 44:edcacba44760 | 25 | l3gWriteReg(0x23, 0x20); |
DavidEGrayson | 44:edcacba44760 | 26 | |
DavidEGrayson | 44:edcacba44760 | 27 | // CTRL5: High-pass filter disabled |
DavidEGrayson | 44:edcacba44760 | 28 | l3gWriteReg(0x24, 0x00); |
DavidEGrayson | 44:edcacba44760 | 29 | |
DavidEGrayson | 44:edcacba44760 | 30 | return 0; // success |
DavidEGrayson | 44:edcacba44760 | 31 | } |
DavidEGrayson | 44:edcacba44760 | 32 | |
DavidEGrayson | 44:edcacba44760 | 33 | int32_t l3gReadReg(char reg) |
DavidEGrayson | 44:edcacba44760 | 34 | { |
DavidEGrayson | 44:edcacba44760 | 35 | int result = i2c.write(address, ®, 1); |
DavidEGrayson | 44:edcacba44760 | 36 | if (result != 0) |
DavidEGrayson | 44:edcacba44760 | 37 | { |
DavidEGrayson | 44:edcacba44760 | 38 | return -1; // error |
DavidEGrayson | 44:edcacba44760 | 39 | } |
DavidEGrayson | 44:edcacba44760 | 40 | |
DavidEGrayson | 44:edcacba44760 | 41 | char data; |
DavidEGrayson | 44:edcacba44760 | 42 | result = i2c.read(address, &data, 1); |
DavidEGrayson | 44:edcacba44760 | 43 | if (result != 0) |
DavidEGrayson | 44:edcacba44760 | 44 | { |
DavidEGrayson | 44:edcacba44760 | 45 | return -2; // error |
DavidEGrayson | 44:edcacba44760 | 46 | } |
DavidEGrayson | 44:edcacba44760 | 47 | |
DavidEGrayson | 44:edcacba44760 | 48 | return data; |
DavidEGrayson | 44:edcacba44760 | 49 | } |
DavidEGrayson | 44:edcacba44760 | 50 | |
DavidEGrayson | 44:edcacba44760 | 51 | int32_t l3gWriteReg(char reg, char value) |
DavidEGrayson | 44:edcacba44760 | 52 | { |
DavidEGrayson | 44:edcacba44760 | 53 | char data[2]; |
DavidEGrayson | 44:edcacba44760 | 54 | data[0] = reg; |
DavidEGrayson | 44:edcacba44760 | 55 | data[1] = value; |
DavidEGrayson | 44:edcacba44760 | 56 | int result = i2c.write(address, data, 2); |
DavidEGrayson | 44:edcacba44760 | 57 | if (result != 0) |
DavidEGrayson | 44:edcacba44760 | 58 | { |
DavidEGrayson | 44:edcacba44760 | 59 | return -1; // error |
DavidEGrayson | 44:edcacba44760 | 60 | } |
DavidEGrayson | 44:edcacba44760 | 61 | |
DavidEGrayson | 44:edcacba44760 | 62 | return 0; |
DavidEGrayson | 44:edcacba44760 | 63 | } |
DavidEGrayson | 44:edcacba44760 | 64 | |
DavidEGrayson | 44:edcacba44760 | 65 | int32_t l3gZAvailable() |
DavidEGrayson | 44:edcacba44760 | 66 | { |
DavidEGrayson | 44:edcacba44760 | 67 | // Read STATUS |
DavidEGrayson | 44:edcacba44760 | 68 | int32_t result = l3gReadReg(0x27); |
DavidEGrayson | 44:edcacba44760 | 69 | if (result < 0) |
DavidEGrayson | 44:edcacba44760 | 70 | { |
DavidEGrayson | 44:edcacba44760 | 71 | return result; |
DavidEGrayson | 44:edcacba44760 | 72 | } |
DavidEGrayson | 44:edcacba44760 | 73 | |
DavidEGrayson | 44:edcacba44760 | 74 | // Read the ZDA bit. |
DavidEGrayson | 44:edcacba44760 | 75 | return result >> 2 & 1; |
DavidEGrayson | 44:edcacba44760 | 76 | } |
DavidEGrayson | 44:edcacba44760 | 77 | |
DavidEGrayson | 44:edcacba44760 | 78 | int32_t l3gZRead() |
DavidEGrayson | 44:edcacba44760 | 79 | { |
DavidEGrayson | 44:edcacba44760 | 80 | char reg = 0x80 | 0x2C; // OUT_Z_L, with slave address updating |
DavidEGrayson | 44:edcacba44760 | 81 | int result = i2c.write(address, ®, 1); |
DavidEGrayson | 44:edcacba44760 | 82 | if (result != 0) |
DavidEGrayson | 44:edcacba44760 | 83 | { |
DavidEGrayson | 44:edcacba44760 | 84 | return -500001; |
DavidEGrayson | 44:edcacba44760 | 85 | } |
DavidEGrayson | 44:edcacba44760 | 86 | |
DavidEGrayson | 44:edcacba44760 | 87 | char data[2]; |
DavidEGrayson | 44:edcacba44760 | 88 | result = i2c.read(address, data, 2); |
DavidEGrayson | 44:edcacba44760 | 89 | if(result != 0) |
DavidEGrayson | 44:edcacba44760 | 90 | { |
DavidEGrayson | 44:edcacba44760 | 91 | return -500002; |
DavidEGrayson | 44:edcacba44760 | 92 | } |
DavidEGrayson | 44:edcacba44760 | 93 | |
DavidEGrayson | 44:edcacba44760 | 94 | return (int16_t)(data[1] << 8 | data[0]); |
DavidEGrayson | 44:edcacba44760 | 95 | } |