David Kester / Mbed 2 deprecated GonioTrainer

Device to measure angle and get IMU measurements.

Dependencies:   mbed commands BLE_API nRF51822

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

Committer:
dkester
Date:
Wed May 20 08:44:35 2015 +0000
Revision:
0:1c5088dae6e1
Child:
1:b44bd62c542f
Read sensors

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 0:1c5088dae6e1 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 0:1c5088dae6e1 21
dkester 0:1c5088dae6e1 22 setupIMU();
dkester 0:1c5088dae6e1 23 setupAngle();
dkester 0:1c5088dae6e1 24 }
dkester 0:1c5088dae6e1 25
dkester 0:1c5088dae6e1 26
dkester 0:1c5088dae6e1 27 Sensors* Sensors::getInstance(){
dkester 0:1c5088dae6e1 28 return instance;
dkester 0:1c5088dae6e1 29 }
dkester 0:1c5088dae6e1 30
dkester 0:1c5088dae6e1 31 float Sensors::getAngle()
dkester 0:1c5088dae6e1 32 {
dkester 0:1c5088dae6e1 33 /* Read angle from AS5600 */
dkester 0:1c5088dae6e1 34 cmd[0] = 0x0E;
dkester 0:1c5088dae6e1 35 i2c.write(AS5600,cmd,1);
dkester 0:1c5088dae6e1 36 i2c.read(AS5600,out,2);
dkester 0:1c5088dae6e1 37
dkester 0:1c5088dae6e1 38 angle = ((out[0] << 8) + out[1]) * 0.087912087 ;
dkester 0:1c5088dae6e1 39 return angle;
dkester 0:1c5088dae6e1 40 }
dkester 0:1c5088dae6e1 41
dkester 0:1c5088dae6e1 42 void Sensors::setupAngle()
dkester 0:1c5088dae6e1 43 {
dkester 0:1c5088dae6e1 44 }
dkester 0:1c5088dae6e1 45
dkester 0:1c5088dae6e1 46 void Sensors::setupIMU()
dkester 0:1c5088dae6e1 47 {
dkester 0:1c5088dae6e1 48 /* Setup MPU6500 */
dkester 0:1c5088dae6e1 49 // Get current value of register 0x6B – Power Management 1 PWR_MGMT_1
dkester 0:1c5088dae6e1 50 cmd[0] = 0x6B;
dkester 0:1c5088dae6e1 51 i2c.write(MPU6500,cmd,1);
dkester 0:1c5088dae6e1 52 i2c.read(MPU6500,out,1);
dkester 0:1c5088dae6e1 53 int PW = out[0];
dkester 0:1c5088dae6e1 54
dkester 0:1c5088dae6e1 55 // SLEEP = 1, CYCLE = 0, ClOCK = 001, RESET = 0
dkester 0:1c5088dae6e1 56 cmd[1] = (PW & 0x10) | 0x41; // OR with 0x1 for SLEEP = 0
dkester 0:1c5088dae6e1 57 PW = cmd[1];
dkester 0:1c5088dae6e1 58 i2c.write(MPU6500,cmd,2);
dkester 0:1c5088dae6e1 59
dkester 0:1c5088dae6e1 60 // Set resulution for gyroscope and accelerometer
dkester 0:1c5088dae6e1 61 cmd[0] = 0x1B;
dkester 0:1c5088dae6e1 62 cmd[1] = 0x00;
dkester 0:1c5088dae6e1 63 cmd[2] = 0x00;
dkester 0:1c5088dae6e1 64 i2c.write(MPU6500,cmd,3);
dkester 0:1c5088dae6e1 65
dkester 0:1c5088dae6e1 66 // Set Reset = 0 & Sleep = 0
dkester 0:1c5088dae6e1 67 cmd[0] = 0x6B;
dkester 0:1c5088dae6e1 68 cmd[1] = PW & 0x3F;
dkester 0:1c5088dae6e1 69 i2c.write(MPU6500,cmd,2);
dkester 0:1c5088dae6e1 70 }
dkester 0:1c5088dae6e1 71
dkester 0:1c5088dae6e1 72 void Sensors::updateIMU()
dkester 0:1c5088dae6e1 73 {
dkester 0:1c5088dae6e1 74 /* Read measurements from MPU6500 */
dkester 0:1c5088dae6e1 75 cmd[0] = 0x3B;
dkester 0:1c5088dae6e1 76 i2c.write(MPU6500,cmd,1,true);
dkester 0:1c5088dae6e1 77 i2c.read(MPU6500,out,14);
dkester 0:1c5088dae6e1 78
dkester 0:1c5088dae6e1 79 this->accel_x = (out[0] << 8) + out[1];
dkester 0:1c5088dae6e1 80 this->accel_y = (out[2] << 8) + out[3];
dkester 0:1c5088dae6e1 81 this->accel_z = (out[4] << 8) + out[5];
dkester 0:1c5088dae6e1 82
dkester 0:1c5088dae6e1 83 this->temp = (out[6] << 8) + out[7];
dkester 0:1c5088dae6e1 84
dkester 0:1c5088dae6e1 85 this->gyro_x = (out[8] << 8) + out[9];
dkester 0:1c5088dae6e1 86 this->gyro_y = (out[10] << 8) + out[11];
dkester 0:1c5088dae6e1 87 this->gyro_z = (out[12] << 8) + out[13];
dkester 0:1c5088dae6e1 88 }
dkester 0:1c5088dae6e1 89
dkester 0:1c5088dae6e1 90 int16_t Sensors::getAccelX() {
dkester 0:1c5088dae6e1 91 return this->accel_x;
dkester 0:1c5088dae6e1 92 }
dkester 0:1c5088dae6e1 93 int16_t Sensors::getAccelY() {
dkester 0:1c5088dae6e1 94 return this->accel_y;
dkester 0:1c5088dae6e1 95 }
dkester 0:1c5088dae6e1 96 int16_t Sensors::getAccelZ() {
dkester 0:1c5088dae6e1 97 return this->accel_z;
dkester 0:1c5088dae6e1 98 }
dkester 0:1c5088dae6e1 99 float Sensors::getTemp() {
dkester 0:1c5088dae6e1 100 return (this->temp)/340 + 36.53;
dkester 0:1c5088dae6e1 101 }
dkester 0:1c5088dae6e1 102 int16_t Sensors::getGyroX() {
dkester 0:1c5088dae6e1 103 return this->accel_x;
dkester 0:1c5088dae6e1 104 }
dkester 0:1c5088dae6e1 105 int16_t Sensors::getGyroY() {
dkester 0:1c5088dae6e1 106 return this->gyro_y;
dkester 0:1c5088dae6e1 107 }
dkester 0:1c5088dae6e1 108 int16_t Sensors::getGyroZ() {
dkester 0:1c5088dae6e1 109 return this->gyro_z;
dkester 0:1c5088dae6e1 110 }