David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
l3g.cpp
- Committer:
- DavidEGrayson
- Date:
- 2015-04-15
- Revision:
- 47:cb5c1504c24d
- Parent:
- 44:edcacba44760
File content as of revision 47:cb5c1504c24d:
// Code for reading from the L3GD20H gyro with I2C. #include <mbed.h> #include "l3g.h" #include "pc_serial.h" I2C i2c(p9, p10); 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]); }