/*Exaple for Ax-11 goto table target*/ #define pow 40 #define ref 70 #define A 1 #define B 2 #define LEFT 1 #define RIGHT 2 #define PASS 10 #define TRACK_OK ((leftref)&&(rightref)&&(mid>ref)&&(right>ref)) #define OVER_RIGHT (((leftref))||((leftref)&&(right>ref))) #define OVER_LEFT (((left>ref)&&(midref)&&(mid>ref)&&(right>ref))) #define CROSS_LEFT ((left>ref)&&(mid>ref)&&(rightref)&&(right>ref)) int left=0,right=0,mid=0; // For keep input analog value int target,base; void main(void) { ao(); select_base(); target = select_target(); goto_start(); goto_target(target); run_fd(1.0); ao(); while(1); } void read_sensor() { left = analog(29); // Read status left sensor mid = analog(25); // Read status middle sensor right = analog(21); // Read status right sensor } void spin_find_line() { int spin=0; while(spin!=1) { read_sensor(); if(OVER_RIGHT) { turn_right(0.1); } else if(OVER_LEFT) { turn_left(0.1); } else if(TRACK_OK) { spin = 1; } } } void cross_all(int dir_control) { int result=0,spin=0; while(result!=1) { read_sensor(); if(CROSS_ALL) { beep(); run_fd(0.01); if(base==B) { if(dir_control==LEFT) turn_left(0.8); else if(dir_control==RIGHT) turn_right(0.8); } else if(base==A) { if(dir_control==LEFT) turn_right(0.8); else if(dir_control==RIGHT) turn_left(0.8); } if(dir_control==PASS) run_fd(0.4); spin_find_line(); result = 1; } else if(OVER_RIGHT) turn_right(0.1); else if(OVER_LEFT) turn_left(0.1); else { run_fd(0.01); } } } void cross_left(int dir_control) { int result=0,spin=0; if(base==A) { while(result!=1) { read_sensor(); if(CROSS_RIGHT) { beep(); run_fd(0.01); if(dir_control==LEFT) turn_right(0.8); else if(dir_control==RIGHT) turn_left(0.8); if(dir_control==PASS) run_fd(0.4); spin_find_line(); result = 1; } else if(OVER_RIGHT) turn_right(0.1); else if(OVER_LEFT) turn_left(0.1); else { run_fd(0.01); } } } else if(base==B) { while(result!=1) { read_sensor(); if(CROSS_LEFT) { beep(); run_fd(0.01); if(dir_control==LEFT) turn_left(0.8); else if(dir_control==RIGHT) turn_right(0.8); if(dir_control==PASS) run_fd(0.4); spin_find_line(); result = 1; } else if(OVER_RIGHT) turn_right(0.1); else if(OVER_LEFT) turn_left(0.1); else { run_fd(0.01); } } } } void cross_right(int dir_control) { int result=0,spin=0; if(base==A) { while(result!=1) { read_sensor(); if(CROSS_LEFT) { beep(); run_fd(0.01); if(dir_control==LEFT) turn_right(0.8); else if(dir_control==RIGHT) turn_left(0.8); if(dir_control==PASS) run_fd(0.4); spin_find_line(); result = 1; } else if(OVER_RIGHT) turn_right(0.1); else if(OVER_LEFT) turn_left(0.1); else { run_fd(0.01); } } } else if(base==B) { while(result!=1) { read_sensor(); if(CROSS_RIGHT) { beep(); run_fd(0.01); if(dir_control==LEFT) turn_left(0.8); else if(dir_control==RIGHT) turn_right(0.8); if(dir_control==PASS) run_fd(0.4); spin_find_line(); result = 1; } else if(OVER_RIGHT) turn_right(0.1); else if(OVER_LEFT) turn_left(0.1); else { run_fd(0.01); } } } } void global_part1() { cross_right(RIGHT); cross_left(LEFT); } void global_part2() { cross_right(PASS); global_part1(); } void global_part3() { cross_right(PASS); global_part2(); } void global_part4() { cross_all(PASS); cross_all(RIGHT); global_part1(); } void global_part5() { cross_all(PASS); cross_all(RIGHT); global_part2(); } void global_part6() { cross_right(PASS); cross_right(RIGHT); global_part3(); } void goto_target(int num) { if(num==1) { /*RETUTN*/ if(base==B) turn_right(2.0); else if(base==A) turn_left(2.0); cross_all(RIGHT); cross_all(RIGHT); global_part1(); } else if(num==2) { /*GO TO TARGET*/ cross_right(RIGHT); cross_all(PASS); /*RETUTN*/ if(base==B) turn_right(2.0); else if(base==A) turn_left(2.0); cross_all(RIGHT); global_part2(); } else if(num==3) { /*GO TO TARGET*/ cross_right(RIGHT); cross_all(PASS); cross_all(PASS); /*RETUTN*/ if(base==B) turn_right(2.0); else if(base==A) turn_left(2.0); cross_all(RIGHT); global_part3(); } else if(num==4) { /*GO TO TARGET*/ cross_right(PASS); /*RETUTN*/ if(base==B) turn_right(2.0); else if(base==A) turn_left(2.0); cross_all(RIGHT); global_part4(); } else if(num==5) { /*GO TO TARGET*/ cross_right(RIGHT); cross_all(PASS); /*RETUTN*/ if(base==B) turn_left(2.0); else if(base==A) turn_right(2.0); cross_all(RIGHT); cross_all(RIGHT); global_part5(); } else if(num==6) { /*GO TO TARGET*/ cross_right(RIGHT); cross_all(PASS); cross_all(PASS); /*RETUTN*/ if(base==B) turn_left(2.0); else if(base==A) turn_right(2.0); cross_all(RIGHT); cross_all(RIGHT); global_part6(); } else if(num==7) { /*GO TO TARGET*/ cross_right(PASS); cross_right(RIGHT); /*RETUTN*/ if(base==B) turn_left(2.0); else if(base==A) turn_right(2.0); cross_all(LEFT); cross_left(LEFT); cross_left(PASS); cross_left(PASS); cross_right(RIGHT); } else if(num==8) { /*GO TO TARGET*/ cross_right(PASS); cross_right(RIGHT); cross_all(PASS); /*RETUTN*/ if(base==B) turn_left(2.0); else if(base==A) turn_right(2.0); cross_all(RIGHT); cross_right(RIGHT); cross_all(PASS); global_part5(); } else if(num==9) { /*GO TO TARGET*/ cross_right(PASS); cross_right(RIGHT); cross_all(PASS); cross_all(PASS); /*RETUTN*/ if(base==B) turn_left(2.0); else if(base==A) turn_right(2.0); cross_all(RIGHT); cross_right(RIGHT); cross_right(PASS); global_part6(); } } void goto_start(void) { cross_all(RIGHT); cross_all(LEFT); } int select_target() { int val=0; while(!start_button()) { val = (knob()/30)+1; printf("Target: %d Push Start\n",val); sleep(0.1); } beep(); sleep(0.1); beep(); return(val); } void select_base() { int val=0; while(!start_button()) { val = knob()/100; if(val<=1) { printf("Start Base: A Push Start\n"); base = A; } else if(val==2) { printf("Start Base: B Push Start\n"); base = B; } sleep(0.1); } sleep(0.5); beep(); sleep(0.1); beep(); } void turn_right(float spin_time) { off(0); motor(1,pow); sleep(spin_time); } void turn_left(float spin_time) { motor(0,pow); off(1); sleep(spin_time); } void run_fd(float spin_time) { motor(0,pow); motor(1,pow); sleep(spin_time); } void run_bk(float spin_time) { motor(0,-pow); motor(1,-pow); sleep(spin_time); }