David Kester / Mbed 2 deprecated GonioTrainer

Device to measure angle and get IMU measurements.

Dependencies:   mbed commands BLE_API nRF51822

Sensors.cpp@8:c6345e8d607c, 2015-06-11 (annotated)

Committer:
dkester
Date:
Thu Jun 11 20:59:22 2015 +0000
Revision:
8:c6345e8d607c
Parent:
6:75263c93daf7 
working without Iic

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