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@48:597738b77f77, 2019-08-13 (annotated)

Committer:
DavidEGrayson
Date:
Tue Aug 13 21:21:17 2019 +0000
Revision:
48:597738b77f77
Parent:
42:96671b71aac5 
Changes from before the contest, I think.

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 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, &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 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 }