Vincent Belanger / Mbed 2 deprecated APP

APP 1 S5.

Dependencies:   mbed

APP.cpp@7:b00843da0530, 2016-01-10 (annotated)

Committer:
vinbel93
Date:
Sun Jan 10 22:28:48 2016 +0000
Revision:
7:b00843da0530
Child:
11:eafa5c07f060
Move defines to header

Who changed what in which revision?

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