David Grayson / Mbed 2 deprecated LineFollowing

David's line following code from the LVBots competition, 2015.

Dependencies:   GeneralDebouncer Pacer PololuEncoder mbed

Fork of DeadReckoning by David Grayson

l3g.cpp@57:99bec7fab454, 2015-04-16 (annotated)

Committer:
DavidEGrayson
Date:
Thu Apr 16 22:00:15 2015 +0000
Revision:
57:99bec7fab454
Parent:
44:edcacba44760 
Doubled the encoder counts for indicating the end of the course because we might have to start a little bit back from the finish line.

Who changed what in which revision?

UserRevisionLine numberNew 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, &reg, 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, &reg, 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 }