David Buck
/
Digital_level
main.cpp
- Committer:
- Kaikestu
- Date:
- 2009-12-09
- Revision:
- 0:ffae044d3949
File content as of revision 0:ffae044d3949:
#include "mbed.h" #include "LIS302.h" #include "MobileLCD.h" #define pi 3.141592653589793238462643383279502884197 DigitalIn din (20); MobileLCD lcd (5,6,7,8,9); LIS302 acc (11,12,13,14); //gloable Var's float x; float y; float numx; float numy; float numxmid; float numymid; float xi; float yi; //float d; //Functions float calculateangle(float, float); float turnpos(float); void display(void); float average(float); //Main Prog int main() { while (1) { //read in from accelaromitor and filter the result x = acc.x(); x= average(x); y = acc.y(); y= average(y); //Call calculate angle numx = calculateangle(x,y); numy = calculateangle(y,x); //inverse direction of bubbles numx *= -1; numy *= -1; //Button Press if (din == 1) { xi = numx ; yi = numy - 99; } numx -= xi; numy -= yi; //set bubble to middle numxmid = ((numx/90)*130) + 64; numymid = ((numy/90)*130) + 64; //Turn negative numbers positive numx = turnpos(numx); numy = turnpos(numy); //setup display display(); } }//End of main program //angle calculations float calculateangle(float i, float j) { float num; num = pi * 20 *(atan((i/j))); return num; } //Turn Number Positive float turnpos(float l) { if (l < 0) { l *= -1; } return l; } //Set up Display void display (void) { lcd.background(0x000000); if (numy < 50) { //black tollarence lines //xline lcd.fill (0, 8, 130, 1, 0xffffff); lcd.fill (60 , 0, 1, 8, 0xffffff); lcd.fill (70 , 0, 1, 8, 0xffffff); //x bubble lcd.fill (numymid , 2, 4, 4, 0x00ffbb); } else { //black tollarence lines //yline lcd.fill (8, 0, 1, 130, 0xffffff); lcd.fill (0, 60 , 8, 1, 0xffffff); lcd.fill (0, 70 , 8, 1, 0xffffff); //y bubble lcd.fill (2 , numxmid, 4, 4, 0xffbb00); } //Display angle in middle of the screen lcd.locate(3,7); lcd.printf("Top = %.0f ", numy); lcd.locate(3,9); lcd.printf("Left = %.0f ", numx); //set refresh rate wait(0.3); lcd.cls(); } //Digital Filter float average(float i) { float av = 0; for (int j=0; j < 50; j++) { av += i; av /=50; } return av; }