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