David Grayson / Mbed 2 deprecated DeadReckoning

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@40:6fa672be85ec, 2019-07-25 (annotated)

Committer:
DavidEGrayson
Date:
Thu Jul 25 02:53:34 2019 +0000
Revision:
40:6fa672be85ec
Child:
42:96671b71aac5
Add TurnSensor and L3G code but I am not happy with how the Gyro drifts a degree every few seconds or so.

Who changed what in which revision?

UserRevisionLine numberNew 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 40:6fa672be85ec 9 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, &reg, 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, &reg, 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 40:6fa672be85ec 95 }