Diff: APP.cpp

Revision:

12:4961ec71bc8f

Parent:

11:eafa5c07f060

Child:

13:08ef55cd14c6

--- a/APP.cpp	Mon Jan 11 19:34:58 2016 +0000
+++ b/APP.cpp	Tue Jan 12 15:09:47 2016 +0000
@@ -58,32 +58,6 @@
     return value;
 }
  
-uint32_t squareRoot(uint32_t input)
-{
-    uint32_t op = input;
-    uint32_t res = 0;
-    uint32_t one = 1UL << 30;
- 
-    while (one > op)
-    {
-        one >>= 2;
-    }
- 
-    while (one != 0)
-    {
-        if (op >= res + one)
-        {
-            op = op - (res + one);
-            res = res + 2 * one;
-        }
- 
-        res >>= 1;
-        one >>= 2;
-    }
- 
-    return res;
-}
- 
 int16_t readAndComputeAngle()
 {
     uint8_t result[6];
@@ -93,9 +67,8 @@
     int16_t accX = convertToAcceleration(result[0], result[1]);
     int16_t accY = convertToAcceleration(result[2], result[3]);
     int16_t accZ = convertToAcceleration(result[4], result[5]);
- 
-    uint32_t modulus = squareRoot(accX*accX + accY*accY + accZ*accZ);
-    float angle = acos((float) accZ / modulus) * 180.0 / PI;
+
+    float angle = acos((float) accZ / sqrt((float) accX*accX + accY*accY + accZ*accZ)) * 180.0 / PI;
  
     // To use integer operations instead of floating-point arithmetic, the angle is multiplied by 100.
     // This allows for two decimals of precision on the display.
@@ -112,8 +85,15 @@
 {
     uint8_t digits[4];
     sprintf((char*) digits, "%d", angle);
- 
-    if (angle < 1000)
+
+    if (angle < 100)
+    {
+        uart.putc(0);
+        uart.putc(0);
+        uart.putc(digits[0]);
+        uart.putc(digits[1]);
+    }
+    else if (angle < 1000)
     {
         uart.putc(0);
         uart.putc(digits[0]);