Added 360 degrees resolution of angle calculated from accelerometer

PS3BT.cpp

@@ -482,7 +482,7 @@ uint8_t PS3BT::getAnalogHat(AnalogHat a)
         return 0;                        
     return (uint8_t)(l2capinbuf[(uint16_t)a]);            
 }
-uint32_t PS3BT::getSensor(Sensor a)
+int32_t PS3BT::getSensor(Sensor a)
 {
     if (l2capinbuf == NULL)
         return 0;
@@ -504,43 +504,62 @@ uint32_t PS3BT::getSensor(Sensor a)
             return 0;                
     }
     else if (a == tempMove)
-    {
-        if (l2capinbuf == NULL)
-            return 0;
         return (((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4) | (l2capinbuf[(uint16_t)a] << 4));
-    }
+    else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove
-    else
+        return ((l2capinbuf[(uint16_t)a + 1] << 8) | l2capinbuf[(uint16_t)a]);
-    {
-        
-        return (((l2capinbuf[(uint16_t)a + 1] << 8) | l2capinbuf[(uint16_t)a]) - 0x8000);                
-    }
 }
 double PS3BT::getAngle(Angle a) {        
-    // Data for the Kionix KXPC4 used in DualShock 3
+    double accXval;
-    const double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g)
+    double accYval;
-    const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
+    double accZval;
     
-    double accXval = (zeroG - getSensor(aX)) / sensivity; // Convert to g's
+    if(PS3BTConnected) {
-    accXval *= 2;    
+        // Data for the Kionix KXPC4 used in the DualShock 3
-    double accYval = (zeroG - getSensor(aY)) / sensivity; // Convert to g's
+        double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g)
-    accYval *= 2;    
+        double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
-    double accZval = (zeroG - getSensor(aZ)) / sensivity; // Convert to g's
+        accXval = ((double)getSensor(aX)-zeroG) / sensivity; // Convert to g's
-    accZval *= 2;
+        accXval *= 2;    
+        accYval = ((double)getSensor(aY)-zeroG) / sensivity; // Convert to g's
+        accYval *= 2;    
+        accZval = ((double)getSensor(aZ)-zeroG) / sensivity; // Convert to g's
+        accZval *= 2;
+    } else if(PS3MoveBTConnected) {
+        // It's a Kionix KXSC4 inside the Motion controller        
+        const uint16_t sensivity = 28285; // Find by experimenting
+        accXval = (double)getSensor(aXmove)/sensivity;
+        accYval = (double)getSensor(aYmove)/sensivity;
+        accZval = (double)getSensor(aZmove)/sensivity;        
         
-    double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval);
+        if(accXval < -1) // Convert to g's
-    // convert read values to -π/2 to π/2 - Needed for atan2
+            accXval = ((1+accXval)-(1-1.15))*(-1/0.15);
-    double angleX = acos(accXval/R)-0.5*PI;
+        else if(accXval > 1)            
-    double angleY = acos(accYval/R)-0.5*PI;
+            accXval = ((1+accXval)-(1+1.15))*(-1/0.15);
-    double angleZ = acos(accZval/R)-0.5*PI;
+        
+        if(accYval < -1) // Convert to g's
+            accYval = ((1+accYval)-(1-1.15))*(-1/0.15);
+        else if(accYval > 1)            
+            accYval = ((1+accYval)-(1+1.15))*(-1/0.15);
+        
+        if(accZval < -1) // Convert to g's
+            accZval = ((1+accZval)-(1-1.15))*(-1/0.15);
+        else if(accZval > 1)
+            accZval = ((1+accZval)-(1+1.15))*(-1/0.15);
+    }
+    
+    double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval); // Calculate the length of the force vector
+    // Normalize vectors    
+    accXval = accXval/R; 
+    accYval = accYval/R;
+    accZval = accZval/R;
     
     // Convert to 360 degrees resolution
     // atan2 outputs the value of -π to π (radians)
     // We are then converting it to 0 to 2π and then to degrees  
     if (a == Pitch) {        
-        double angle = (atan2(-angleY,-angleZ)+PI)*RAD_TO_DEG;
+        double angle = (atan2(-accYval,-accZval)+PI)*RAD_TO_DEG;
         return angle;
     } else {
-        double angle = (atan2(-angleX,-angleZ)+PI)*RAD_TO_DEG;
+        double angle = (atan2(-accXval,-accZval)+PI)*RAD_TO_DEG;
         return angle;
     }    
 }

PS3BT.h

@@ -338,7 +338,7 @@ public:
     bool getButton(Button b);
     uint8_t getAnalogButton(AnalogButton a);
     uint8_t getAnalogHat(AnalogHat a);
-    uint32_t getSensor(Sensor a);
+    int32_t getSensor(Sensor a);
     double getAngle(Angle a);
     bool getStatus(Status c);  
     String getStatusString();    

examples/PS3BT/PS3BT.ino

@@ -142,10 +142,14 @@ void loop()
       } else {
         if(BT.getButton(SELECT_MOVE)) {
           Serial.print(F(" - Select"));
-          printTemperature = false;
+          printTemperature = !printTemperature;
+          while(BT.getButton(SELECT_MOVE))
+            Usb.Task();          
         } if(BT.getButton(START_MOVE)) {
           Serial.print(F(" - Start"));
-          printTemperature = true;
+          printAngle = !printAngle;
+          while(BT.getButton(START_MOVE))
+            Usb.Task();                              
         } if(BT.getButton(TRIANGLE_MOVE)) {            
           Serial.print(F(" - Triangle"));
           BT.moveSetBulb(Red);
@@ -170,7 +174,13 @@ void loop()
         Serial.println("");
       }
     }
-    if(printTemperature) {
+    if(printAngle) {
+      Serial.print(F("Pitch: "));               
+      Serial.print(BT.getAngle(Pitch));                  
+      Serial.print(F("\tRoll: ")); 
+      Serial.println(BT.getAngle(Roll));
+    }
+    else if(printTemperature) {
       String templow;
       String temphigh;
       String input = String(BT.getSensor(tempMove));