Tadao Iida
エレキジャック Web版 mbedで初めてのマイコン開発 センサを使ってみよう 「CHOROQ HYBRIDを使って赤外線リモコン制御の基礎を学ぶ」の記事で作成したCHOROQ HYBRIDをオリジナルのジョイスティックリモコンで操作するプログラムです。赤外線リモコンを製作するための基礎が分かります。 http://www.eleki-jack.com/arm/2011/06/mbed-45choroq-hybrid.html http://www.eleki-jack.com/arm/2011/06/mbed-55choroq-hybrid.html
main.cpp@0:a28199d8881a, 2011-06-26 (annotated)
- Committer:
- sunifu
- Date:
- Sun Jun 26 13:47:55 2011 +0000
- Revision:
- 0:a28199d8881a
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| sunifu | 0:a28199d8881a | 1 | #include "mbed.h" |
| sunifu | 0:a28199d8881a | 2 | #include "TextLCD.h" |
| sunifu | 0:a28199d8881a | 3 | |
| sunifu | 0:a28199d8881a | 4 | #define U_THR 0.65 |
| sunifu | 0:a28199d8881a | 5 | #define D_THR 0.2 |
| sunifu | 0:a28199d8881a | 6 | #define L_THR 0.2 |
| sunifu | 0:a28199d8881a | 7 | #define R_THR 0.65 |
| sunifu | 0:a28199d8881a | 8 | #define H_THR 0.6 |
| sunifu | 0:a28199d8881a | 9 | #define CH_THR 0.6 |
| sunifu | 0:a28199d8881a | 10 | |
| sunifu | 0:a28199d8881a | 11 | #define HEADER 2 |
| sunifu | 0:a28199d8881a | 12 | #define CHA_INTERVAL1 130 // 130 |
| sunifu | 0:a28199d8881a | 13 | #define CHA_INTERVAL2 10 // 10 |
| sunifu | 0:a28199d8881a | 14 | #define CHB_INTERVAL1 110 // 130 |
| sunifu | 0:a28199d8881a | 15 | #define CHB_INTERVAL2 30 // 10 |
| sunifu | 0:a28199d8881a | 16 | #define CHC_INTERVAL1 90 // 130 |
| sunifu | 0:a28199d8881a | 17 | #define CHC_INTERVAL2 50 // 10 |
| sunifu | 0:a28199d8881a | 18 | #define CHD_INTERVAL1 70 // 130 |
| sunifu | 0:a28199d8881a | 19 | #define CHD_INTERVAL2 70 // 10 |
| sunifu | 0:a28199d8881a | 20 | |
| sunifu | 0:a28199d8881a | 21 | typedef enum { OFF, ON } state; |
| sunifu | 0:a28199d8881a | 22 | |
| sunifu | 0:a28199d8881a | 23 | TextLCD lcd(p24, p26, p27, p28, p29, p30); |
| sunifu | 0:a28199d8881a | 24 | PwmOut out(p21); |
| sunifu | 0:a28199d8881a | 25 | AnalogIn lr_in(p18); // Joystick Left,Right -> Left+Right |
| sunifu | 0:a28199d8881a | 26 | AnalogIn ud_in(p19); // Joystick Up,Down -> Forward+Backward |
| sunifu | 0:a28199d8881a | 27 | AnalogIn hs_in(p20); // PSW hs -> Dash! |
| sunifu | 0:a28199d8881a | 28 | AnalogIn ch_in(p17); // PSW ch -> Change CH A,B,C,D |
| sunifu | 0:a28199d8881a | 29 | |
| sunifu | 0:a28199d8881a | 30 | typedef void (*FUNCPTR)(); |
| sunifu | 0:a28199d8881a | 31 | FUNCPTR pFuncBand; |
| sunifu | 0:a28199d8881a | 32 | // Remote Control Signal ON |
| sunifu | 0:a28199d8881a | 33 | void on(){ |
| sunifu | 0:a28199d8881a | 34 | int begin; |
| sunifu | 0:a28199d8881a | 35 | Timer ton; |
| sunifu | 0:a28199d8881a | 36 | ton.start(); |
| sunifu | 0:a28199d8881a | 37 | begin = ton.read_us(); |
| sunifu | 0:a28199d8881a | 38 | while ( 500 > ton.read_us() - begin ) |
| sunifu | 0:a28199d8881a | 39 | out.write(0.0f) ; |
| sunifu | 0:a28199d8881a | 40 | begin = ton.read_us(); |
| sunifu | 0:a28199d8881a | 41 | while ( 1000 > ton.read_us() - begin ) |
| sunifu | 0:a28199d8881a | 42 | out.write(0.5f) ; |
| sunifu | 0:a28199d8881a | 43 | ton.stop(); |
| sunifu | 0:a28199d8881a | 44 | } |
| sunifu | 0:a28199d8881a | 45 | |
| sunifu | 0:a28199d8881a | 46 | // Remote Control Signal OFF |
| sunifu | 0:a28199d8881a | 47 | void off(){ |
| sunifu | 0:a28199d8881a | 48 | int begin; |
| sunifu | 0:a28199d8881a | 49 | Timer toff; |
| sunifu | 0:a28199d8881a | 50 | toff.start(); |
| sunifu | 0:a28199d8881a | 51 | begin = toff.read_us(); |
| sunifu | 0:a28199d8881a | 52 | while( 500 > toff.read_us() - begin ) |
| sunifu | 0:a28199d8881a | 53 | out.write(0.0f) ; |
| sunifu | 0:a28199d8881a | 54 | |
| sunifu | 0:a28199d8881a | 55 | begin = toff.read_us(); |
| sunifu | 0:a28199d8881a | 56 | while ( 500 > toff.read_us() - begin ) |
| sunifu | 0:a28199d8881a | 57 | out.write(0.5f) ; |
| sunifu | 0:a28199d8881a | 58 | toff.stop() ; |
| sunifu | 0:a28199d8881a | 59 | } |
| sunifu | 0:a28199d8881a | 60 | |
| sunifu | 0:a28199d8881a | 61 | int flag=0; |
| sunifu | 0:a28199d8881a | 62 | void tail() |
| sunifu | 0:a28199d8881a | 63 | { |
| sunifu | 0:a28199d8881a | 64 | out.write(0.0f) ; |
| sunifu | 0:a28199d8881a | 65 | flag = 0; |
| sunifu | 0:a28199d8881a | 66 | } |
| sunifu | 0:a28199d8881a | 67 | |
| sunifu | 0:a28199d8881a | 68 | void bandA(){ |
| sunifu | 0:a28199d8881a | 69 | off();off(); |
| sunifu | 0:a28199d8881a | 70 | } |
| sunifu | 0:a28199d8881a | 71 | void bandB(){ |
| sunifu | 0:a28199d8881a | 72 | off();on(); |
| sunifu | 0:a28199d8881a | 73 | } |
| sunifu | 0:a28199d8881a | 74 | void bandC(){ |
| sunifu | 0:a28199d8881a | 75 | on();off(); |
| sunifu | 0:a28199d8881a | 76 | } |
| sunifu | 0:a28199d8881a | 77 | void bandD(){ |
| sunifu | 0:a28199d8881a | 78 | on();on(); |
| sunifu | 0:a28199d8881a | 79 | } |
| sunifu | 0:a28199d8881a | 80 | |
| sunifu | 0:a28199d8881a | 81 | Ticker in; |
| sunifu | 0:a28199d8881a | 82 | Timer timer; |
| sunifu | 0:a28199d8881a | 83 | float lr, ud, hs ; |
| sunifu | 0:a28199d8881a | 84 | volatile float ch; |
| sunifu | 0:a28199d8881a | 85 | int interval1, interval2; |
| sunifu | 0:a28199d8881a | 86 | |
| sunifu | 0:a28199d8881a | 87 | // Joystick Sensor |
| sunifu | 0:a28199d8881a | 88 | void input() |
| sunifu | 0:a28199d8881a | 89 | { |
| sunifu | 0:a28199d8881a | 90 | lr = lr_in; |
| sunifu | 0:a28199d8881a | 91 | ud = ud_in; |
| sunifu | 0:a28199d8881a | 92 | hs = hs_in; |
| sunifu | 0:a28199d8881a | 93 | ch = ch_in ; |
| sunifu | 0:a28199d8881a | 94 | } |
| sunifu | 0:a28199d8881a | 95 | |
| sunifu | 0:a28199d8881a | 96 | volatile int psw = 0; |
| sunifu | 0:a28199d8881a | 97 | |
| sunifu | 0:a28199d8881a | 98 | void interval(int iTime,state s){ |
| sunifu | 0:a28199d8881a | 99 | float signal; |
| sunifu | 0:a28199d8881a | 100 | int t; |
| sunifu | 0:a28199d8881a | 101 | signal = (s==OFF)? 0.0f : 0.5f; |
| sunifu | 0:a28199d8881a | 102 | timer.start(); |
| sunifu | 0:a28199d8881a | 103 | t = timer.read_ms(); |
| sunifu | 0:a28199d8881a | 104 | while ( iTime > timer.read_ms() - t ) |
| sunifu | 0:a28199d8881a | 105 | out.write(signal) ; |
| sunifu | 0:a28199d8881a | 106 | timer.stop(); |
| sunifu | 0:a28199d8881a | 107 | } |
| sunifu | 0:a28199d8881a | 108 | |
| sunifu | 0:a28199d8881a | 109 | int main() { |
| sunifu | 0:a28199d8881a | 110 | in.attach(&input, 0.05); |
| sunifu | 0:a28199d8881a | 111 | |
| sunifu | 0:a28199d8881a | 112 | out.period_us(26); |
| sunifu | 0:a28199d8881a | 113 | lcd.cls() ; |
| sunifu | 0:a28199d8881a | 114 | lcd.locate(1,1); |
| sunifu | 0:a28199d8881a | 115 | lcd.printf("BAND [A]"); |
| sunifu | 0:a28199d8881a | 116 | interval1 = CHA_INTERVAL1; |
| sunifu | 0:a28199d8881a | 117 | interval2 = CHA_INTERVAL2; |
| sunifu | 0:a28199d8881a | 118 | (pFuncBand) = &bandA; |
| sunifu | 0:a28199d8881a | 119 | |
| sunifu | 0:a28199d8881a | 120 | while(1) { |
| sunifu | 0:a28199d8881a | 121 | // #DEBUG printf("UD%4.2f HS%4.2f ",ud,hs); |
| sunifu | 0:a28199d8881a | 122 | // #DEBUG printf("\tLR%4.2f CH%1d %4.2f \r\n",lr,psw,ch); |
| sunifu | 0:a28199d8881a | 123 | if( ch >= CH_THR ){ |
| sunifu | 0:a28199d8881a | 124 | while(1){ |
| sunifu | 0:a28199d8881a | 125 | if(ch < 0.2 ){ |
| sunifu | 0:a28199d8881a | 126 | if ( psw < 3 ){ |
| sunifu | 0:a28199d8881a | 127 | psw++; |
| sunifu | 0:a28199d8881a | 128 | }else{ |
| sunifu | 0:a28199d8881a | 129 | psw=0; |
| sunifu | 0:a28199d8881a | 130 | } |
| sunifu | 0:a28199d8881a | 131 | break; |
| sunifu | 0:a28199d8881a | 132 | } |
| sunifu | 0:a28199d8881a | 133 | } |
| sunifu | 0:a28199d8881a | 134 | |
| sunifu | 0:a28199d8881a | 135 | lcd.locate(1,1); |
| sunifu | 0:a28199d8881a | 136 | |
| sunifu | 0:a28199d8881a | 137 | |
| sunifu | 0:a28199d8881a | 138 | if(psw == 0 ){ |
| sunifu | 0:a28199d8881a | 139 | lcd.printf("BAND [A]"); printf("banda\r\n"); |
| sunifu | 0:a28199d8881a | 140 | (pFuncBand) = &bandA; |
| sunifu | 0:a28199d8881a | 141 | interval1 = CHA_INTERVAL1; |
| sunifu | 0:a28199d8881a | 142 | interval2 = CHA_INTERVAL2; |
| sunifu | 0:a28199d8881a | 143 | }else if(psw == 1){ |
| sunifu | 0:a28199d8881a | 144 | lcd.printf("BAND [B]"); |
| sunifu | 0:a28199d8881a | 145 | (pFuncBand) = &bandB; |
| sunifu | 0:a28199d8881a | 146 | interval1 = CHB_INTERVAL1; |
| sunifu | 0:a28199d8881a | 147 | interval2 = CHB_INTERVAL2; |
| sunifu | 0:a28199d8881a | 148 | }else if(psw == 2){; |
| sunifu | 0:a28199d8881a | 149 | lcd.printf("BAND [C]"); |
| sunifu | 0:a28199d8881a | 150 | pFuncBand = &bandC; |
| sunifu | 0:a28199d8881a | 151 | interval1 = CHC_INTERVAL1; |
| sunifu | 0:a28199d8881a | 152 | interval2 = CHC_INTERVAL2; |
| sunifu | 0:a28199d8881a | 153 | }else{ |
| sunifu | 0:a28199d8881a | 154 | lcd.printf("BAND [D]"); printf("band D\r\n"); |
| sunifu | 0:a28199d8881a | 155 | pFuncBand = &bandD; |
| sunifu | 0:a28199d8881a | 156 | interval1 = CHD_INTERVAL1; |
| sunifu | 0:a28199d8881a | 157 | interval2 = CHD_INTERVAL2; |
| sunifu | 0:a28199d8881a | 158 | } |
| sunifu | 0:a28199d8881a | 159 | } |
| sunifu | 0:a28199d8881a | 160 | lcd.locate(0,0); |
| sunifu | 0:a28199d8881a | 161 | if ( (U_THR < ud && H_THR < hs) && (L_THR < lr && lr < R_THR)){ |
| sunifu | 0:a28199d8881a | 162 | // Forward + Dash!(0101) |
| sunifu | 0:a28199d8881a | 163 | lcd.printf("Forward+Dash! "); |
| sunifu | 0:a28199d8881a | 164 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 165 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 166 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 167 | off(); on(); off();on(); |
| sunifu | 0:a28199d8881a | 168 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 169 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 170 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 171 | off(); on(); off(); on(); |
| sunifu | 0:a28199d8881a | 172 | tail(); |
| sunifu | 0:a28199d8881a | 173 | }else if((ud < D_THR && H_THR < hs) && (L_THR < lr && lr < R_THR)){ |
| sunifu | 0:a28199d8881a | 174 | // Backward + Dash!(1100) |
| sunifu | 0:a28199d8881a | 175 | lcd.printf("Backward+Dash! "); |
| sunifu | 0:a28199d8881a | 176 | interval(interval1,OFF); // chA Interval1 |
| sunifu | 0:a28199d8881a | 177 | interval(HEADER,ON); //HEADER |
| sunifu | 0:a28199d8881a | 178 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 179 | on(); on(); off();off(); // Control Code 1100 |
| sunifu | 0:a28199d8881a | 180 | interval(interval2,OFF); // chA Interval2 |
| sunifu | 0:a28199d8881a | 181 | interval(HEADER,ON); // HEADER |
| sunifu | 0:a28199d8881a | 182 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 183 | on(); on(); off(); off(); // Control Code 1100 |
| sunifu | 0:a28199d8881a | 184 | tail(); |
| sunifu | 0:a28199d8881a | 185 | }else if ((U_THR < ud && H_THR < hs) && (lr < L_THR)){ |
| sunifu | 0:a28199d8881a | 186 | // Forward + Left + Dash! |
| sunifu | 0:a28199d8881a | 187 | lcd.printf("For+Left+Dash! "); |
| sunifu | 0:a28199d8881a | 188 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 189 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 190 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 191 | on(); off(); off(); off(); |
| sunifu | 0:a28199d8881a | 192 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 193 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 194 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 195 | on(); off(); off(); off(); |
| sunifu | 0:a28199d8881a | 196 | tail(); |
| sunifu | 0:a28199d8881a | 197 | }else if((U_THR < ud && H_THR < hs) && (R_THR < lr)){ |
| sunifu | 0:a28199d8881a | 198 | // Forward + Right + Dash!; |
| sunifu | 0:a28199d8881a | 199 | lcd.printf("For+Right+Dash! "); |
| sunifu | 0:a28199d8881a | 200 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 201 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 202 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 203 | on(); off(); off(); on(); |
| sunifu | 0:a28199d8881a | 204 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 205 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 206 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 207 | on(); off(); off(); on(); |
| sunifu | 0:a28199d8881a | 208 | tail(); |
| sunifu | 0:a28199d8881a | 209 | }else if((ud < D_THR && H_THR < hs) && (lr < L_THR)){ |
| sunifu | 0:a28199d8881a | 210 | // Backward + Left + Dash! |
| sunifu | 0:a28199d8881a | 211 | lcd.printf("Back+Left+Dash! "); |
| sunifu | 0:a28199d8881a | 212 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 213 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 214 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 215 | on(); on(); off(); on(); |
| sunifu | 0:a28199d8881a | 216 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 217 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 218 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 219 | on(); on(); off(); on(); |
| sunifu | 0:a28199d8881a | 220 | tail(); |
| sunifu | 0:a28199d8881a | 221 | }else if((ud < D_THR && H_THR < hs) && (R_THR < lr)){ |
| sunifu | 0:a28199d8881a | 222 | // Backward + Right + Dash! |
| sunifu | 0:a28199d8881a | 223 | lcd.printf("Back+Right+Dash!"); |
| sunifu | 0:a28199d8881a | 224 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 225 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 226 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 227 | on(); on(); on(); off(); |
| sunifu | 0:a28199d8881a | 228 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 229 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 230 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 231 | on(); on(); on(); off(); |
| sunifu | 0:a28199d8881a | 232 | tail(); |
| sunifu | 0:a28199d8881a | 233 | }else if ( U_THR < ud && lr < L_THR ){ |
| sunifu | 0:a28199d8881a | 234 | // Forward + Left |
| sunifu | 0:a28199d8881a | 235 | lcd.printf("Forward+Left "); |
| sunifu | 0:a28199d8881a | 236 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 237 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 238 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 239 | off(); on(); on(); off(); |
| sunifu | 0:a28199d8881a | 240 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 241 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 242 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 243 | off(); on(); on(); off(); |
| sunifu | 0:a28199d8881a | 244 | tail(); |
| sunifu | 0:a28199d8881a | 245 | }else if( U_THR < ud && R_THR < lr){ |
| sunifu | 0:a28199d8881a | 246 | // Forward + Right |
| sunifu | 0:a28199d8881a | 247 | lcd.printf("Forward+Right "); |
| sunifu | 0:a28199d8881a | 248 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 249 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 250 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 251 | off(); on(); on(); on(); |
| sunifu | 0:a28199d8881a | 252 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 253 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 254 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 255 | off(); on(); on(); on(); |
| sunifu | 0:a28199d8881a | 256 | tail(); |
| sunifu | 0:a28199d8881a | 257 | }else if( ud < D_THR && lr < L_THR){ |
| sunifu | 0:a28199d8881a | 258 | // Backward+Left(1010) |
| sunifu | 0:a28199d8881a | 259 | lcd.printf("Backward+Left "); |
| sunifu | 0:a28199d8881a | 260 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 261 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 262 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 263 | on(); off(); on(); off(); |
| sunifu | 0:a28199d8881a | 264 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 265 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 266 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 267 | on(); off(); on(); off(); |
| sunifu | 0:a28199d8881a | 268 | tail(); |
| sunifu | 0:a28199d8881a | 269 | }else if (ud < D_THR && R_THR < lr){ |
| sunifu | 0:a28199d8881a | 270 | // Backward + Right (1011) |
| sunifu | 0:a28199d8881a | 271 | lcd.printf("Backward+Right "); |
| sunifu | 0:a28199d8881a | 272 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 273 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 274 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 275 | on(); off(); on(); on(); |
| sunifu | 0:a28199d8881a | 276 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 277 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 278 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 279 | on(); off(); on(); on(); |
| sunifu | 0:a28199d8881a | 280 | tail(); |
| sunifu | 0:a28199d8881a | 281 | }else if((U_THR <= ud)&&(L_THR < lr && lr < R_THR)){ |
| sunifu | 0:a28199d8881a | 282 | // Forward(0001) |
| sunifu | 0:a28199d8881a | 283 | lcd.printf("Forward "); |
| sunifu | 0:a28199d8881a | 284 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 285 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 286 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 287 | off();off();off();on(); |
| sunifu | 0:a28199d8881a | 288 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 289 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 290 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 291 | off();off();off();on(); |
| sunifu | 0:a28199d8881a | 292 | tail(); |
| sunifu | 0:a28199d8881a | 293 | }else if((ud <= D_THR)&&(L_THR < lr && lr < R_THR)){ |
| sunifu | 0:a28199d8881a | 294 | // Backward(0010) |
| sunifu | 0:a28199d8881a | 295 | lcd.printf("Backward "); |
| sunifu | 0:a28199d8881a | 296 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 297 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 298 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 299 | off();off();on();off(); |
| sunifu | 0:a28199d8881a | 300 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 301 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 302 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 303 | off();off();on();off(); |
| sunifu | 0:a28199d8881a | 304 | tail(); |
| sunifu | 0:a28199d8881a | 305 | }else if(lr < L_THR && (D_THR <= ud && ud <= U_THR)){ |
| sunifu | 0:a28199d8881a | 306 | // Left(0011) |
| sunifu | 0:a28199d8881a | 307 | lcd.printf("Left <= "); |
| sunifu | 0:a28199d8881a | 308 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 309 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 310 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 311 | off();off();on();on(); |
| sunifu | 0:a28199d8881a | 312 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 313 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 314 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 315 | off();off();on();on(); |
| sunifu | 0:a28199d8881a | 316 | tail(); |
| sunifu | 0:a28199d8881a | 317 | }else if(R_THR < lr && (D_THR <= ud && ud <= U_THR)){ |
| sunifu | 0:a28199d8881a | 318 | // Right(0100) |
| sunifu | 0:a28199d8881a | 319 | lcd.printf("Right => "); |
| sunifu | 0:a28199d8881a | 320 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 321 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 322 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 323 | off();on();off();off(); |
| sunifu | 0:a28199d8881a | 324 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 325 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 326 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 327 | off();on();off();off(); |
| sunifu | 0:a28199d8881a | 328 | tail(); |
| sunifu | 0:a28199d8881a | 329 | }else if((D_THR <= ud && ud <= U_THR) && (L_THR < lr && lr < R_THR)){ |
| sunifu | 0:a28199d8881a | 330 | if ( flag == 0 ){ |
| sunifu | 0:a28199d8881a | 331 | for ( int i = 0; i < 5 ; i++ ){ |
| sunifu | 0:a28199d8881a | 332 | // STOP(1111); |
| sunifu | 0:a28199d8881a | 333 | lcd.locate(0,0); |
| sunifu | 0:a28199d8881a | 334 | lcd.printf("Stop "); |
| sunifu | 0:a28199d8881a | 335 | interval(interval1,OFF); |
| sunifu | 0:a28199d8881a | 336 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 337 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 338 | on();on();on();on(); |
| sunifu | 0:a28199d8881a | 339 | interval(interval2,OFF); |
| sunifu | 0:a28199d8881a | 340 | interval(HEADER,ON); |
| sunifu | 0:a28199d8881a | 341 | (pFuncBand)(); |
| sunifu | 0:a28199d8881a | 342 | on();on();on();on(); |
| sunifu | 0:a28199d8881a | 343 | } |
| sunifu | 0:a28199d8881a | 344 | flag = 1; |
| sunifu | 0:a28199d8881a | 345 | }else{ |
| sunifu | 0:a28199d8881a | 346 | out.write(0.0f) ; |
| sunifu | 0:a28199d8881a | 347 | lcd.locate(0,0); |
| sunifu | 0:a28199d8881a | 348 | lcd.printf(" "); |
| sunifu | 0:a28199d8881a | 349 | } |
| sunifu | 0:a28199d8881a | 350 | } |
| sunifu | 0:a28199d8881a | 351 | } |
| sunifu | 0:a28199d8881a | 352 | } |