David Buck
main.cpp@0:ffae044d3949, 2009-12-09 (annotated)
- Committer:
- Kaikestu
- Date:
- Wed Dec 09 13:34:55 2009 +0000
- Revision:
- 0:ffae044d3949
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Kaikestu | 0:ffae044d3949 | 1 | #include "mbed.h" |
| Kaikestu | 0:ffae044d3949 | 2 | #include "LIS302.h" |
| Kaikestu | 0:ffae044d3949 | 3 | #include "MobileLCD.h" |
| Kaikestu | 0:ffae044d3949 | 4 | #define pi 3.141592653589793238462643383279502884197 |
| Kaikestu | 0:ffae044d3949 | 5 | |
| Kaikestu | 0:ffae044d3949 | 6 | DigitalIn din (20); |
| Kaikestu | 0:ffae044d3949 | 7 | MobileLCD lcd (5,6,7,8,9); |
| Kaikestu | 0:ffae044d3949 | 8 | LIS302 acc (11,12,13,14); |
| Kaikestu | 0:ffae044d3949 | 9 | |
| Kaikestu | 0:ffae044d3949 | 10 | //gloable Var's |
| Kaikestu | 0:ffae044d3949 | 11 | float x; |
| Kaikestu | 0:ffae044d3949 | 12 | float y; |
| Kaikestu | 0:ffae044d3949 | 13 | float numx; |
| Kaikestu | 0:ffae044d3949 | 14 | float numy; |
| Kaikestu | 0:ffae044d3949 | 15 | float numxmid; |
| Kaikestu | 0:ffae044d3949 | 16 | float numymid; |
| Kaikestu | 0:ffae044d3949 | 17 | float xi; |
| Kaikestu | 0:ffae044d3949 | 18 | float yi; |
| Kaikestu | 0:ffae044d3949 | 19 | //float d; |
| Kaikestu | 0:ffae044d3949 | 20 | |
| Kaikestu | 0:ffae044d3949 | 21 | //Functions |
| Kaikestu | 0:ffae044d3949 | 22 | float calculateangle(float, float); |
| Kaikestu | 0:ffae044d3949 | 23 | float turnpos(float); |
| Kaikestu | 0:ffae044d3949 | 24 | void display(void); |
| Kaikestu | 0:ffae044d3949 | 25 | float average(float); |
| Kaikestu | 0:ffae044d3949 | 26 | |
| Kaikestu | 0:ffae044d3949 | 27 | //Main Prog |
| Kaikestu | 0:ffae044d3949 | 28 | int main() { |
| Kaikestu | 0:ffae044d3949 | 29 | |
| Kaikestu | 0:ffae044d3949 | 30 | while (1) { |
| Kaikestu | 0:ffae044d3949 | 31 | |
| Kaikestu | 0:ffae044d3949 | 32 | //read in from accelaromitor and filter the result |
| Kaikestu | 0:ffae044d3949 | 33 | x = acc.x(); |
| Kaikestu | 0:ffae044d3949 | 34 | x= average(x); |
| Kaikestu | 0:ffae044d3949 | 35 | y = acc.y(); |
| Kaikestu | 0:ffae044d3949 | 36 | y= average(y); |
| Kaikestu | 0:ffae044d3949 | 37 | |
| Kaikestu | 0:ffae044d3949 | 38 | //Call calculate angle |
| Kaikestu | 0:ffae044d3949 | 39 | numx = calculateangle(x,y); |
| Kaikestu | 0:ffae044d3949 | 40 | numy = calculateangle(y,x); |
| Kaikestu | 0:ffae044d3949 | 41 | |
| Kaikestu | 0:ffae044d3949 | 42 | //inverse direction of bubbles |
| Kaikestu | 0:ffae044d3949 | 43 | numx *= -1; |
| Kaikestu | 0:ffae044d3949 | 44 | numy *= -1; |
| Kaikestu | 0:ffae044d3949 | 45 | |
| Kaikestu | 0:ffae044d3949 | 46 | //Button Press |
| Kaikestu | 0:ffae044d3949 | 47 | if (din == 1) |
| Kaikestu | 0:ffae044d3949 | 48 | { |
| Kaikestu | 0:ffae044d3949 | 49 | xi = numx ; |
| Kaikestu | 0:ffae044d3949 | 50 | yi = numy - 99; |
| Kaikestu | 0:ffae044d3949 | 51 | } |
| Kaikestu | 0:ffae044d3949 | 52 | |
| Kaikestu | 0:ffae044d3949 | 53 | numx -= xi; |
| Kaikestu | 0:ffae044d3949 | 54 | numy -= yi; |
| Kaikestu | 0:ffae044d3949 | 55 | |
| Kaikestu | 0:ffae044d3949 | 56 | //set bubble to middle |
| Kaikestu | 0:ffae044d3949 | 57 | numxmid = ((numx/90)*130) + 64; |
| Kaikestu | 0:ffae044d3949 | 58 | numymid = ((numy/90)*130) + 64; |
| Kaikestu | 0:ffae044d3949 | 59 | |
| Kaikestu | 0:ffae044d3949 | 60 | //Turn negative numbers positive |
| Kaikestu | 0:ffae044d3949 | 61 | numx = turnpos(numx); |
| Kaikestu | 0:ffae044d3949 | 62 | numy = turnpos(numy); |
| Kaikestu | 0:ffae044d3949 | 63 | |
| Kaikestu | 0:ffae044d3949 | 64 | //setup display |
| Kaikestu | 0:ffae044d3949 | 65 | display(); |
| Kaikestu | 0:ffae044d3949 | 66 | |
| Kaikestu | 0:ffae044d3949 | 67 | } |
| Kaikestu | 0:ffae044d3949 | 68 | }//End of main program |
| Kaikestu | 0:ffae044d3949 | 69 | |
| Kaikestu | 0:ffae044d3949 | 70 | |
| Kaikestu | 0:ffae044d3949 | 71 | //angle calculations |
| Kaikestu | 0:ffae044d3949 | 72 | float calculateangle(float i, float j) |
| Kaikestu | 0:ffae044d3949 | 73 | { |
| Kaikestu | 0:ffae044d3949 | 74 | float num; |
| Kaikestu | 0:ffae044d3949 | 75 | |
| Kaikestu | 0:ffae044d3949 | 76 | num = pi * 20 *(atan((i/j))); |
| Kaikestu | 0:ffae044d3949 | 77 | |
| Kaikestu | 0:ffae044d3949 | 78 | return num; |
| Kaikestu | 0:ffae044d3949 | 79 | } |
| Kaikestu | 0:ffae044d3949 | 80 | |
| Kaikestu | 0:ffae044d3949 | 81 | //Turn Number Positive |
| Kaikestu | 0:ffae044d3949 | 82 | float turnpos(float l) |
| Kaikestu | 0:ffae044d3949 | 83 | { |
| Kaikestu | 0:ffae044d3949 | 84 | if (l < 0) |
| Kaikestu | 0:ffae044d3949 | 85 | { |
| Kaikestu | 0:ffae044d3949 | 86 | l *= -1; |
| Kaikestu | 0:ffae044d3949 | 87 | } |
| Kaikestu | 0:ffae044d3949 | 88 | |
| Kaikestu | 0:ffae044d3949 | 89 | return l; |
| Kaikestu | 0:ffae044d3949 | 90 | } |
| Kaikestu | 0:ffae044d3949 | 91 | |
| Kaikestu | 0:ffae044d3949 | 92 | //Set up Display |
| Kaikestu | 0:ffae044d3949 | 93 | void display (void) |
| Kaikestu | 0:ffae044d3949 | 94 | { |
| Kaikestu | 0:ffae044d3949 | 95 | lcd.background(0x000000); |
| Kaikestu | 0:ffae044d3949 | 96 | |
| Kaikestu | 0:ffae044d3949 | 97 | if (numy < 50) |
| Kaikestu | 0:ffae044d3949 | 98 | { |
| Kaikestu | 0:ffae044d3949 | 99 | //black tollarence lines |
| Kaikestu | 0:ffae044d3949 | 100 | //xline |
| Kaikestu | 0:ffae044d3949 | 101 | lcd.fill (0, 8, 130, 1, 0xffffff); |
| Kaikestu | 0:ffae044d3949 | 102 | lcd.fill (60 , 0, 1, 8, 0xffffff); |
| Kaikestu | 0:ffae044d3949 | 103 | lcd.fill (70 , 0, 1, 8, 0xffffff); |
| Kaikestu | 0:ffae044d3949 | 104 | |
| Kaikestu | 0:ffae044d3949 | 105 | //x bubble |
| Kaikestu | 0:ffae044d3949 | 106 | lcd.fill (numymid , 2, 4, 4, 0x00ffbb); |
| Kaikestu | 0:ffae044d3949 | 107 | } |
| Kaikestu | 0:ffae044d3949 | 108 | else |
| Kaikestu | 0:ffae044d3949 | 109 | { |
| Kaikestu | 0:ffae044d3949 | 110 | //black tollarence lines |
| Kaikestu | 0:ffae044d3949 | 111 | //yline |
| Kaikestu | 0:ffae044d3949 | 112 | lcd.fill (8, 0, 1, 130, 0xffffff); |
| Kaikestu | 0:ffae044d3949 | 113 | lcd.fill (0, 60 , 8, 1, 0xffffff); |
| Kaikestu | 0:ffae044d3949 | 114 | lcd.fill (0, 70 , 8, 1, 0xffffff); |
| Kaikestu | 0:ffae044d3949 | 115 | |
| Kaikestu | 0:ffae044d3949 | 116 | //y bubble |
| Kaikestu | 0:ffae044d3949 | 117 | lcd.fill (2 , numxmid, 4, 4, 0xffbb00); |
| Kaikestu | 0:ffae044d3949 | 118 | } |
| Kaikestu | 0:ffae044d3949 | 119 | |
| Kaikestu | 0:ffae044d3949 | 120 | //Display angle in middle of the screen |
| Kaikestu | 0:ffae044d3949 | 121 | lcd.locate(3,7); |
| Kaikestu | 0:ffae044d3949 | 122 | lcd.printf("Top = %.0f ", numy); |
| Kaikestu | 0:ffae044d3949 | 123 | |
| Kaikestu | 0:ffae044d3949 | 124 | lcd.locate(3,9); |
| Kaikestu | 0:ffae044d3949 | 125 | lcd.printf("Left = %.0f ", numx); |
| Kaikestu | 0:ffae044d3949 | 126 | |
| Kaikestu | 0:ffae044d3949 | 127 | //set refresh rate |
| Kaikestu | 0:ffae044d3949 | 128 | wait(0.3); |
| Kaikestu | 0:ffae044d3949 | 129 | lcd.cls(); |
| Kaikestu | 0:ffae044d3949 | 130 | } |
| Kaikestu | 0:ffae044d3949 | 131 | |
| Kaikestu | 0:ffae044d3949 | 132 | //Digital Filter |
| Kaikestu | 0:ffae044d3949 | 133 | float average(float i) |
| Kaikestu | 0:ffae044d3949 | 134 | { |
| Kaikestu | 0:ffae044d3949 | 135 | float av = 0; |
| Kaikestu | 0:ffae044d3949 | 136 | for (int j=0; j < 50; j++) |
| Kaikestu | 0:ffae044d3949 | 137 | { |
| Kaikestu | 0:ffae044d3949 | 138 | av += i; |
| Kaikestu | 0:ffae044d3949 | 139 | av /=50; |
| Kaikestu | 0:ffae044d3949 | 140 | } |
| Kaikestu | 0:ffae044d3949 | 141 | return av; |
| Kaikestu | 0:ffae044d3949 | 142 | } |