Hajime Muraki / Mbed 2 deprecated chronograph

main

Dependencies:   TextLCD mbed PID

main.cpp@2:59edff92b599, 2014-03-03 (annotated)

Committer:
com3
Date:
Mon Mar 03 00:24:44 2014 +0000
Revision:
2:59edff92b599
Parent:
1:fb4277ce4d93
Child:
3:440e774cc24b
pid

Who changed what in which revision?

UserRevisionLine numberNew contents of line
com3 0:e6d14fec4954 1 #include "mbed.h"
com3 0:e6d14fec4954 2 #include "TextLCD.h"
com3 2:59edff92b599 3 #include "PID.h"
com3 0:e6d14fec4954 4 #include "common.h"
com3 0:e6d14fec4954 5 #include <math.h>
com3 0:e6d14fec4954 6 #include <sstream>
com3 0:e6d14fec4954 7
com3 1:fb4277ce4d93 8 #define MOTOR_P 30
com3 2:59edff92b599 9 #define NO_IR 45
com3 1:fb4277ce4d93 10 #define LINE_LP 30
com3 2:59edff92b599 11 #define LINE_FP 30
com3 1:fb4277ce4d93 12 #define LINE_ON 0xFFF0
com3 1:fb4277ce4d93 13 #define LINE_TIME 0.5
com3 0:e6d14fec4954 14 #define R 1.0
com3 1:fb4277ce4d93 15 #define S_MAX 5
com3 1:fb4277ce4d93 16 #define S1 15
com3 1:fb4277ce4d93 17 #define S2 10
com3 1:fb4277ce4d93 18 #define S3 5
com3 2:59edff92b599 19 #define RATE 0.05
com3 2:59edff92b599 20 #define P_GAIN 0.8
com3 2:59edff92b599 21 #define I_GAIN 0.0
com3 2:59edff92b599 22 #define D_GAIN 0.02
com3 1:fb4277ce4d93 23 //誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
com3 1:fb4277ce4d93 24 #define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )
com3 0:e6d14fec4954 25
com3 1:fb4277ce4d93 26 DigitalIn sw(p5);
com3 2:59edff92b599 27 DigitalIn start(p7);
com3 0:e6d14fec4954 28 DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
com3 0:e6d14fec4954 29 Serial motor(p9,p10);
com3 0:e6d14fec4954 30 Serial sensor(p13,p14);
com3 0:e6d14fec4954 31 Serial pc(USBTX, USBRX);
com3 0:e6d14fec4954 32 TextLCD lcd(p26, p25, p24, p23, p22, p21);
com3 1:fb4277ce4d93 33 AnalogIn adcline[4] = {p16, p17, p19, p20};
com3 2:59edff92b599 34 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
com3 1:fb4277ce4d93 35 Timeout liner0;
com3 1:fb4277ce4d93 36 Timeout liner1;
com3 1:fb4277ce4d93 37 Timeout liner2;
com3 1:fb4277ce4d93 38 Timeout liner3;
com3 2:59edff92b599 39 Ticker pidupdata;
com3 0:e6d14fec4954 40 //HMC6352 dcompass(p9,p10);
com3 0:e6d14fec4954 41
com3 0:e6d14fec4954 42 extern string StringFIN;
com3 0:e6d14fec4954 43 extern void array(int,int,int,int);
com3 0:e6d14fec4954 44 extern void micon_rx(void);
com3 0:e6d14fec4954 45
com3 0:e6d14fec4954 46 //uint16_t analogHex[4] = {0};
com3 0:e6d14fec4954 47 int speed[4] = {0};
com3 1:fb4277ce4d93 48 uint8_t value_ir = 0, ir_num = 0;
com3 0:e6d14fec4954 49 uint8_t ping[4] = {0};
com3 1:fb4277ce4d93 50 uint8_t line[4] = {0}, line_stop[4] = {0};
com3 1:fb4277ce4d93 51 uint8_t back = 0;
com3 0:e6d14fec4954 52 int compass = 0;
com3 0:e6d14fec4954 53 int x = 0, y = 0, s = 0, i = 0, line_on = 0;
com3 0:e6d14fec4954 54 int compassdef = 0, data = 0;
com3 1:fb4277ce4d93 55 uint8_t pingdef[4] = {0};
com3 1:fb4277ce4d93 56
com3 2:59edff92b599 57 double pidinput = 180.0;
com3 2:59edff92b599 58 double compasspid = 0;
com3 2:59edff92b599 59
com3 1:fb4277ce4d93 60 double way[8][2] = {
com3 1:fb4277ce4d93 61 { 0 , 1 },
com3 1:fb4277ce4d93 62 {-0.707, 0.707},
com3 1:fb4277ce4d93 63 {-1 , 0 },
com3 1:fb4277ce4d93 64 {-0.707,-0.707},
com3 1:fb4277ce4d93 65 { 0 ,-1 },
com3 1:fb4277ce4d93 66 { 0.707,-0.707},
com3 1:fb4277ce4d93 67 { 1 , 0 },
com3 1:fb4277ce4d93 68 { 0.707, 0.707}
com3 1:fb4277ce4d93 69 };
com3 0:e6d14fec4954 70
com3 0:e6d14fec4954 71
com3 0:e6d14fec4954 72 void move(int vx, int vy, int vs){
com3 0:e6d14fec4954 73 double pwm[4] = {0};
com3 0:e6d14fec4954 74
com3 0:e6d14fec4954 75 pwm[0] = (double)((vx) + vs);
com3 0:e6d14fec4954 76 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
com3 0:e6d14fec4954 77 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
com3 0:e6d14fec4954 78 pwm[3] = 0;
com3 0:e6d14fec4954 79
com3 0:e6d14fec4954 80 for(i = 0; i < 4; i++){
com3 0:e6d14fec4954 81 if(pwm[i] > 100){
com3 0:e6d14fec4954 82 pwm[i] = 100;
com3 0:e6d14fec4954 83 } else if(pwm[i] < -100){
com3 0:e6d14fec4954 84 pwm[i] = -100;
com3 0:e6d14fec4954 85 }
com3 0:e6d14fec4954 86 speed[i] = pwm[i];
com3 0:e6d14fec4954 87 }
com3 0:e6d14fec4954 88 }
com3 0:e6d14fec4954 89
com3 0:e6d14fec4954 90 //通信（モータ用）
com3 0:e6d14fec4954 91 void tx_motor(){
com3 0:e6d14fec4954 92 array(speed[0],speed[1],speed[3],speed[2]);
com3 0:e6d14fec4954 93 motor.printf("%s",StringFIN.c_str());
com3 0:e6d14fec4954 94 }
com3 0:e6d14fec4954 95
com3 0:e6d14fec4954 96 //ライン判断
com3 0:e6d14fec4954 97 void line_state(){
com3 0:e6d14fec4954 98 if(line[0]){
com3 1:fb4277ce4d93 99 y = -LINE_FP;
com3 0:e6d14fec4954 100 }
com3 0:e6d14fec4954 101 if(line[1]){
com3 1:fb4277ce4d93 102 x = LINE_LP;
com3 0:e6d14fec4954 103 }
com3 0:e6d14fec4954 104 if(line[2]){
com3 1:fb4277ce4d93 105 y = LINE_FP;
com3 0:e6d14fec4954 106 }
com3 0:e6d14fec4954 107 if(line[3]){
com3 1:fb4277ce4d93 108 x = -LINE_LP;
com3 0:e6d14fec4954 109 }
com3 0:e6d14fec4954 110 }
com3 0:e6d14fec4954 111
com3 1:fb4277ce4d93 112 void lcds(int lcd1){
com3 1:fb4277ce4d93 113 lcd.cls();
com3 1:fb4277ce4d93 114 lcd.locate(0, 0);
com3 1:fb4277ce4d93 115 lcd.printf("%d", lcd1);
com3 1:fb4277ce4d93 116 //lcd.locate(2, 1);
com3 1:fb4277ce4d93 117 //lcd.printf("YATTIYATTI");
com3 1:fb4277ce4d93 118 }
com3 1:fb4277ce4d93 119
com3 1:fb4277ce4d93 120 void lcd_ping(){
com3 1:fb4277ce4d93 121 lcd.cls();
com3 1:fb4277ce4d93 122 lcd.locate(0,0);
com3 1:fb4277ce4d93 123 lcd.printf("%03d %03d\n%03d %03d", ping[FRONT], ping[LEFT], ping[BACK], ping[RIGHT]);
com3 1:fb4277ce4d93 124 }
com3 1:fb4277ce4d93 125
com3 1:fb4277ce4d93 126 void lcd_line(){
com3 1:fb4277ce4d93 127 lcd.cls();
com3 1:fb4277ce4d93 128 lcd.locate(0,0);
com3 1:fb4277ce4d93 129 lcd.printf("%03d %03d\n%03d %03d", line[FRONT], line[LEFT], line[BACK], line[RIGHT]);
com3 1:fb4277ce4d93 130 }
com3 1:fb4277ce4d93 131
com3 1:fb4277ce4d93 132
com3 1:fb4277ce4d93 133 void home(){
com3 1:fb4277ce4d93 134 if(ping[LEFT] > 80){
com3 1:fb4277ce4d93 135 x = -20;
com3 1:fb4277ce4d93 136 } else if(ping[RIGHT] > 80){
com3 1:fb4277ce4d93 137 x = 20;
com3 1:fb4277ce4d93 138 }
com3 1:fb4277ce4d93 139 back = 30;
com3 1:fb4277ce4d93 140 /* if(EER_EQ(ping[LEFT]+ping[RIGHT],pingdef[LEFT]+pingdef[RIGHT], 5)){
com3 1:fb4277ce4d93 141 back = 10;
com3 1:fb4277ce4d93 142 }*/
com3 1:fb4277ce4d93 143 if(ping[BACK] > back){
com3 1:fb4277ce4d93 144 y = -20;
com3 1:fb4277ce4d93 145 }
com3 1:fb4277ce4d93 146 }
com3 1:fb4277ce4d93 147
com3 1:fb4277ce4d93 148 void line_stop0(){
com3 1:fb4277ce4d93 149 line_stop[0] = 0;
com3 1:fb4277ce4d93 150 }
com3 1:fb4277ce4d93 151 void line_stop1(){
com3 1:fb4277ce4d93 152 line_stop[1] = 0;
com3 1:fb4277ce4d93 153 }
com3 1:fb4277ce4d93 154 void line_stop2(){
com3 1:fb4277ce4d93 155 line_stop[2] = 0;
com3 1:fb4277ce4d93 156 }
com3 1:fb4277ce4d93 157 void line_stop3(){
com3 1:fb4277ce4d93 158 line_stop[3] = 0;
com3 2:59edff92b599 159 }
com3 2:59edff92b599 160
com3 2:59edff92b599 161 void line_check()
com3 2:59edff92b599 162 {
com3 2:59edff92b599 163 if(!line_stop[0]){
com3 2:59edff92b599 164 if(adcline[0].read_u16() > LINE_ON){
com3 2:59edff92b599 165 line[0] = 1;
com3 2:59edff92b599 166 line_stop[0] = 1;
com3 2:59edff92b599 167 myled[0] = 1;
com3 2:59edff92b599 168 liner0.attach(&line_stop0,LINE_TIME);
com3 2:59edff92b599 169 } else {
com3 2:59edff92b599 170 line[0] = 0;
com3 2:59edff92b599 171 myled[0] = 0;
com3 2:59edff92b599 172 }
com3 2:59edff92b599 173 }
com3 2:59edff92b599 174 if(!line_stop[1]){
com3 2:59edff92b599 175 if(adcline[1].read_u16() > LINE_ON){
com3 2:59edff92b599 176 line[1] = 1;
com3 2:59edff92b599 177 line_stop[1] = 1;
com3 2:59edff92b599 178 myled[1] = 1;
com3 2:59edff92b599 179 liner1.attach(&line_stop1,LINE_TIME);
com3 2:59edff92b599 180 } else {
com3 2:59edff92b599 181 line[1] = 0;
com3 2:59edff92b599 182 myled[1] = 0;
com3 2:59edff92b599 183 }
com3 2:59edff92b599 184 }
com3 2:59edff92b599 185 if(!line_stop[2]){
com3 2:59edff92b599 186 if(adcline[2].read_u16() > LINE_ON){
com3 2:59edff92b599 187 line[2] = 1;
com3 2:59edff92b599 188 line_stop[2] = 1;
com3 2:59edff92b599 189 myled[2] = 1;
com3 2:59edff92b599 190 liner2.attach(&line_stop2,LINE_TIME);
com3 2:59edff92b599 191 } else {
com3 2:59edff92b599 192 line[2] = 0;
com3 2:59edff92b599 193 myled[2] = 0;
com3 2:59edff92b599 194 }
com3 2:59edff92b599 195 }
com3 2:59edff92b599 196 if(!line_stop[3]){
com3 2:59edff92b599 197 if(adcline[3].read_u16() > LINE_ON){
com3 2:59edff92b599 198 line[3] = 1;
com3 2:59edff92b599 199 line_stop[3] = 1;
com3 2:59edff92b599 200 myled[3] = 1;
com3 2:59edff92b599 201 liner3.attach(&line_stop3,LINE_TIME);
com3 2:59edff92b599 202 } else {
com3 2:59edff92b599 203 line[3] = 0;
com3 2:59edff92b599 204 myled[3] = 0;
com3 2:59edff92b599 205 }
com3 2:59edff92b599 206 }
com3 2:59edff92b599 207 }
com3 2:59edff92b599 208
com3 2:59edff92b599 209
com3 2:59edff92b599 210 void pidupdate()
com3 2:59edff92b599 211 {
com3 2:59edff92b599 212 pidinput = compass;
com3 2:59edff92b599 213 pid.setProcessValue(pidinput);
com3 2:59edff92b599 214
com3 2:59edff92b599 215 compasspid = -pid.compute();
com3 2:59edff92b599 216 }
com3 1:fb4277ce4d93 217
com3 0:e6d14fec4954 218 int main() {
com3 0:e6d14fec4954 219
com3 0:e6d14fec4954 220 //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
com3 1:fb4277ce4d93 221 uint8_t num = 0;
com3 0:e6d14fec4954 222
com3 0:e6d14fec4954 223 wait(1);
com3 0:e6d14fec4954 224
com3 0:e6d14fec4954 225 motor.baud(115200); //ボーレート設定
com3 0:e6d14fec4954 226 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
com3 0:e6d14fec4954 227 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
com3 0:e6d14fec4954 228 sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
com3 0:e6d14fec4954 229 //compassdef = (compass / 10); //コンパス初期値保存
com3 0:e6d14fec4954 230 //compassdef = (dcompass.sample() / 10);
com3 2:59edff92b599 231 pid.setInputLimits(0.0,360.0);
com3 2:59edff92b599 232 pid.setOutputLimits(-30.0,30.0);
com3 2:59edff92b599 233 pid.setBias(0.0);
com3 2:59edff92b599 234 pid.setMode(AUTO_MODE);
com3 2:59edff92b599 235 pid.setSetPoint(180.0);
com3 2:59edff92b599 236 pidupdata.attach(&pidupdate, 0.05);
com3 0:e6d14fec4954 237
com3 1:fb4277ce4d93 238 sw.mode(PullUp);
com3 1:fb4277ce4d93 239 start.mode(PullUp);
com3 0:e6d14fec4954 240
com3 1:fb4277ce4d93 241 myled[0] = 1;
com3 1:fb4277ce4d93 242 while(start){}
com3 1:fb4277ce4d93 243 myled[0] = 0;
com3 2:59edff92b599 244
com3 2:59edff92b599 245 /*
com3 2:59edff92b599 246 while(1){
com3 2:59edff92b599 247 s = compasspid;
com3 2:59edff92b599 248 pc.printf("%d\n", s);
com3 2:59edff92b599 249 wait(0.1);
com3 2:59edff92b599 250 }
com3 2:59edff92b599 251 */
com3 2:59edff92b599 252
com3 2:59edff92b599 253 while(1){
com3 2:59edff92b599 254 line_check();
com3 2:59edff92b599 255
com3 2:59edff92b599 256 //x = MOTOR_P;
com3 2:59edff92b599 257 //y = 0;
com3 2:59edff92b599 258 if(ir_num > 7){
com3 2:59edff92b599 259 ir_num = 0;
com3 2:59edff92b599 260 }
com3 2:59edff92b599 261 x = MOTOR_P * way[ir_num][0];
com3 2:59edff92b599 262 y = MOTOR_P * way[ir_num][1];
com3 2:59edff92b599 263 s = compasspid;
com3 2:59edff92b599 264
com3 2:59edff92b599 265 if(value_ir > NO_IR){
com3 2:59edff92b599 266 home();
com3 2:59edff92b599 267 }
com3 2:59edff92b599 268
com3 2:59edff92b599 269 line_state();
com3 2:59edff92b599 270
com3 2:59edff92b599 271 move(x,y,s);
com3 2:59edff92b599 272 }
com3 1:fb4277ce4d93 273 while(1){
com3 2:59edff92b599 274 line_check();
com3 2:59edff92b599 275 //pc.printf("%d\n", adcline[1].read_u16());
com3 2:59edff92b599 276 //pc.printf("%d %d %d %d\n", line[0], line[1], line[2], line[3]);
com3 2:59edff92b599 277 if(ir_num > 7){
com3 2:59edff92b599 278 ir_num = 0;
com3 2:59edff92b599 279 }
com3 2:59edff92b599 280 x = MOTOR_P;//MOTOR_P * way[ir_num][0];
com3 2:59edff92b599 281 y = 0;//MOTOR_P * way[ir_num][1];
com3 2:59edff92b599 282 s = (compass - 180) / 3;
com3 2:59edff92b599 283 if(s > S1){
com3 2:59edff92b599 284 s = S1;
com3 2:59edff92b599 285 } else if(s < -S1){
com3 2:59edff92b599 286 s = -S1;
com3 2:59edff92b599 287 } else if(s > S2){
com3 2:59edff92b599 288 s = S2;
com3 2:59edff92b599 289 } else if(s < -S2){
com3 2:59edff92b599 290 s = -S2;
com3 2:59edff92b599 291 } else if(s > 0){
com3 2:59edff92b599 292 s = S3;
com3 2:59edff92b599 293 } else if(s < 0){
com3 2:59edff92b599 294 s = -S3;
com3 2:59edff92b599 295 }
com3 2:59edff92b599 296
com3 2:59edff92b599 297 line_state();
com3 2:59edff92b599 298
com3 2:59edff92b599 299 move(x,y,s);
com3 2:59edff92b599 300 }
com3 2:59edff92b599 301
com3 2:59edff92b599 302 /*
com3 2:59edff92b599 303 while(1){
com3 2:59edff92b599 304 x = MOTOR_P * way[num][0];
com3 2:59edff92b599 305 y = MOTOR_P * way[num][1];
com3 1:fb4277ce4d93 306 s = (compass - 180) / 3;
com3 1:fb4277ce4d93 307 if(s > S1){
com3 1:fb4277ce4d93 308 s = S1;
com3 1:fb4277ce4d93 309 } else if(s < -S1){
com3 1:fb4277ce4d93 310 s = -S1;
com3 1:fb4277ce4d93 311 } else if(s > S2){
com3 1:fb4277ce4d93 312 s = S2;
com3 1:fb4277ce4d93 313 } else if(s < -S2){
com3 1:fb4277ce4d93 314 s = -S2;
com3 1:fb4277ce4d93 315 } else if(s > S3){
com3 1:fb4277ce4d93 316 s = S3;
com3 1:fb4277ce4d93 317 } else if(s < -S3){
com3 1:fb4277ce4d93 318 s = -S3;
com3 1:fb4277ce4d93 319 }
com3 2:59edff92b599 320 if(!sw){
com3 2:59edff92b599 321 num++;
com3 2:59edff92b599 322 wait(0.2);
com3 2:59edff92b599 323 if(num > 7){
com3 2:59edff92b599 324 num = 0;
com3 2:59edff92b599 325 }
com3 2:59edff92b599 326 lcds(num);
com3 2:59edff92b599 327 }
com3 2:59edff92b599 328 //pc.printf("%d\n", s);
com3 2:59edff92b599 329
com3 1:fb4277ce4d93 330 move(x,y,s);
com3 2:59edff92b599 331 //wait(0.1);
com3 2:59edff92b599 332 }
com3 2:59edff92b599 333 */
com3 2:59edff92b599 334 /*
com3 2:59edff92b599 335 while(1){
com3 2:59edff92b599 336 x = 30;
com3 2:59edff92b599 337 y = 0;
com3 2:59edff92b599 338 move(x,y,s);
com3 2:59edff92b599 339 wait(0.5);
com3 2:59edff92b599 340 x = -30;
com3 2:59edff92b599 341 y = 0;
com3 2:59edff92b599 342 move(x,y,s);
com3 2:59edff92b599 343 wait(0.5);
com3 1:fb4277ce4d93 344 //pc.printf("%d\n", s);
com3 1:fb4277ce4d93 345 //pc.printf("%d\n", adcline[3].read_u16());
com3 1:fb4277ce4d93 346 }
com3 1:fb4277ce4d93 347 */
com3 2:59edff92b599 348
com3 1:fb4277ce4d93 349 /* while(1){
com3 1:fb4277ce4d93 350 if(ir_num > 7){
com3 1:fb4277ce4d93 351 ir_num = 0;
com3 1:fb4277ce4d93 352 }
com3 1:fb4277ce4d93 353 x = MOTOR_P * way[ir_num][0];
com3 1:fb4277ce4d93 354 y = MOTOR_P * way[ir_num][1];
com3 1:fb4277ce4d93 355 s = (compass - 180) / 3;
com3 1:fb4277ce4d93 356 if(s > S_MAX){
com3 1:fb4277ce4d93 357 s = S_MAX;
com3 1:fb4277ce4d93 358 } else if(s < -S_MAX){
com3 1:fb4277ce4d93 359 s = -S_MAX;
com3 1:fb4277ce4d93 360 }
com3 1:fb4277ce4d93 361
com3 1:fb4277ce4d93 362 move(x,y,s);
com3 1:fb4277ce4d93 363 }
com3 2:59edff92b599 364 */
com3 0:e6d14fec4954 365 /*
com3 0:e6d14fec4954 366 while(1){
com3 0:e6d14fec4954 367 i = 3;
com3 0:e6d14fec4954 368 analogHex[i] = adcIn[i].read_u16();
com3 0:e6d14fec4954 369 if(analogHex[i] > 10000){
com3 0:e6d14fec4954 370 myled[i] = 1;
com3 0:e6d14fec4954 371 line[i] = 1;
com3 0:e6d14fec4954 372 line_on = 1;
com3 0:e6d14fec4954 373 } else {
com3 0:e6d14fec4954 374 myled[i] = 0;
com3 0:e6d14fec4954 375 line[i] = 0;
com3 0:e6d14fec4954 376 line_on = 0;
com3 0:e6d14fec4954 377 }
com3 0:e6d14fec4954 378
com3 0:e6d14fec4954 379 x = 30;
com3 0:e6d14fec4954 380 y = 0;
com3 0:e6d14fec4954 381
com3 0:e6d14fec4954 382 if(line[1]){
com3 0:e6d14fec4954 383 x = LINE_P;
com3 0:e6d14fec4954 384 } else if(line[3]){
com3 0:e6d14fec4954 385 x = -LINE_P;
com3 0:e6d14fec4954 386 }
com3 0:e6d14fec4954 387
com3 0:e6d14fec4954 388 if(line[0]){
com3 0:e6d14fec4954 389 y = -LINE_P;
com3 0:e6d14fec4954 390 } else if(line[2]){
com3 0:e6d14fec4954 391 y = LINE_P;
com3 0:e6d14fec4954 392 }
com3 0:e6d14fec4954 393 move(x,y,s);
com3 0:e6d14fec4954 394
com3 0:e6d14fec4954 395 if(line_on)wait(0.3);
com3 0:e6d14fec4954 396 }
com3 0:e6d14fec4954 397 */
com3 0:e6d14fec4954 398 /*
com3 0:e6d14fec4954 399 while(1){
com3 0:e6d14fec4954 400 for(i = 0; i < 4; i++){
com3 0:e6d14fec4954 401 analogHex[i] = adcIn[i].read_u16();
com3 0:e6d14fec4954 402 if(analogHex[i] > 10000){
com3 0:e6d14fec4954 403 myled[i] = 1;
com3 0:e6d14fec4954 404 line[i] = 1;
com3 0:e6d14fec4954 405 } else {
com3 0:e6d14fec4954 406 myled[i] = 0;
com3 0:e6d14fec4954 407 line[i] = 0;
com3 0:e6d14fec4954 408 }
com3 0:e6d14fec4954 409 //wait(0.1);
com3 0:e6d14fec4954 410 }
com3 0:e6d14fec4954 411
com3 0:e6d14fec4954 412 if(line[0]){
com3 0:e6d14fec4954 413 x = -30;
com3 0:e6d14fec4954 414 } else if(line[2]){
com3 0:e6d14fec4954 415 x = 30;
com3 0:e6d14fec4954 416 }
com3 0:e6d14fec4954 417
com3 0:e6d14fec4954 418 if(line[1]){
com3 0:e6d14fec4954 419 y = 30;
com3 0:e6d14fec4954 420 } else if(line[3]){
com3 0:e6d14fec4954 421 y = -30;
com3 0:e6d14fec4954 422 }
com3 0:e6d14fec4954 423
com3 0:e6d14fec4954 424 move(x,y);
com3 0:e6d14fec4954 425 x = 0;
com3 0:e6d14fec4954 426 y = 0;
com3 0:e6d14fec4954 427 //pc.printf("%05d %05d %05d %05d\n", analogHex[0], analogHex[1], analogHex[2], analogHex[3]);
com3 0:e6d14fec4954 428 }
com3 0:e6d14fec4954 429 */
com3 0:e6d14fec4954 430 }