David's line following code from the LVBots competition, 2015.
Dependencies: GeneralDebouncer Pacer PololuEncoder mbed
Fork of DeadReckoning by
Diff: l3g.cpp
- Revision:
- 44:edcacba44760
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/l3g.cpp Wed Apr 15 19:14:39 2015 +0000 @@ -0,0 +1,95 @@ +// 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]); +} \ No newline at end of file