Diff: APP.cpp

Revision:

7:b00843da0530

Child:

11:eafa5c07f060

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/APP.cpp	Sun Jan 10 22:28:48 2016 +0000
@@ -0,0 +1,149 @@
+#include <math.h>
+#include <stdint.h>
+
+#include "mbed.h"
+#include "APP.h"
+
+
+static I2C    i2c(p28, p27);
+static Serial uart(p9, p10);
+
+void setup()
+{
+    // Setup UART and I2C.
+    uart.format(8, Serial::None, 1);
+    uart.baud(9600);
+    i2c.frequency(100000);
+ 
+    // Clear display
+    uart.putc(0x76);
+ 
+    // Decimal point for display
+    uart.putc(0x77);
+    uart.putc(0x02);
+ 
+    // Maximum brightness
+    uart.putc(0x7A);
+    uart.putc(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)
+    {
+        uart.putc(0);
+        uart.putc(digits[0]);
+        uart.putc(digits[1]);
+        uart.putc(digits[2]);
+    }
+    else
+    {
+        uart.putc(digits[0]);
+        uart.putc(digits[1]);
+        uart.putc(digits[2]);
+        uart.putc(digits[3]);
+    }
+}
+ 
+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);
+    }
+}