Vincent Belanger / Mbed 2 deprecated APP

APP 1 S5.

Dependencies:   mbed

APP.cpp@8:e38cd9e16055, 2016-01-11 (annotated)

Committer:
vinbel93
Date:
Mon Jan 11 01:25:29 2016 +0000
Revision:
8:e38cd9e16055
Parent:
6:ccdbd5923e37 
Almost done

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vinbel93 6:ccdbd5923e37 1 #include <math.h>
vinbel93 6:ccdbd5923e37 2 #include <stdint.h>
vinbel93 6:ccdbd5923e37 3
vinbel93 6:ccdbd5923e37 4 #include "mbed.h"
vinbel93 6:ccdbd5923e37 5 #include "UART.h"
vinbel93 6:ccdbd5923e37 6 #include "APP.h"
vinbel93 6:ccdbd5923e37 7
vinbel93 6:ccdbd5923e37 8
vinbel93 6:ccdbd5923e37 9 static I2C i2c(p28, p27);
vinbel93 6:ccdbd5923e37 10
vinbel93 6:ccdbd5923e37 11 void setup()
vinbel93 6:ccdbd5923e37 12 {
vinbel93 6:ccdbd5923e37 13 // Setup UART and I2C.
vinbel93 6:ccdbd5923e37 14 UARTInit(9600);
vinbel93 6:ccdbd5923e37 15 i2c.frequency(100000);
vinbel93 6:ccdbd5923e37 16
vinbel93 6:ccdbd5923e37 17 // Clear display
vinbel93 6:ccdbd5923e37 18 UARTSend(0x76);
vinbel93 6:ccdbd5923e37 19
vinbel93 6:ccdbd5923e37 20 // Decimal point for display
vinbel93 6:ccdbd5923e37 21 UARTSend(0x77);
vinbel93 6:ccdbd5923e37 22 UARTSend(0x02);
vinbel93 6:ccdbd5923e37 23
vinbel93 6:ccdbd5923e37 24 // Maximum brightness
vinbel93 6:ccdbd5923e37 25 UARTSend(0x7A);
vinbel93 6:ccdbd5923e37 26 UARTSend(0xFF);
vinbel93 6:ccdbd5923e37 27 }
vinbel93 6:ccdbd5923e37 28
vinbel93 6:ccdbd5923e37 29 void activateAccelerometer()
vinbel93 6:ccdbd5923e37 30 {
vinbel93 6:ccdbd5923e37 31 uint8_t data[2] = {CTRL_REG, 0x01};
vinbel93 6:ccdbd5923e37 32 i2c.write(ACC_ADDRESS, (char*) data, 2);
vinbel93 6:ccdbd5923e37 33 }
vinbel93 6:ccdbd5923e37 34
vinbel93 6:ccdbd5923e37 35 uint8_t readStatus()
vinbel93 6:ccdbd5923e37 36 {
vinbel93 6:ccdbd5923e37 37 uint8_t result[1];
vinbel93 6:ccdbd5923e37 38 uint8_t reg[1] = {STATUS_REG};
vinbel93 6:ccdbd5923e37 39 i2c.write(ACC_ADDRESS, (char*) reg, 1, true);
vinbel93 6:ccdbd5923e37 40 i2c.read(ACC_ADDRESS, (char*) result, 1);
vinbel93 6:ccdbd5923e37 41
vinbel93 6:ccdbd5923e37 42 return result[0];
vinbel93 6:ccdbd5923e37 43 }
vinbel93 6:ccdbd5923e37 44
vinbel93 6:ccdbd5923e37 45 int16_t convertToAcceleration(uint8_t hi, uint8_t lo)
vinbel93 6:ccdbd5923e37 46 {
vinbel93 6:ccdbd5923e37 47 int16_t value = (lo >> 2) | (hi << 6);
vinbel93 6:ccdbd5923e37 48 if (value > UINT14_MAX/2)
vinbel93 6:ccdbd5923e37 49 {
vinbel93 6:ccdbd5923e37 50 value -= UINT14_MAX;
vinbel93 6:ccdbd5923e37 51 }
vinbel93 6:ccdbd5923e37 52
vinbel93 6:ccdbd5923e37 53 return value;
vinbel93 6:ccdbd5923e37 54 }
vinbel93 6:ccdbd5923e37 55
vinbel93 6:ccdbd5923e37 56 uint32_t squareRoot(uint32_t input)
vinbel93 6:ccdbd5923e37 57 {
vinbel93 6:ccdbd5923e37 58 uint32_t op = input;
vinbel93 6:ccdbd5923e37 59 uint32_t res = 0;
vinbel93 6:ccdbd5923e37 60 uint32_t one = 1UL << 30;
vinbel93 6:ccdbd5923e37 61
vinbel93 6:ccdbd5923e37 62 while (one > op)
vinbel93 6:ccdbd5923e37 63 {
vinbel93 6:ccdbd5923e37 64 one >>= 2;
vinbel93 6:ccdbd5923e37 65 }
vinbel93 6:ccdbd5923e37 66
vinbel93 6:ccdbd5923e37 67 while (one != 0)
vinbel93 6:ccdbd5923e37 68 {
vinbel93 6:ccdbd5923e37 69 if (op >= res + one)
vinbel93 6:ccdbd5923e37 70 {
vinbel93 6:ccdbd5923e37 71 op = op - (res + one);
vinbel93 6:ccdbd5923e37 72 res = res + 2 * one;
vinbel93 6:ccdbd5923e37 73 }
vinbel93 6:ccdbd5923e37 74
vinbel93 6:ccdbd5923e37 75 res >>= 1;
vinbel93 6:ccdbd5923e37 76 one >>= 2;
vinbel93 6:ccdbd5923e37 77 }
vinbel93 6:ccdbd5923e37 78
vinbel93 6:ccdbd5923e37 79 return res;
vinbel93 6:ccdbd5923e37 80 }
vinbel93 6:ccdbd5923e37 81
vinbel93 6:ccdbd5923e37 82 int16_t readAndComputeAngle()
vinbel93 6:ccdbd5923e37 83 {
vinbel93 6:ccdbd5923e37 84 uint8_t result[6];
vinbel93 6:ccdbd5923e37 85 uint8_t reg[1] = {ACC_X_REG};
vinbel93 6:ccdbd5923e37 86 i2c.write(ACC_ADDRESS, (char*) reg, 1, true);
vinbel93 6:ccdbd5923e37 87 i2c.read(ACC_ADDRESS, (char*) result, 6);
vinbel93 6:ccdbd5923e37 88 int16_t accX = convertToAcceleration(result[0], result[1]);
vinbel93 6:ccdbd5923e37 89 int16_t accY = convertToAcceleration(result[2], result[3]);
vinbel93 6:ccdbd5923e37 90 int16_t accZ = convertToAcceleration(result[4], result[5]);
vinbel93 6:ccdbd5923e37 91
vinbel93 6:ccdbd5923e37 92 uint32_t modulus = squareRoot(accX*accX + accY*accY + accZ*accZ);
vinbel93 6:ccdbd5923e37 93 float angle = acos((float) accZ / modulus) * 180.0 / PI;
vinbel93 6:ccdbd5923e37 94
vinbel93 6:ccdbd5923e37 95 // To use integer operations instead of floating-point arithmetic, the angle is multiplied by 100.
vinbel93 6:ccdbd5923e37 96 // This allows for two decimals of precision on the display.
vinbel93 6:ccdbd5923e37 97 int16_t angleHundreds = angle * 100;
vinbel93 6:ccdbd5923e37 98 if (angleHundreds > 9000)
vinbel93 6:ccdbd5923e37 99 {
vinbel93 6:ccdbd5923e37 100 angleHundreds = 18000 - angleHundreds;
vinbel93 6:ccdbd5923e37 101 }
vinbel93 6:ccdbd5923e37 102
vinbel93 6:ccdbd5923e37 103 return angleHundreds;
vinbel93 6:ccdbd5923e37 104 }
vinbel93 6:ccdbd5923e37 105
vinbel93 6:ccdbd5923e37 106 void updateDisplay(int16_t angle)
vinbel93 6:ccdbd5923e37 107 {
vinbel93 6:ccdbd5923e37 108 uint8_t digits[4];
vinbel93 6:ccdbd5923e37 109 sprintf((char*) digits, "%d", angle);
vinbel93 8:e38cd9e16055 110
vinbel93 6:ccdbd5923e37 111 if (angle < 1000)
vinbel93 6:ccdbd5923e37 112 {
vinbel93 6:ccdbd5923e37 113 UARTSend(0);
vinbel93 6:ccdbd5923e37 114 UARTSend(digits, 3);
vinbel93 6:ccdbd5923e37 115 }
vinbel93 6:ccdbd5923e37 116 else
vinbel93 6:ccdbd5923e37 117 {
vinbel93 6:ccdbd5923e37 118 UARTSend(digits, 4);
vinbel93 6:ccdbd5923e37 119 }
vinbel93 6:ccdbd5923e37 120 }
vinbel93 6:ccdbd5923e37 121
vinbel93 6:ccdbd5923e37 122 int32_t main()
vinbel93 6:ccdbd5923e37 123 {
vinbel93 6:ccdbd5923e37 124 setup();
vinbel93 6:ccdbd5923e37 125 activateAccelerometer();
vinbel93 6:ccdbd5923e37 126
vinbel93 6:ccdbd5923e37 127 while (true)
vinbel93 6:ccdbd5923e37 128 {
vinbel93 6:ccdbd5923e37 129 // Read bit 3 from the status register
vinbel93 6:ccdbd5923e37 130 int8_t status = (readStatus() >> 3) & 0x01;
vinbel93 6:ccdbd5923e37 131
vinbel93 6:ccdbd5923e37 132 if (status == 1)
vinbel93 6:ccdbd5923e37 133 {
vinbel93 6:ccdbd5923e37 134 // Get accelerations, compute angle
vinbel93 6:ccdbd5923e37 135 uint16_t angle = readAndComputeAngle();
vinbel93 6:ccdbd5923e37 136
vinbel93 6:ccdbd5923e37 137 // Update 7 segment display
vinbel93 6:ccdbd5923e37 138 updateDisplay(angle);
vinbel93 6:ccdbd5923e37 139 }
vinbel93 6:ccdbd5923e37 140
vinbel93 6:ccdbd5923e37 141 wait(0.4);
vinbel93 6:ccdbd5923e37 142 }
vinbel93 6:ccdbd5923e37 143 }