Vincent Belanger
/
APP
APP 1 S5.
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]);