David Kester / Mbed 2 deprecated GonioTrainer

Device to measure angle and get IMU measurements.

Dependencies:   mbed commands BLE_API nRF51822

Sensors.cpp@2:871b5efb2043, 2015-05-30 (annotated)

Committer:
dkester
Date:
Sat May 30 12:26:02 2015 +0000
Revision:
2:871b5efb2043
Parent:
1:b44bd62c542f
Child:
3:a3e1a06c486d
added goniometer class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dkester 0:1c5088dae6e1 1 #include "Sensors.h"
dkester 0:1c5088dae6e1 2
dkester 2:871b5efb2043 3
dkester 0:1c5088dae6e1 4 I2C i2c(p29, p28);
dkester 0:1c5088dae6e1 5
dkester 0:1c5088dae6e1 6 Sensors* Sensors::instance = new Sensors();
dkester 0:1c5088dae6e1 7
dkester 0:1c5088dae6e1 8 Sensors::Sensors()
dkester 0:1c5088dae6e1 9 {
dkester 0:1c5088dae6e1 10 i2c.frequency(300000);
dkester 0:1c5088dae6e1 11 this->AS5600 = 0x36 << 1;
dkester 0:1c5088dae6e1 12 this->MPU6500 = 0x68 << 1;
dkester 1:b44bd62c542f 13
dkester 0:1c5088dae6e1 14 this->angle = 0;
dkester 0:1c5088dae6e1 15 this->accel_x = 0;
dkester 0:1c5088dae6e1 16 this->accel_y = 0;
dkester 0:1c5088dae6e1 17 this->accel_z = 0;
dkester 0:1c5088dae6e1 18 this->temp = 0;
dkester 0:1c5088dae6e1 19 this->gyro_x = 0;
dkester 0:1c5088dae6e1 20 this->gyro_y = 0;
dkester 0:1c5088dae6e1 21 this->gyro_z = 0;
dkester 1:b44bd62c542f 22
dkester 0:1c5088dae6e1 23 setupIMU();
dkester 0:1c5088dae6e1 24 setupAngle();
dkester 0:1c5088dae6e1 25 }
dkester 0:1c5088dae6e1 26
dkester 0:1c5088dae6e1 27
dkester 1:b44bd62c542f 28 Sensors* Sensors::getInstance()
dkester 1:b44bd62c542f 29 {
dkester 0:1c5088dae6e1 30 return instance;
dkester 0:1c5088dae6e1 31 }
dkester 0:1c5088dae6e1 32
dkester 0:1c5088dae6e1 33 float Sensors::getAngle()
dkester 0:1c5088dae6e1 34 {
dkester 0:1c5088dae6e1 35 /* Read angle from AS5600 */
dkester 0:1c5088dae6e1 36 cmd[0] = 0x0E;
dkester 0:1c5088dae6e1 37 i2c.write(AS5600,cmd,1);
dkester 0:1c5088dae6e1 38 i2c.read(AS5600,out,2);
dkester 0:1c5088dae6e1 39
dkester 0:1c5088dae6e1 40 angle = ((out[0] << 8) + out[1]) * 0.087912087 ;
dkester 0:1c5088dae6e1 41 return angle;
dkester 0:1c5088dae6e1 42 }
dkester 0:1c5088dae6e1 43
dkester 0:1c5088dae6e1 44 void Sensors::setupAngle()
dkester 0:1c5088dae6e1 45 {
dkester 2:871b5efb2043 46 //TODO
dkester 0:1c5088dae6e1 47 }
dkester 0:1c5088dae6e1 48
dkester 0:1c5088dae6e1 49 void Sensors::setupIMU()
dkester 0:1c5088dae6e1 50 {
dkester 2:871b5efb2043 51 //Sets sample rate to 8000/1+79 = 100Hz
dkester 2:871b5efb2043 52 writeRegister(0x19,0x4f);
dkester 2:871b5efb2043 53 //Disable FSync, 256 DLPF
dkester 2:871b5efb2043 54 writeRegister(0x1A,0x00);
dkester 2:871b5efb2043 55
dkester 2:871b5efb2043 56 // Disable gyroscope self tests, scale of 200 degrees/s
dkester 2:871b5efb2043 57 // Disable accelerometer self tests, scale of +-2g, no DHPF
dkester 2:871b5efb2043 58 cmd[0] = 0x1B;
dkester 2:871b5efb2043 59 cmd[1] = 0x00; //bit4 and bit 3 to choose scale range
dkester 2:871b5efb2043 60 cmd[2] = 0x00;
dkester 2:871b5efb2043 61 i2c.write(MPU6500,cmd,3);
dkester 2:871b5efb2043 62
dkester 2:871b5efb2043 63 //Freefall threshold of 0mg and duration limit of 0
dkester 2:871b5efb2043 64 cmd[0] = 0x1D;
dkester 2:871b5efb2043 65 i2c.write(MPU6500,cmd,3);
dkester 2:871b5efb2043 66
dkester 2:871b5efb2043 67 //Motion threshold of 0mg and duration limit of 0
dkester 2:871b5efb2043 68 cmd[0] = 0x21;
dkester 2:871b5efb2043 69 i2c.write(MPU6500,cmd,3);
dkester 2:871b5efb2043 70
dkester 2:871b5efb2043 71 //Disable sensor output FIFO buffer
dkester 2:871b5efb2043 72 writeRegister(0x23,0x00);
dkester 2:871b5efb2043 73
dkester 2:871b5efb2043 74 //Set aux I2C, from 0x24 to 0x36 = 0x00
dkester 2:871b5efb2043 75
dkester 2:871b5efb2043 76 //Setup INT pin and AUX I2C pass through
dkester 2:871b5efb2043 77 cmd[0] = 0x37;
dkester 2:871b5efb2043 78 cmd[1] = 0x02;
dkester 2:871b5efb2043 79 cmd[2] = 0x01;
dkester 2:871b5efb2043 80 i2c.write(MPU6500,cmd,3);
dkester 2:871b5efb2043 81
dkester 2:871b5efb2043 82 //Reset sensor signal paths
dkester 2:871b5efb2043 83 writeRegister(0x68,0x00);
dkester 2:871b5efb2043 84
dkester 2:871b5efb2043 85 //Motion detection control
dkester 2:871b5efb2043 86 writeRegister(0x69,0x00);
dkester 2:871b5efb2043 87
dkester 2:871b5efb2043 88 //Disables FIFO, AUX I2C and I2C reset bits to 0
dkester 2:871b5efb2043 89 writeRegister(0x6A,0x00);
dkester 2:871b5efb2043 90
dkester 2:871b5efb2043 91 //Sets clock source to gyro reference w/ PLL
dkester 2:871b5efb2043 92 writeRegister(0x6B,0x02);
dkester 2:871b5efb2043 93
dkester 2:871b5efb2043 94 //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
dkester 2:871b5efb2043 95 writeRegister(0x6C,0x00);
dkester 2:871b5efb2043 96
dkester 2:871b5efb2043 97 //Data transfer to and from the FIFO buffer
dkester 2:871b5efb2043 98 writeRegister(0x74,0x00);
dkester 2:871b5efb2043 99 /*
dkester 2:871b5efb2043 100
dkester 0:1c5088dae6e1 101 // Get current value of register 0x6B – Power Management 1 PWR_MGMT_1
dkester 0:1c5088dae6e1 102 cmd[0] = 0x6B;
dkester 0:1c5088dae6e1 103 // SLEEP = 1, CYCLE = 0, ClOCK = 001, RESET = 0
dkester 2:871b5efb2043 104 cmd[1] = (readRegister(0x6B) & 0x10) | 0x41; // OR with 0x1 for SLEEP = 0
dkester 0:1c5088dae6e1 105 i2c.write(MPU6500,cmd,2);
dkester 2:871b5efb2043 106 */
dkester 2:871b5efb2043 107
dkester 2:871b5efb2043 108 }
dkester 0:1c5088dae6e1 109
dkester 0:1c5088dae6e1 110
dkester 0:1c5088dae6e1 111 void Sensors::updateIMU()
dkester 0:1c5088dae6e1 112 {
dkester 0:1c5088dae6e1 113 /* Read measurements from MPU6500 */
dkester 0:1c5088dae6e1 114 cmd[0] = 0x3B;
dkester 0:1c5088dae6e1 115 i2c.write(MPU6500,cmd,1,true);
dkester 0:1c5088dae6e1 116 i2c.read(MPU6500,out,14);
dkester 0:1c5088dae6e1 117
dkester 2:871b5efb2043 118 this->accel_x = (out[0] << 8) | out[1];
dkester 2:871b5efb2043 119 this->accel_y = (out[2] << 8) | out[3];
dkester 2:871b5efb2043 120 this->accel_z = (out[4] << 8) | out[5];
dkester 0:1c5088dae6e1 121
dkester 2:871b5efb2043 122 this->temp = (out[6] << 8) | out[7];
dkester 0:1c5088dae6e1 123
dkester 2:871b5efb2043 124 this->gyro_x = (out[8] << 8) | out[9];
dkester 2:871b5efb2043 125 this->gyro_y = (out[10] << 8) | out[11];
dkester 2:871b5efb2043 126 this->gyro_z = (out[12] << 8) | out[13];
dkester 0:1c5088dae6e1 127 }
dkester 0:1c5088dae6e1 128
dkester 1:b44bd62c542f 129 int16_t Sensors::getAccelX()
dkester 1:b44bd62c542f 130 {
dkester 0:1c5088dae6e1 131 return this->accel_x;
dkester 0:1c5088dae6e1 132 }
dkester 1:b44bd62c542f 133 int16_t Sensors::getAccelY()
dkester 1:b44bd62c542f 134 {
dkester 0:1c5088dae6e1 135 return this->accel_y;
dkester 0:1c5088dae6e1 136 }
dkester 1:b44bd62c542f 137 int16_t Sensors::getAccelZ()
dkester 1:b44bd62c542f 138 {
dkester 0:1c5088dae6e1 139 return this->accel_z;
dkester 0:1c5088dae6e1 140 }
dkester 1:b44bd62c542f 141 float Sensors::getTemp()
dkester 1:b44bd62c542f 142 {
dkester 0:1c5088dae6e1 143 return (this->temp)/340 + 36.53;
dkester 0:1c5088dae6e1 144 }
dkester 1:b44bd62c542f 145 int16_t Sensors::getGyroX()
dkester 1:b44bd62c542f 146 {
dkester 2:871b5efb2043 147 return this->gyro_x;
dkester 0:1c5088dae6e1 148 }
dkester 1:b44bd62c542f 149 int16_t Sensors::getGyroY()
dkester 1:b44bd62c542f 150 {
dkester 0:1c5088dae6e1 151 return this->gyro_y;
dkester 0:1c5088dae6e1 152 }
dkester 1:b44bd62c542f 153 int16_t Sensors::getGyroZ()
dkester 1:b44bd62c542f 154 {
dkester 0:1c5088dae6e1 155 return this->gyro_z;
dkester 0:1c5088dae6e1 156 }
dkester 1:b44bd62c542f 157
dkester 1:b44bd62c542f 158 int8_t Sensors::readRegister(int8_t addr)
dkester 1:b44bd62c542f 159 {
dkester 1:b44bd62c542f 160 cmd[0] = addr;
dkester 1:b44bd62c542f 161 i2c.write(MPU6500,cmd,1);
dkester 1:b44bd62c542f 162 i2c.read(MPU6500,out,1);
dkester 1:b44bd62c542f 163 return out[0];
dkester 1:b44bd62c542f 164 }
dkester 1:b44bd62c542f 165
dkester 1:b44bd62c542f 166 void Sensors::writeRegister(int8_t addr, int8_t value)
dkester 1:b44bd62c542f 167 {
dkester 1:b44bd62c542f 168 cmd[0] = addr;
dkester 1:b44bd62c542f 169 cmd[1] = value;
dkester 1:b44bd62c542f 170 i2c.write(MPU6500,cmd,2);
dkester 1:b44bd62c542f 171 }
dkester 1:b44bd62c542f 172
dkester 1:b44bd62c542f 173
dkester 1:b44bd62c542f 174
dkester 1:b44bd62c542f 175
dkester 1:b44bd62c542f 176
dkester 2:871b5efb2043 177
dkester 2:871b5efb2043 178
dkester 2:871b5efb2043 179