David Kester / Mbed 2 deprecated GonioTrainer

Device to measure angle and get IMU measurements.

Dependencies:   mbed commands BLE_API nRF51822

Sensors.cpp@6:75263c93daf7, 2015-06-07 (annotated)

Committer:
dkester
Date:
Sun Jun 07 21:58:02 2015 +0000
Revision:
6:75263c93daf7
Parent:
5:46947b447701
Child:
8:c6345e8d607c
Working 7 juni

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dkester 0:1c5088dae6e1 1 #include "Sensors.h"
dkester 0:1c5088dae6e1 2
dkester 0:1c5088dae6e1 3 I2C i2c(p29, p28);
dkester 0:1c5088dae6e1 4
dkester 3:a3e1a06c486d 5 const int AS5600 = 0x36 << 1;
dkester 6:75263c93daf7 6 const int MPU6050 = 0x68 << 1;
dkester 3:a3e1a06c486d 7
dkester 0:1c5088dae6e1 8 Sensors::Sensors()
dkester 0:1c5088dae6e1 9 {
dkester 6:75263c93daf7 10 i2c.frequency(100000);
dkester 3:a3e1a06c486d 11
dkester 6:75263c93daf7 12 for(int i = 0; i < 12; i++){
dkester 6:75263c93daf7 13 imuData[i] = 0;
dkester 6:75263c93daf7 14 }
dkester 6:75263c93daf7 15
dkester 6:75263c93daf7 16 angle[0] = angle[1] = 0;
dkester 3:a3e1a06c486d 17
dkester 6:75263c93daf7 18 setupAngle();
dkester 6:75263c93daf7 19 }
dkester 1:b44bd62c542f 20
dkester 6:75263c93daf7 21 void Sensors::setupAngle()
dkester 6:75263c93daf7 22 {
dkester 6:75263c93daf7 23 cmd[0] = 0x0C;
dkester 6:75263c93daf7 24 i2c.write(AS5600,cmd,1);
dkester 6:75263c93daf7 25 i2c.read(AS5600,out,2);
dkester 6:75263c93daf7 26
dkester 6:75263c93daf7 27 cmd[0]= 0x01;
dkester 6:75263c93daf7 28 cmd[1] = out[0];
dkester 6:75263c93daf7 29 cmd[2] = out[1];
dkester 6:75263c93daf7 30 i2c.write(AS5600,cmd,3);
dkester 6:75263c93daf7 31
dkester 6:75263c93daf7 32 }
dkester 4:2a5a08b14539 33
dkester 6:75263c93daf7 34 void Sensors::wakeIMU(){
dkester 6:75263c93daf7 35 writeRegister(0x6B, (readRegister(0x6B) & 0xbf));
dkester 6:75263c93daf7 36 }
dkester 6:75263c93daf7 37
dkester 6:75263c93daf7 38 void Sensors::disableIMU(){
dkester 6:75263c93daf7 39 writeRegister(0x6B, (readRegister(0x6B) | (1 << 6)));
dkester 0:1c5088dae6e1 40 }
dkester 0:1c5088dae6e1 41
dkester 0:1c5088dae6e1 42
dkester 6:75263c93daf7 43 void Sensors::setupIMU()
dkester 1:b44bd62c542f 44 {
dkester 6:75263c93daf7 45 //ResetIMU
dkester 6:75263c93daf7 46 //writeRegister(0x6B, readRegister(0x6B) | (1<<7));
dkester 6:75263c93daf7 47 //wait(0.1);
dkester 6:75263c93daf7 48
dkester 6:75263c93daf7 49
dkester 6:75263c93daf7 50 //Set sample rate to 8000/1+79 = 100Hz
dkester 6:75263c93daf7 51 writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
dkester 6:75263c93daf7 52
dkester 6:75263c93daf7 53 //Disable Frame Synchronization (FSYNC)
dkester 6:75263c93daf7 54 writeRegister(0x1A,0x0);
dkester 6:75263c93daf7 55
dkester 6:75263c93daf7 56 //Gyroscope Configuration
dkester 6:75263c93daf7 57 //bit 4 and 3 is the full scale
dkester 6:75263c93daf7 58 //0 = 250, 1 = 500, 2 = 1000, 3 = 2000
dkester 6:75263c93daf7 59 writeRegister(0x1B,(0x0 << 3));
dkester 6:75263c93daf7 60
dkester 6:75263c93daf7 61 //Accelerometer Configuration
dkester 6:75263c93daf7 62 //bit 4 and 3 is the full scale
dkester 6:75263c93daf7 63 //0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g
dkester 6:75263c93daf7 64 writeRegister(0x1C,(0x3 << 3));
dkester 6:75263c93daf7 65
dkester 6:75263c93daf7 66 //Set Motion Free Fall Threshold
dkester 6:75263c93daf7 67 writeRegister(0x1D,0x01);
dkester 6:75263c93daf7 68
dkester 6:75263c93daf7 69 //Set Motion Free Fall Dur
dkester 6:75263c93daf7 70 writeRegister(0x1E,0xff);
dkester 6:75263c93daf7 71
dkester 6:75263c93daf7 72 //Set Motion Detection Threshold
dkester 6:75263c93daf7 73 writeRegister(0x1F,0x5);
dkester 6:75263c93daf7 74
dkester 6:75263c93daf7 75 //Set Motion Detection Dur
dkester 6:75263c93daf7 76 writeRegister(0x20,0x01);
dkester 6:75263c93daf7 77
dkester 6:75263c93daf7 78 //Set Zero Motion Threshold
dkester 6:75263c93daf7 79 writeRegister(0x21,0xf1);
dkester 6:75263c93daf7 80
dkester 6:75263c93daf7 81 //Set Zero Motion Dur
dkester 6:75263c93daf7 82 writeRegister(0x22,0x01);
dkester 6:75263c93daf7 83
dkester 6:75263c93daf7 84 //Disable FIFO buffer
dkester 6:75263c93daf7 85 writeRegister(0x23,0x00);
dkester 6:75263c93daf7 86
dkester 6:75263c93daf7 87 //Set aux I2C, from 0x24 to 0x36 = 0x00
dkester 6:75263c93daf7 88
dkester 6:75263c93daf7 89 //Setup INT pin and AUX I2C pass through
dkester 6:75263c93daf7 90 writeRegister(0x37,0x02);
dkester 6:75263c93daf7 91
dkester 6:75263c93daf7 92 //Interrupt Enable
dkester 6:75263c93daf7 93 //[7] FF_EN [6] MOT_EN [5] ZMOT_EN [4] FIFO_OFLOW_EN [3] I2C_MST_INT_EN [2] PLL_RDY_INT_EN [1] DMP_INT_EN [0] RAW_RDY_EN
dkester 6:75263c93daf7 94 writeRegister(0x38,0xe1);
dkester 6:75263c93daf7 95
dkester 6:75263c93daf7 96 //Motion detection control
dkester 6:75263c93daf7 97 writeRegister(0x69,(1<<4));
dkester 6:75263c93daf7 98
dkester 6:75263c93daf7 99 //User control
dkester 6:75263c93daf7 100 //[7] DMP_EN [6] FIFO_EN [5] I2C_MST_EN [4] I2C_IF_DIS [3] DMP_RESET [2] FIFO_RESET [1] I2C_MST_RESET [0] SIG_COND_RESET
dkester 6:75263c93daf7 101 writeRegister(0x6A,0x00);
dkester 6:75263c93daf7 102
dkester 6:75263c93daf7 103 //Set power managenemt 1
dkester 6:75263c93daf7 104 //[7] DEVICE_RESET [6] SLEEP [5] CYCLE [3] TEMP_DIS [2:0] CLK_SEL
dkester 6:75263c93daf7 105 writeRegister(0x6B,0x02);
dkester 6:75263c93daf7 106
dkester 6:75263c93daf7 107 //Set power managenemt 2
dkester 6:75263c93daf7 108 writeRegister(0x6C,0x00);
dkester 0:1c5088dae6e1 109 }
dkester 0:1c5088dae6e1 110
dkester 3:a3e1a06c486d 111 void Sensors::updateAngle()
dkester 0:1c5088dae6e1 112 {
dkester 0:1c5088dae6e1 113 /* Read angle from AS5600 */
dkester 0:1c5088dae6e1 114 cmd[0] = 0x0E;
dkester 0:1c5088dae6e1 115 i2c.write(AS5600,cmd,1);
dkester 0:1c5088dae6e1 116 i2c.read(AS5600,out,2);
dkester 3:a3e1a06c486d 117
dkester 6:75263c93daf7 118 angle[0] = out[0];
dkester 6:75263c93daf7 119 angle[1] = out[1];
dkester 2:871b5efb2043 120 }
dkester 0:1c5088dae6e1 121
dkester 0:1c5088dae6e1 122 void Sensors::updateIMU()
dkester 0:1c5088dae6e1 123 {
dkester 0:1c5088dae6e1 124 cmd[0] = 0x3B;
dkester 6:75263c93daf7 125 i2c.write(MPU6050,cmd,1,true);
dkester 6:75263c93daf7 126 i2c.read(MPU6050,out,14);
dkester 3:a3e1a06c486d 127
dkester 6:75263c93daf7 128 imuData[0] = out[0];
dkester 6:75263c93daf7 129 imuData[1] = out[1];
dkester 6:75263c93daf7 130 imuData[2] = out[2];
dkester 6:75263c93daf7 131 imuData[3] = out[3];
dkester 6:75263c93daf7 132 imuData[4] = out[4];
dkester 6:75263c93daf7 133 imuData[5] = out[5];
dkester 5:46947b447701 134
dkester 6:75263c93daf7 135 imuData[6] = out[8];
dkester 6:75263c93daf7 136 imuData[7] = out[9];
dkester 6:75263c93daf7 137 imuData[8] = out[10];
dkester 6:75263c93daf7 138 imuData[9] = out[11];
dkester 6:75263c93daf7 139 imuData[10] = out[12];
dkester 6:75263c93daf7 140 imuData[11] = out[13];
dkester 0:1c5088dae6e1 141 }
dkester 0:1c5088dae6e1 142
dkester 6:75263c93daf7 143 int8_t Sensors::getInterruptStatus(){
dkester 6:75263c93daf7 144 //[7] FF_INT [6] MOT_INT [5] ZMOT_INT [4] FIFO_OFLOW_INT [3] I2C_MST_INT [2] PLL_RDY_INT [1] DMP_INT [0] RAW_RDY_IN
dkester 6:75263c93daf7 145 return readRegister(0x3A);
dkester 0:1c5088dae6e1 146 }
dkester 3:a3e1a06c486d 147
dkester 6:75263c93daf7 148 uint8_t Sensors::getAngle(int i){
dkester 6:75263c93daf7 149 return angle[i];
dkester 6:75263c93daf7 150 }
dkester 3:a3e1a06c486d 151
dkester 6:75263c93daf7 152 int8_t Sensors::getIMU(int i){
dkester 6:75263c93daf7 153 return imuData[i];
dkester 6:75263c93daf7 154 }
dkester 3:a3e1a06c486d 155
dkester 1:b44bd62c542f 156
dkester 1:b44bd62c542f 157 int8_t Sensors::readRegister(int8_t addr)
dkester 1:b44bd62c542f 158 {
dkester 1:b44bd62c542f 159 cmd[0] = addr;
dkester 6:75263c93daf7 160 i2c.write(MPU6050,cmd,1);
dkester 6:75263c93daf7 161 i2c.read(MPU6050,out,1);
dkester 1:b44bd62c542f 162 return out[0];
dkester 1:b44bd62c542f 163 }
dkester 1:b44bd62c542f 164
dkester 1:b44bd62c542f 165 void Sensors::writeRegister(int8_t addr, int8_t value)
dkester 1:b44bd62c542f 166 {
dkester 1:b44bd62c542f 167 cmd[0] = addr;
dkester 1:b44bd62c542f 168 cmd[1] = value;
dkester 6:75263c93daf7 169 i2c.write(MPU6050,cmd,2);
dkester 1:b44bd62c542f 170 }
dkester 1:b44bd62c542f 171
dkester 6:75263c93daf7 172 //http://www.i2cdevlib.com/devices/mpu6050#registers
dkester 6:75263c93daf7 173
dkester 1:b44bd62c542f 174
dkester 1:b44bd62c542f 175
dkester 1:b44bd62c542f 176
dkester 1:b44bd62c542f 177
dkester 2:871b5efb2043 178