main.cpp

00001 #include "mbed.h"
00002 #include "LIS302.h"
00003 #include "MobileLCD.h"
00004 #define pi 3.141592653589793238462643383279502884197
00005 
00006 DigitalIn din (20);
00007 MobileLCD lcd (5,6,7,8,9);
00008 LIS302 acc (11,12,13,14);
00009 
00010 //gloable Var's
00011 float x; 
00012 float y; 
00013 float numx; 
00014 float numy; 
00015 float numxmid; 
00016 float numymid; 
00017 float xi; 
00018 float yi;
00019 //float d;
00020 
00021 //Functions
00022 float calculateangle(float, float);
00023 float turnpos(float);
00024 void display(void);
00025 float average(float);
00026 
00027 //Main Prog
00028 int main() {
00029 
00030 while (1) {
00031 
00032 //read in from accelaromitor and filter the result
00033 x = acc.x();
00034 x= average(x);
00035 y = acc.y();
00036 y= average(y);
00037 
00038 //Call calculate angle
00039 numx = calculateangle(x,y);
00040 numy = calculateangle(y,x);
00041 
00042 //inverse direction of bubbles
00043 numx *= -1;
00044 numy *= -1;
00045 
00046 //Button Press
00047 if (din == 1)
00048 {
00049     xi = numx ;
00050     yi = numy - 99;
00051 }
00052 
00053 numx -= xi;
00054 numy -= yi;
00055 
00056 //set bubble to middle
00057 numxmid = ((numx/90)*130) + 64;
00058 numymid = ((numy/90)*130) + 64;
00059 
00060 //Turn negative numbers positive
00061 numx = turnpos(numx);
00062 numy = turnpos(numy);
00063 
00064 //setup display
00065 display();
00066 
00067 }
00068 }//End of main program
00069 
00070 
00071 //angle calculations
00072 float calculateangle(float i, float j)
00073 {
00074 float num;
00075 
00076     num = pi * 20 *(atan((i/j)));
00077 
00078 return num;
00079 }
00080 
00081 //Turn Number Positive
00082 float turnpos(float l)
00083 {
00084 if (l < 0)
00085 {
00086     l *= -1;
00087 }
00088 
00089 return l;
00090 }
00091 
00092 //Set up Display
00093 void display (void)
00094 {
00095 lcd.background(0x000000);
00096 
00097 if (numy < 50)
00098 {
00099     //black tollarence lines
00100     //xline
00101     lcd.fill (0, 8, 130, 1, 0xffffff);
00102     lcd.fill (60 , 0, 1, 8, 0xffffff);
00103     lcd.fill (70 , 0, 1, 8, 0xffffff);
00104     
00105     //x bubble
00106     lcd.fill (numymid , 2, 4, 4, 0x00ffbb);
00107 }
00108     else    
00109     {
00110         //black tollarence lines
00111         //yline
00112         lcd.fill (8, 0, 1, 130, 0xffffff);
00113         lcd.fill (0, 60 , 8, 1, 0xffffff);
00114         lcd.fill (0, 70 , 8, 1, 0xffffff);
00115         
00116         //y bubble
00117         lcd.fill (2 , numxmid, 4, 4, 0xffbb00);
00118     }
00119 
00120 //Display angle in middle of the screen
00121 lcd.locate(3,7);
00122 lcd.printf("Top  = %.0f ", numy);
00123 
00124 lcd.locate(3,9);
00125 lcd.printf("Left = %.0f ", numx);
00126 
00127 //set refresh rate
00128 wait(0.3);
00129 lcd.cls();
00130 }
00131 
00132 //Digital Filter
00133 float average(float i)
00134 {
00135 float av = 0;
00136 for (int j=0; j < 50; j++)
00137 {
00138 av += i;
00139 av /=50;
00140 }
00141 return av;
00142 }