David Kester / Mbed 2 deprecated GonioTrainer

Device to measure angle and get IMU measurements.

Dependencies:   mbed commands BLE_API nRF51822

Sensors.cpp@1:b44bd62c542f, 2015-05-20 (annotated)

Committer:
dkester
Date:
Wed May 20 11:45:38 2015 +0000
Revision:
1:b44bd62c542f
Parent:
0:1c5088dae6e1
Child:
2:871b5efb2043
update;

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 0:1c5088dae6e1 7 Sensors::Sensors()
dkester 0:1c5088dae6e1 8 {
dkester 0:1c5088dae6e1 9 i2c.frequency(300000);
dkester 0:1c5088dae6e1 10 this->AS5600 = 0x36 << 1;
dkester 0:1c5088dae6e1 11 this->MPU6500 = 0x68 << 1;
dkester 1:b44bd62c542f 12
dkester 0:1c5088dae6e1 13 this->angle = 0;
dkester 0:1c5088dae6e1 14 this->accel_x = 0;
dkester 0:1c5088dae6e1 15 this->accel_y = 0;
dkester 0:1c5088dae6e1 16 this->accel_z = 0;
dkester 0:1c5088dae6e1 17 this->temp = 0;
dkester 0:1c5088dae6e1 18 this->gyro_x = 0;
dkester 0:1c5088dae6e1 19 this->gyro_y = 0;
dkester 0:1c5088dae6e1 20 this->gyro_z = 0;
dkester 1:b44bd62c542f 21
dkester 0:1c5088dae6e1 22 setupIMU();
dkester 0:1c5088dae6e1 23 setupAngle();
dkester 0:1c5088dae6e1 24 }
dkester 0:1c5088dae6e1 25
dkester 0:1c5088dae6e1 26
dkester 1:b44bd62c542f 27 Sensors* Sensors::getInstance()
dkester 1:b44bd62c542f 28 {
dkester 0:1c5088dae6e1 29 return instance;
dkester 0:1c5088dae6e1 30 }
dkester 0:1c5088dae6e1 31
dkester 0:1c5088dae6e1 32 float Sensors::getAngle()
dkester 0:1c5088dae6e1 33 {
dkester 0:1c5088dae6e1 34 /* Read angle from AS5600 */
dkester 0:1c5088dae6e1 35 cmd[0] = 0x0E;
dkester 0:1c5088dae6e1 36 i2c.write(AS5600,cmd,1);
dkester 0:1c5088dae6e1 37 i2c.read(AS5600,out,2);
dkester 0:1c5088dae6e1 38
dkester 0:1c5088dae6e1 39 angle = ((out[0] << 8) + out[1]) * 0.087912087 ;
dkester 0:1c5088dae6e1 40 return angle;
dkester 0:1c5088dae6e1 41 }
dkester 0:1c5088dae6e1 42
dkester 0:1c5088dae6e1 43 void Sensors::setupAngle()
dkester 0:1c5088dae6e1 44 {
dkester 0:1c5088dae6e1 45 }
dkester 0:1c5088dae6e1 46
dkester 0:1c5088dae6e1 47 void Sensors::setupIMU()
dkester 0:1c5088dae6e1 48 {
dkester 0:1c5088dae6e1 49 /* Setup MPU6500 */
dkester 0:1c5088dae6e1 50 // Get current value of register 0x6B – Power Management 1 PWR_MGMT_1
dkester 0:1c5088dae6e1 51 cmd[0] = 0x6B;
dkester 0:1c5088dae6e1 52 i2c.write(MPU6500,cmd,1);
dkester 0:1c5088dae6e1 53 i2c.read(MPU6500,out,1);
dkester 0:1c5088dae6e1 54 int PW = out[0];
dkester 0:1c5088dae6e1 55
dkester 0:1c5088dae6e1 56 // SLEEP = 1, CYCLE = 0, ClOCK = 001, RESET = 0
dkester 0:1c5088dae6e1 57 cmd[1] = (PW & 0x10) | 0x41; // OR with 0x1 for SLEEP = 0
dkester 0:1c5088dae6e1 58 PW = cmd[1];
dkester 0:1c5088dae6e1 59 i2c.write(MPU6500,cmd,2);
dkester 0:1c5088dae6e1 60
dkester 0:1c5088dae6e1 61 // Set resulution for gyroscope and accelerometer
dkester 0:1c5088dae6e1 62 cmd[0] = 0x1B;
dkester 0:1c5088dae6e1 63 cmd[1] = 0x00;
dkester 0:1c5088dae6e1 64 cmd[2] = 0x00;
dkester 0:1c5088dae6e1 65 i2c.write(MPU6500,cmd,3);
dkester 0:1c5088dae6e1 66
dkester 0:1c5088dae6e1 67 // Set Reset = 0 & Sleep = 0
dkester 0:1c5088dae6e1 68 cmd[0] = 0x6B;
dkester 0:1c5088dae6e1 69 cmd[1] = PW & 0x3F;
dkester 0:1c5088dae6e1 70 i2c.write(MPU6500,cmd,2);
dkester 0:1c5088dae6e1 71 }
dkester 0:1c5088dae6e1 72
dkester 0:1c5088dae6e1 73 void Sensors::updateIMU()
dkester 0:1c5088dae6e1 74 {
dkester 0:1c5088dae6e1 75 /* Read measurements from MPU6500 */
dkester 0:1c5088dae6e1 76 cmd[0] = 0x3B;
dkester 0:1c5088dae6e1 77 i2c.write(MPU6500,cmd,1,true);
dkester 0:1c5088dae6e1 78 i2c.read(MPU6500,out,14);
dkester 0:1c5088dae6e1 79
dkester 0:1c5088dae6e1 80 this->accel_x = (out[0] << 8) + out[1];
dkester 0:1c5088dae6e1 81 this->accel_y = (out[2] << 8) + out[3];
dkester 0:1c5088dae6e1 82 this->accel_z = (out[4] << 8) + out[5];
dkester 0:1c5088dae6e1 83
dkester 0:1c5088dae6e1 84 this->temp = (out[6] << 8) + out[7];
dkester 0:1c5088dae6e1 85
dkester 0:1c5088dae6e1 86 this->gyro_x = (out[8] << 8) + out[9];
dkester 0:1c5088dae6e1 87 this->gyro_y = (out[10] << 8) + out[11];
dkester 0:1c5088dae6e1 88 this->gyro_z = (out[12] << 8) + out[13];
dkester 0:1c5088dae6e1 89 }
dkester 0:1c5088dae6e1 90
dkester 1:b44bd62c542f 91 int16_t Sensors::getAccelX()
dkester 1:b44bd62c542f 92 {
dkester 0:1c5088dae6e1 93 return this->accel_x;
dkester 0:1c5088dae6e1 94 }
dkester 1:b44bd62c542f 95 int16_t Sensors::getAccelY()
dkester 1:b44bd62c542f 96 {
dkester 0:1c5088dae6e1 97 return this->accel_y;
dkester 0:1c5088dae6e1 98 }
dkester 1:b44bd62c542f 99 int16_t Sensors::getAccelZ()
dkester 1:b44bd62c542f 100 {
dkester 0:1c5088dae6e1 101 return this->accel_z;
dkester 0:1c5088dae6e1 102 }
dkester 1:b44bd62c542f 103 float Sensors::getTemp()
dkester 1:b44bd62c542f 104 {
dkester 0:1c5088dae6e1 105 return (this->temp)/340 + 36.53;
dkester 0:1c5088dae6e1 106 }
dkester 1:b44bd62c542f 107 int16_t Sensors::getGyroX()
dkester 1:b44bd62c542f 108 {
dkester 0:1c5088dae6e1 109 return this->accel_x;
dkester 0:1c5088dae6e1 110 }
dkester 1:b44bd62c542f 111 int16_t Sensors::getGyroY()
dkester 1:b44bd62c542f 112 {
dkester 0:1c5088dae6e1 113 return this->gyro_y;
dkester 0:1c5088dae6e1 114 }
dkester 1:b44bd62c542f 115 int16_t Sensors::getGyroZ()
dkester 1:b44bd62c542f 116 {
dkester 0:1c5088dae6e1 117 return this->gyro_z;
dkester 0:1c5088dae6e1 118 }
dkester 1:b44bd62c542f 119
dkester 1:b44bd62c542f 120 int8_t Sensors::readRegister(int8_t addr)
dkester 1:b44bd62c542f 121 {
dkester 1:b44bd62c542f 122 cmd[0] = addr;
dkester 1:b44bd62c542f 123 i2c.write(MPU6500,cmd,1);
dkester 1:b44bd62c542f 124 i2c.read(MPU6500,out,1);
dkester 1:b44bd62c542f 125 return out[0];
dkester 1:b44bd62c542f 126 }
dkester 1:b44bd62c542f 127
dkester 1:b44bd62c542f 128 void Sensors::writeRegister(int8_t addr, int8_t value)
dkester 1:b44bd62c542f 129 {
dkester 1:b44bd62c542f 130 cmd[0] = addr;
dkester 1:b44bd62c542f 131 cmd[1] = value;
dkester 1:b44bd62c542f 132 i2c.write(MPU6500,cmd,2);
dkester 1:b44bd62c542f 133 }
dkester 1:b44bd62c542f 134
dkester 1:b44bd62c542f 135 void Sensors::setDMP(int8_t th)
dkester 1:b44bd62c542f 136 {
dkester 1:b44bd62c542f 137 writeRegister(0x6A, ( readRegister(0x6A) | (1 << 3))); //reset DMP
dkester 1:b44bd62c542f 138 writeRegister(0x6A, ( readRegister(0x6A) | (1 << 7))); //set DMP
dkester 1:b44bd62c542f 139 writeRegister(0x38, ( readRegister(0x38) | (1 << 6))); //enable motion interrupt
dkester 1:b44bd62c542f 140 writeRegister(0x38, ( readRegister(0x38) | (1 << 1))); //enable motion interrupt
dkester 1:b44bd62c542f 141 writeRegister(0x1F, th); //set threshold
dkester 1:b44bd62c542f 142 }
dkester 1:b44bd62c542f 143
dkester 1:b44bd62c542f 144
dkester 1:b44bd62c542f 145 bool Sensors::checkDMP()
dkester 1:b44bd62c542f 146 {
dkester 1:b44bd62c542f 147
dkester 1:b44bd62c542f 148 if (((readRegister(0x3A) >> 6) & 1) == 1) {
dkester 1:b44bd62c542f 149 return true;
dkester 1:b44bd62c542f 150 } else {
dkester 1:b44bd62c542f 151 return false;
dkester 1:b44bd62c542f 152 }
dkester 1:b44bd62c542f 153 }
dkester 1:b44bd62c542f 154
dkester 1:b44bd62c542f 155
dkester 1:b44bd62c542f 156
dkester 1:b44bd62c542f 157