David Kester / Mbed 2 deprecated GonioTrainer

Device to measure angle and get IMU measurements.

Dependencies:   mbed commands BLE_API nRF51822

Sensors.cpp@5:46947b447701, 2015-06-07 (annotated)

Committer:
dkester
Date:
Sun Jun 07 14:11:26 2015 +0000
Revision:
5:46947b447701
Parent:
4:2a5a08b14539
Child:
6:75263c93daf7
Werking

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 0:1c5088dae6e1 5 Sensors* Sensors::instance = new Sensors();
dkester 0:1c5088dae6e1 6
dkester 3:a3e1a06c486d 7 const int AS5600 = 0x36 << 1;
dkester 3:a3e1a06c486d 8 const int MPU6500 = 0x68 << 1;
dkester 3:a3e1a06c486d 9
dkester 0:1c5088dae6e1 10 Sensors::Sensors()
dkester 0:1c5088dae6e1 11 {
dkester 5:46947b447701 12 i2c.frequency(250000);
dkester 1:b44bd62c542f 13
dkester 3:a3e1a06c486d 14 this->angle_h = 0;
dkester 3:a3e1a06c486d 15 this->angle_l = 0;
dkester 3:a3e1a06c486d 16
dkester 3:a3e1a06c486d 17 this->accel_x_h = 0;
dkester 3:a3e1a06c486d 18 this->accel_y_h = 0;
dkester 3:a3e1a06c486d 19 this->accel_z_h = 0;
dkester 3:a3e1a06c486d 20 this->accel_x_l = 0;
dkester 3:a3e1a06c486d 21 this->accel_y_l = 0;
dkester 3:a3e1a06c486d 22 this->accel_z_l = 0;
dkester 3:a3e1a06c486d 23
dkester 3:a3e1a06c486d 24 this->gyro_x_h = 0;
dkester 3:a3e1a06c486d 25 this->gyro_y_h = 0;
dkester 3:a3e1a06c486d 26 this->gyro_z_h = 0;
dkester 3:a3e1a06c486d 27 this->gyro_x_l = 0;
dkester 3:a3e1a06c486d 28 this->gyro_y_l = 0;
dkester 3:a3e1a06c486d 29 this->gyro_z_l = 0;
dkester 1:b44bd62c542f 30
dkester 4:2a5a08b14539 31
dkester 5:46947b447701 32 //setupIMU();
dkester 0:1c5088dae6e1 33 setupAngle();
dkester 5:46947b447701 34 //disableIMU();
dkester 0:1c5088dae6e1 35 }
dkester 0:1c5088dae6e1 36
dkester 0:1c5088dae6e1 37
dkester 1:b44bd62c542f 38 Sensors* Sensors::getInstance()
dkester 1:b44bd62c542f 39 {
dkester 0:1c5088dae6e1 40 return instance;
dkester 0:1c5088dae6e1 41 }
dkester 0:1c5088dae6e1 42
dkester 3:a3e1a06c486d 43 void Sensors::updateAngle()
dkester 0:1c5088dae6e1 44 {
dkester 0:1c5088dae6e1 45 /* Read angle from AS5600 */
dkester 0:1c5088dae6e1 46 cmd[0] = 0x0E;
dkester 0:1c5088dae6e1 47 i2c.write(AS5600,cmd,1);
dkester 0:1c5088dae6e1 48 i2c.read(AS5600,out,2);
dkester 0:1c5088dae6e1 49
dkester 4:2a5a08b14539 50 this->angle_h = out[0]; //((out[0] << 8) + out[1]) * 0.087912087;
dkester 4:2a5a08b14539 51 this->angle_l = out[1];
dkester 3:a3e1a06c486d 52 }
dkester 3:a3e1a06c486d 53
dkester 4:2a5a08b14539 54 float Sensors::getAngle(){
dkester 4:2a5a08b14539 55 return ((getAngleH() << 8) + getAngleL()) * 0.087912087;
dkester 4:2a5a08b14539 56 }
dkester 4:2a5a08b14539 57
dkester 4:2a5a08b14539 58 char Sensors::getAngleH(){
dkester 3:a3e1a06c486d 59 return this->angle_h;
dkester 3:a3e1a06c486d 60 }
dkester 4:2a5a08b14539 61 char Sensors::getAngleL(){
dkester 3:a3e1a06c486d 62 return this->angle_l;
dkester 0:1c5088dae6e1 63 }
dkester 0:1c5088dae6e1 64
dkester 0:1c5088dae6e1 65 void Sensors::setupAngle()
dkester 0:1c5088dae6e1 66 {
dkester 2:871b5efb2043 67 //TODO
dkester 0:1c5088dae6e1 68 }
dkester 0:1c5088dae6e1 69
dkester 3:a3e1a06c486d 70 void Sensors::wakeIMU(){
dkester 3:a3e1a06c486d 71 writeRegister(0x6B, (readRegister(0x6B) & 0xbf));
dkester 3:a3e1a06c486d 72 }
dkester 3:a3e1a06c486d 73
dkester 3:a3e1a06c486d 74 void Sensors::disableIMU(){
dkester 3:a3e1a06c486d 75 writeRegister(0x6B, (readRegister(0x6B) | 1 << 6));
dkester 3:a3e1a06c486d 76 }
dkester 5:46947b447701 77
dkester 5:46947b447701 78 int8_t Sensors::getFs(){
dkester 5:46947b447701 79 return 8000 / (readRegister(0x19) + 1);
dkester 5:46947b447701 80 }
dkester 3:a3e1a06c486d 81
dkester 0:1c5088dae6e1 82 void Sensors::setupIMU()
dkester 0:1c5088dae6e1 83 {
dkester 2:871b5efb2043 84 //Sets sample rate to 8000/1+79 = 100Hz
dkester 4:2a5a08b14539 85 writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
dkester 2:871b5efb2043 86 //Disable FSync, 256 DLPF
dkester 2:871b5efb2043 87 writeRegister(0x1A,0x00);
dkester 2:871b5efb2043 88
dkester 2:871b5efb2043 89 // Disable gyroscope self tests, scale of 200 degrees/s
dkester 2:871b5efb2043 90 // Disable accelerometer self tests, scale of +-2g, no DHPF
dkester 2:871b5efb2043 91 cmd[0] = 0x1B;
dkester 2:871b5efb2043 92 cmd[1] = 0x00; //bit4 and bit 3 to choose scale range
dkester 2:871b5efb2043 93 cmd[2] = 0x00;
dkester 2:871b5efb2043 94 i2c.write(MPU6500,cmd,3);
dkester 2:871b5efb2043 95
dkester 2:871b5efb2043 96 //Freefall threshold of 0mg and duration limit of 0
dkester 2:871b5efb2043 97 cmd[0] = 0x1D;
dkester 2:871b5efb2043 98 i2c.write(MPU6500,cmd,3);
dkester 2:871b5efb2043 99
dkester 2:871b5efb2043 100 //Motion threshold of 0mg and duration limit of 0
dkester 2:871b5efb2043 101 cmd[0] = 0x21;
dkester 2:871b5efb2043 102 i2c.write(MPU6500,cmd,3);
dkester 2:871b5efb2043 103
dkester 2:871b5efb2043 104 //Disable sensor output FIFO buffer
dkester 2:871b5efb2043 105 writeRegister(0x23,0x00);
dkester 2:871b5efb2043 106
dkester 2:871b5efb2043 107 //Set aux I2C, from 0x24 to 0x36 = 0x00
dkester 2:871b5efb2043 108
dkester 2:871b5efb2043 109 //Setup INT pin and AUX I2C pass through
dkester 2:871b5efb2043 110 cmd[0] = 0x37;
dkester 2:871b5efb2043 111 cmd[1] = 0x02;
dkester 2:871b5efb2043 112 cmd[2] = 0x01;
dkester 2:871b5efb2043 113 i2c.write(MPU6500,cmd,3);
dkester 2:871b5efb2043 114
dkester 2:871b5efb2043 115 //Reset sensor signal paths
dkester 2:871b5efb2043 116 writeRegister(0x68,0x00);
dkester 2:871b5efb2043 117
dkester 2:871b5efb2043 118 //Motion detection control
dkester 2:871b5efb2043 119 writeRegister(0x69,0x00);
dkester 2:871b5efb2043 120
dkester 2:871b5efb2043 121 //Disables FIFO, AUX I2C and I2C reset bits to 0
dkester 2:871b5efb2043 122 writeRegister(0x6A,0x00);
dkester 2:871b5efb2043 123
dkester 2:871b5efb2043 124 //Sets clock source to gyro reference w/ PLL
dkester 3:a3e1a06c486d 125 /* SLEEP = 0 */
dkester 2:871b5efb2043 126 writeRegister(0x6B,0x02);
dkester 2:871b5efb2043 127
dkester 2:871b5efb2043 128 //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
dkester 2:871b5efb2043 129 writeRegister(0x6C,0x00);
dkester 2:871b5efb2043 130
dkester 2:871b5efb2043 131 //Data transfer to and from the FIFO buffer
dkester 2:871b5efb2043 132 writeRegister(0x74,0x00);
dkester 2:871b5efb2043 133 }
dkester 0:1c5088dae6e1 134
dkester 0:1c5088dae6e1 135 void Sensors::updateIMU()
dkester 0:1c5088dae6e1 136 {
dkester 3:a3e1a06c486d 137 // Read measurements from MPU6500
dkester 0:1c5088dae6e1 138 cmd[0] = 0x3B;
dkester 0:1c5088dae6e1 139 i2c.write(MPU6500,cmd,1,true);
dkester 0:1c5088dae6e1 140 i2c.read(MPU6500,out,14);
dkester 3:a3e1a06c486d 141
dkester 3:a3e1a06c486d 142 this->accel_x_h = out[0];
dkester 3:a3e1a06c486d 143 this->accel_x_l = out[1];
dkester 3:a3e1a06c486d 144 this->accel_y_h = out[2];
dkester 3:a3e1a06c486d 145 this->accel_y_l = out[3];
dkester 3:a3e1a06c486d 146 this->accel_z_h = out[4];
dkester 3:a3e1a06c486d 147 this->accel_z_l = out[5];
dkester 5:46947b447701 148
dkester 5:46947b447701 149 int16_t temp16 = ((out[6] << 8) | out[7]);
dkester 5:46947b447701 150
dkester 5:46947b447701 151 this->temp = temp16/340 + 36.53;
dkester 3:a3e1a06c486d 152
dkester 3:a3e1a06c486d 153 this->gyro_x_h = out[8];
dkester 3:a3e1a06c486d 154 this->gyro_x_l = out[9];
dkester 3:a3e1a06c486d 155 this->gyro_y_h = out[10];
dkester 3:a3e1a06c486d 156 this->gyro_y_l = out[11];
dkester 3:a3e1a06c486d 157 this->gyro_z_h = out[12];
dkester 3:a3e1a06c486d 158 this->gyro_z_l = out[13];
dkester 3:a3e1a06c486d 159
dkester 0:1c5088dae6e1 160 }
dkester 0:1c5088dae6e1 161
dkester 3:a3e1a06c486d 162 int8_t Sensors::getAccelXH(){
dkester 3:a3e1a06c486d 163 return this->accel_x_h;
dkester 3:a3e1a06c486d 164 }
dkester 3:a3e1a06c486d 165 int8_t Sensors::getAccelXL(){
dkester 3:a3e1a06c486d 166 return this->accel_x_l;
dkester 3:a3e1a06c486d 167 }
dkester 3:a3e1a06c486d 168 int8_t Sensors::getAccelYH(){
dkester 3:a3e1a06c486d 169 return this->accel_y_h;
dkester 0:1c5088dae6e1 170 }
dkester 3:a3e1a06c486d 171 int8_t Sensors::getAccelYL(){
dkester 3:a3e1a06c486d 172 return this->accel_y_l;
dkester 0:1c5088dae6e1 173 }
dkester 3:a3e1a06c486d 174 int8_t Sensors::getAccelZH(){
dkester 3:a3e1a06c486d 175 return this->accel_z_h;
dkester 3:a3e1a06c486d 176 }
dkester 3:a3e1a06c486d 177 int8_t Sensors::getAccelZL(){
dkester 3:a3e1a06c486d 178 return this->accel_z_l;
dkester 0:1c5088dae6e1 179 }
dkester 3:a3e1a06c486d 180
dkester 3:a3e1a06c486d 181
dkester 3:a3e1a06c486d 182 int8_t Sensors::getGyroXH(){
dkester 3:a3e1a06c486d 183 return this->gyro_x_h;
dkester 0:1c5088dae6e1 184 }
dkester 3:a3e1a06c486d 185 int8_t Sensors::getGyroXL(){
dkester 3:a3e1a06c486d 186 return this->gyro_x_l;
dkester 3:a3e1a06c486d 187 }
dkester 3:a3e1a06c486d 188 int8_t Sensors::getGyroYH(){
dkester 3:a3e1a06c486d 189 return this->gyro_y_h;
dkester 0:1c5088dae6e1 190 }
dkester 3:a3e1a06c486d 191 int8_t Sensors::getGyroYL(){
dkester 3:a3e1a06c486d 192 return this->gyro_y_l;
dkester 0:1c5088dae6e1 193 }
dkester 3:a3e1a06c486d 194 int8_t Sensors::getGyroZH(){
dkester 3:a3e1a06c486d 195 return this->gyro_z_h;
dkester 0:1c5088dae6e1 196 }
dkester 3:a3e1a06c486d 197 int8_t Sensors::getGyroZL(){
dkester 3:a3e1a06c486d 198 return this->gyro_z_l;
dkester 3:a3e1a06c486d 199 }
dkester 3:a3e1a06c486d 200
dkester 1:b44bd62c542f 201
dkester 4:2a5a08b14539 202 int16_t Sensors::getAccelX(){
dkester 4:2a5a08b14539 203 return ((getAccelXH() << 8) + getAccelXL());
dkester 4:2a5a08b14539 204 }
dkester 4:2a5a08b14539 205 int16_t Sensors::getAccelY(){
dkester 4:2a5a08b14539 206 return ((getAccelYH() << 8) + getAccelYL());
dkester 4:2a5a08b14539 207 }
dkester 4:2a5a08b14539 208 int16_t Sensors::getAccelZ(){
dkester 4:2a5a08b14539 209 return ((getAccelZH() << 8) + getAccelZL());
dkester 4:2a5a08b14539 210 }
dkester 4:2a5a08b14539 211
dkester 4:2a5a08b14539 212 int16_t Sensors::getGyroX(){
dkester 4:2a5a08b14539 213 return ((getGyroXH() << 8) + getGyroXL());
dkester 4:2a5a08b14539 214 }
dkester 5:46947b447701 215
dkester 5:46947b447701 216 float Sensors::getTemp(){
dkester 5:46947b447701 217 return this->temp;
dkester 5:46947b447701 218 }
dkester 4:2a5a08b14539 219
dkester 1:b44bd62c542f 220 int8_t Sensors::readRegister(int8_t addr)
dkester 1:b44bd62c542f 221 {
dkester 1:b44bd62c542f 222 cmd[0] = addr;
dkester 1:b44bd62c542f 223 i2c.write(MPU6500,cmd,1);
dkester 1:b44bd62c542f 224 i2c.read(MPU6500,out,1);
dkester 1:b44bd62c542f 225 return out[0];
dkester 1:b44bd62c542f 226 }
dkester 1:b44bd62c542f 227
dkester 1:b44bd62c542f 228 void Sensors::writeRegister(int8_t addr, int8_t value)
dkester 1:b44bd62c542f 229 {
dkester 1:b44bd62c542f 230 cmd[0] = addr;
dkester 1:b44bd62c542f 231 cmd[1] = value;
dkester 1:b44bd62c542f 232 i2c.write(MPU6500,cmd,2);
dkester 1:b44bd62c542f 233 }
dkester 1:b44bd62c542f 234
dkester 1:b44bd62c542f 235
dkester 1:b44bd62c542f 236
dkester 1:b44bd62c542f 237
dkester 1:b44bd62c542f 238
dkester 2:871b5efb2043 239
dkester 2:871b5efb2043 240
dkester 2:871b5efb2043 241