APP.cpp

00001 // APP.cpp
00002 // Vincent Belanger - belv1802
00003 // Jeremy Pare      - parj2713
00004 
00005 #include <math.h>
00006 #include <stdint.h>
00007 
00008 #include "mbed.h"
00009 #include "APP.h"
00010 
00011 
00012 static I2C    i2c(p28, p27);
00013 static Serial uart(p9, p10);
00014 
00015 void setup()
00016 {
00017     // Setup UART and I2C.
00018     uart.format(8, Serial::None, 1);
00019     uart.baud(9600);
00020     i2c.frequency(100000);
00021  
00022     // Clear display
00023     uart.putc(0x76);
00024  
00025     // Decimal point for display
00026     uart.putc(0x77);
00027     uart.putc(0x02);
00028  
00029     // Maximum brightness
00030     uart.putc(0x7A);
00031     uart.putc(0xFF);
00032 }
00033  
00034 void activateAccelerometer()
00035 {
00036     uint8_t data[2] = {CTRL_REG, 0x01};
00037     i2c.write(ACC_ADDRESS, (char*) data, 2);
00038 }
00039  
00040 uint8_t readStatus()
00041 {
00042     uint8_t result[1];
00043     uint8_t reg[1] = {STATUS_REG};
00044     i2c.write(ACC_ADDRESS, (char*) reg, 1, true);
00045     i2c.read(ACC_ADDRESS, (char*) result, 1);
00046  
00047     return result[0];
00048 }    
00049  
00050 int16_t convertToAcceleration(uint8_t hi, uint8_t lo)
00051 {
00052     int16_t value = (lo >> 2) | (hi << 6);
00053     if (value > UINT14_MAX/2)
00054     {
00055         value -= UINT14_MAX;
00056     }
00057  
00058     return value;
00059 }
00060  
00061 int16_t readAndComputeAngle()
00062 {
00063     uint8_t result[6];
00064     uint8_t reg[1] = {ACC_X_REG};
00065     i2c.write(ACC_ADDRESS, (char*) reg, 1, true);
00066     i2c.read(ACC_ADDRESS, (char*) result, 6);
00067     int16_t accX = convertToAcceleration(result[0], result[1]);
00068     int16_t accY = convertToAcceleration(result[2], result[3]);
00069     int16_t accZ = convertToAcceleration(result[4], result[5]);
00070 
00071     float angle = acos((float) accZ / sqrt((float) accX*accX + accY*accY + accZ*accZ)) * 180.0 / PI;
00072  
00073     // To use integer operations instead of floating-point arithmetic, the angle is multiplied by 100.
00074     // This allows for two decimals of precision on the display.
00075     int16_t angleHundreds = angle * 100;
00076     if (angleHundreds > 9000)
00077     {
00078         angleHundreds = 18000 - angleHundreds;
00079     }
00080  
00081     return angleHundreds;
00082 }
00083  
00084 void updateDisplay(int16_t angle)
00085 {
00086     uint8_t digits[4];
00087     sprintf((char*) digits, "%d", angle);
00088 
00089     if (angle < 100)
00090     {
00091         uart.putc('0');
00092         uart.putc('0');
00093         uart.putc(digits[0]);
00094         uart.putc(digits[1]);
00095     }
00096     else if (angle < 1000)
00097     {
00098         uart.putc('0');
00099         uart.putc(digits[0]);
00100         uart.putc(digits[1]);
00101         uart.putc(digits[2]);
00102     }
00103     else
00104     {
00105         uart.putc(digits[0]);
00106         uart.putc(digits[1]);
00107         uart.putc(digits[2]);
00108         uart.putc(digits[3]);
00109     }
00110 }
00111  
00112 int32_t main()
00113 {
00114     setup();
00115     activateAccelerometer();
00116  
00117     while (true)
00118     {
00119         // Read bit 3 from the status register
00120         int8_t status = (readStatus() >> 3) & 0x01;
00121  
00122         if (status == 1)
00123         {
00124             // Get accelerations, compute angle
00125             uint16_t angle = readAndComputeAngle();
00126  
00127             // Update 7 segment display
00128             updateDisplay(angle);
00129         }
00130  
00131         wait(0.4);
00132     }
00133 }