David Kester / Mbed 2 deprecated GonioTrainer

Device to measure angle and get IMU measurements.

Dependencies:   mbed commands BLE_API nRF51822

Sensors.cpp@3:a3e1a06c486d, 2015-06-01 (annotated)

Committer:
dkester
Date:
Mon Jun 01 15:41:01 2015 +0000
Revision:
3:a3e1a06c486d
Parent:
2:871b5efb2043
Child:
4:2a5a08b14539
hoi;

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