Diff: Sensors.cpp

Revision:

0:1c5088dae6e1

Child:

1:b44bd62c542f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors.cpp	Wed May 20 08:44:35 2015 +0000
@@ -0,0 +1,110 @@
+#include "Sensors.h"
+
+I2C i2c(p29, p28);
+
+Sensors* Sensors::instance = new Sensors();
+
+Sensors::Sensors()
+{
+    i2c.frequency(300000);
+    this->AS5600 = 0x36 << 1;
+    this->MPU6500 = 0x68 << 1;
+    
+    this->angle = 0;
+    this->accel_x = 0;
+    this->accel_y = 0;
+    this->accel_z = 0;
+    this->temp = 0;
+    this->gyro_x = 0;
+    this->gyro_y = 0;
+    this->gyro_z = 0;
+    
+    setupIMU();
+    setupAngle();
+}
+
+
+Sensors* Sensors::getInstance(){
+    return instance;
+}
+
+float Sensors::getAngle()
+{
+    /* Read angle from AS5600 */
+    cmd[0] = 0x0E;
+    i2c.write(AS5600,cmd,1);
+    i2c.read(AS5600,out,2);
+
+    angle = ((out[0] << 8) + out[1]) * 0.087912087 ;
+    return angle;
+}
+
+void Sensors::setupAngle()
+{
+}
+
+void Sensors::setupIMU()
+{
+    /* Setup MPU6500 */
+    // Get current value of register 0x6B – Power Management 1 PWR_MGMT_1
+    cmd[0] = 0x6B;
+    i2c.write(MPU6500,cmd,1);
+    i2c.read(MPU6500,out,1);
+    int PW = out[0];
+
+    // SLEEP = 1, CYCLE = 0, ClOCK = 001, RESET = 0
+    cmd[1] = (PW & 0x10) | 0x41; // OR with 0x1 for SLEEP = 0
+    PW = cmd[1];
+    i2c.write(MPU6500,cmd,2);
+
+    // Set resulution for gyroscope and accelerometer
+    cmd[0] = 0x1B;
+    cmd[1] = 0x00;
+    cmd[2] = 0x00;
+    i2c.write(MPU6500,cmd,3);
+
+    // Set Reset = 0 & Sleep = 0
+    cmd[0] = 0x6B;
+    cmd[1] = PW & 0x3F;
+    i2c.write(MPU6500,cmd,2);
+}
+
+void Sensors::updateIMU()
+{
+    /* Read measurements from MPU6500 */
+    cmd[0] = 0x3B;
+    i2c.write(MPU6500,cmd,1,true);
+    i2c.read(MPU6500,out,14);
+
+    this->accel_x = (out[0] << 8) + out[1];
+    this->accel_y = (out[2] << 8) + out[3];
+    this->accel_z = (out[4] << 8) + out[5];
+
+    this->temp = (out[6] << 8) + out[7];
+
+    this->gyro_x = (out[8] << 8) + out[9];
+    this->gyro_y = (out[10] << 8) + out[11];
+    this->gyro_z = (out[12] << 8) + out[13];
+}
+
+int16_t Sensors::getAccelX() {
+    return this->accel_x;
+}
+int16_t Sensors::getAccelY() {
+    return this->accel_y;
+}
+int16_t Sensors::getAccelZ() {
+    return this->accel_z;
+}
+float Sensors::getTemp() {
+    return (this->temp)/340 + 36.53;
+}
+int16_t Sensors::getGyroX() {
+    return this->accel_x;
+}
+int16_t Sensors::getGyroY() {
+    return this->gyro_y;
+}
+int16_t Sensors::getGyroZ() {
+    return this->gyro_z;
+}