Vincent Belanger / Mbed 2 deprecated APP

APP 1 S5.

Dependencies:   mbed

APP.cpp@9:f35a5cde61a3, 2016-01-11 (annotated)

Committer:
vinbel93
Date:
Mon Jan 11 19:07:16 2016 +0000
Revision:
9:f35a5cde61a3
Parent:
6:ccdbd5923e37
Child:
10:f58cbb203c59
Pls work

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