Vincent Belanger
/
APP
APP 1 S5.
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?
User | Revision | Line number | New 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 | } |