Diff: Sensors.cpp

Revision:

1:b44bd62c542f

Parent:

0:1c5088dae6e1

Child:

2:871b5efb2043

diff -r 1c5088dae6e1 -r b44bd62c542f Sensors.cpp
--- a/Sensors.cpp	Wed May 20 08:44:35 2015 +0000
+++ b/Sensors.cpp	Wed May 20 11:45:38 2015 +0000
@@ -9,7 +9,7 @@
     i2c.frequency(300000);
     this->AS5600 = 0x36 << 1;
     this->MPU6500 = 0x68 << 1;
-    
+
     this->angle = 0;
     this->accel_x = 0;
     this->accel_y = 0;
@@ -18,13 +18,14 @@
     this->gyro_x = 0;
     this->gyro_y = 0;
     this->gyro_z = 0;
-    
+
     setupIMU();
     setupAngle();
 }
 
 
-Sensors* Sensors::getInstance(){
+Sensors* Sensors::getInstance()
+{
     return instance;
 }
 
@@ -87,24 +88,70 @@
     this->gyro_z = (out[12] << 8) + out[13];
 }
 
-int16_t Sensors::getAccelX() {
+int16_t Sensors::getAccelX()
+{
     return this->accel_x;
 }
-int16_t Sensors::getAccelY() {
+int16_t Sensors::getAccelY()
+{
     return this->accel_y;
 }
-int16_t Sensors::getAccelZ() {
+int16_t Sensors::getAccelZ()
+{
     return this->accel_z;
 }
-float Sensors::getTemp() {
+float Sensors::getTemp()
+{
     return (this->temp)/340 + 36.53;
 }
-int16_t Sensors::getGyroX() {
+int16_t Sensors::getGyroX()
+{
     return this->accel_x;
 }
-int16_t Sensors::getGyroY() {
+int16_t Sensors::getGyroY()
+{
     return this->gyro_y;
 }
-int16_t Sensors::getGyroZ() {
+int16_t Sensors::getGyroZ()
+{
     return this->gyro_z;
 }
+
+int8_t Sensors::readRegister(int8_t addr)
+{
+    cmd[0] = addr;
+    i2c.write(MPU6500,cmd,1);
+    i2c.read(MPU6500,out,1);
+    return out[0];
+}
+
+void Sensors::writeRegister(int8_t addr, int8_t value)
+{
+    cmd[0] = addr;
+    cmd[1] = value;
+    i2c.write(MPU6500,cmd,2);
+}
+
+void Sensors::setDMP(int8_t th)
+{
+    writeRegister(0x6A, ( readRegister(0x6A) | (1 << 3))); //reset DMP
+    writeRegister(0x6A, ( readRegister(0x6A) | (1 << 7))); //set DMP
+    writeRegister(0x38, ( readRegister(0x38) | (1 << 6))); //enable motion interrupt
+    writeRegister(0x38, ( readRegister(0x38) | (1 << 1))); //enable motion interrupt
+    writeRegister(0x1F, th); //set threshold
+}
+
+
+bool Sensors::checkDMP()
+{
+    
+    if (((readRegister(0x3A) >> 6) & 1) == 1) {
+        return true;
+    }  else {
+        return false;
+    }
+}
+
+
+
+