Diff: Sensors.cpp

Revision:

4:2a5a08b14539

Parent:

3:a3e1a06c486d

Child:

5:46947b447701

--- a/Sensors.cpp	Mon Jun 01 15:41:01 2015 +0000
+++ b/Sensors.cpp	Wed Jun 03 15:10:45 2015 +0000
@@ -28,6 +28,7 @@
     this->gyro_y_l = 0;
     this->gyro_z_l = 0;
 
+
     setupIMU();
     setupAngle();
 }
@@ -45,15 +46,18 @@
     i2c.write(AS5600,cmd,1);
     i2c.read(AS5600,out,2);
 
-    this->angle_h = out[1];   //((out[0] << 8) + out[1]) * 0.087912087 ;
-    this->angle_l = out[0];
+    this->angle_h = out[0];   //((out[0] << 8) + out[1]) * 0.087912087;
+    this->angle_l = out[1];
 }
 
-int8_t Sensors::getAngleH()[
+float Sensors::getAngle(){
+    return ((getAngleH() << 8) + getAngleL()) * 0.087912087;
+    }
+
+char Sensors::getAngleH(){
     return this->angle_h;
 }
-
-int8_t Sensors::getAngleL()[
+char Sensors::getAngleL(){
     return this->angle_l;
 }
 
@@ -73,7 +77,11 @@
 void Sensors::setupIMU()
 {
     //Sets sample rate to 8000/1+79 = 100Hz
-    writeRegister(0x19,0x4f);
+    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
+    
+    wait(0.001);
+    
+    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
     //Disable FSync, 256 DLPF
     writeRegister(0x1A,0x00);
     
@@ -123,7 +131,6 @@
     writeRegister(0x74,0x00);
 }
 
-
 void Sensors::updateIMU()
 {
     // Read measurements from MPU6500  
@@ -200,6 +207,21 @@
 }
 
 
+int16_t Sensors::getAccelX(){
+    return ((getAccelXH() << 8) + getAccelXL());
+    }
+int16_t Sensors::getAccelY(){
+    return ((getAccelYH() << 8) + getAccelYL());
+    }
+int16_t Sensors::getAccelZ(){
+    return ((getAccelZH() << 8) + getAccelZL());
+    }
+    
+int16_t Sensors::getGyroX(){
+    return ((getGyroXH() << 8) + getGyroXL());
+    }
+
+
 int8_t Sensors::readRegister(int8_t addr)
 {
     cmd[0] = addr;