Vincent Belanger
/
APP
APP 1 S5.
Diff: APP.cpp
- Revision:
- 6:ccdbd5923e37
- Child:
- 8:e38cd9e16055
- Child:
- 9:f35a5cde61a3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/APP.cpp Sun Jan 10 22:24:00 2016 +0000 @@ -0,0 +1,144 @@ +#include <math.h> +#include <stdint.h> + +#include "mbed.h" +#include "UART.h" +#include "Type.h" +#include "APP.h" + + +static I2C i2c(p28, p27); + +void setup() +{ + // Setup UART and I2C. + UARTInit(9600); + i2c.frequency(100000); + + // Clear display + UARTSend(0x76); + + // Decimal point for display + UARTSend(0x77); + UARTSend(0x02); + + // Maximum brightness + UARTSend(0x7A); + UARTSend(0xFF); +} + +void activateAccelerometer() +{ + uint8_t data[2] = {CTRL_REG, 0x01}; + i2c.write(ACC_ADDRESS, (char*) data, 2); +} + +uint8_t readStatus() +{ + uint8_t result[1]; + uint8_t reg[1] = {STATUS_REG}; + i2c.write(ACC_ADDRESS, (char*) reg, 1, true); + i2c.read(ACC_ADDRESS, (char*) result, 1); + + return result[0]; +} + +int16_t convertToAcceleration(uint8_t hi, uint8_t lo) +{ + int16_t value = (lo >> 2) | (hi << 6); + if (value > UINT14_MAX/2) + { + value -= UINT14_MAX; + } + + 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]; + uint8_t reg[1] = {ACC_X_REG}; + i2c.write(ACC_ADDRESS, (char*) reg, 1, true); + i2c.read(ACC_ADDRESS, (char*) result, 6); + 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; + + // 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. + int16_t angleHundreds = angle * 100; + if (angleHundreds > 9000) + { + angleHundreds = 18000 - angleHundreds; + } + + return angleHundreds; +} + +void updateDisplay(int16_t angle) +{ + uint8_t digits[4]; + sprintf((char*) digits, "%d", angle); + + if (angle < 1000) + { + UARTSend(0); + UARTSend(digits, 3); + } + else + { + UARTSend(digits, 4); + } +} + +int32_t main() +{ + setup(); + activateAccelerometer(); + + while (true) + { + // Read bit 3 from the status register + int8_t status = (readStatus() >> 3) & 0x01; + + if (status == 1) + { + // Get accelerations, compute angle + uint16_t angle = readAndComputeAngle(); + + // Update 7 segment display + updateDisplay(angle); + } + + wait(0.4); + } +}