/********************************************************************** * Name: Group 7 * Robert Moe * John Zumwalt * Mark Woehrer * Celi Sun * Class: CS 4970.1 Intro to Intelligent Robots * Instructor: Dr. Hougen * Date: 03/31/03 * FileName: main_test.ic *********************************************************************** #define LEFT_LIGHT 2 #define RIGHT_LIGHT 3 #define RIGHT_MOTOR 0 #define LEFT_MOTOR 1 #define LEFT_TRACK 3 #define RIGHT_TRACK 2 #define FRONT_BUMP 7 #define LEFT_RANGE 16 #define RIGHT_RANGE 17 #define RANGE_THRESH 65 #define DETECT_RIGHT 2 #define DETECT_LEFT 3 #define DETECT_NONE 0 #define DEAD_ZONE 25 #define LEFT_SERVO_CENTER 2300 #define RIGHT_SERVO_CENTER 2800 #define SERVO_DELTA 200 #define SERVO_TIME 10 #define SEEK_TIMEOUT 1500 #define left_servo servo0 #define right_servo servo2 /********************************************************************* * Function: move_servos(void) * Inputs: none * Global Variables: * int move_servos_state state indicator of the servo state * int move_servos_count countdown of the servo swing delta * int kill_servo_swing stop the servos from swinging *********************************************************************/ int move_servos_state; int move_servos_count; int kill_servo_swing = 0; void move_servos (void){ if(move_servos_state==0) { // Initialize Servo move_servos_count=SERVO_TIME; move_servos_state=1; } else if(move_servos_state==1){ // Swing L&R Servo FWD if(move_servos_count >=0 ){ move_servos_count--; }else{ move_servos_count=SERVO_TIME; move_servos_state=2; left_servo=LEFT_SERVO_CENTER+SERVO_DELTA; right_servo=RIGHT_SERVO_CENTER-SERVO_DELTA; } } else if(move_servos_state==2){ // Swing L&R Servo BACK if(move_servos_count >=0 ){ move_servos_count--; }else{ move_servos_count=SERVO_TIME; move_servos_state=1; left_servo=LEFT_SERVO_CENTER-SERVO_DELTA; right_servo=RIGHT_SERVO_CENTER+SERVO_DELTA; } } else // Should never get here { beep(); } } /********************************************************************* * Function: seek(void) * Inputs: none * Global Variables: * int seekfun * int seek_state * int seek_timeout * float seek_u_old * float seek_e_old * float seek_timeout *********************************************************************/ float seek_u_old; float seek_e_old; float seek_K=0.0; int seekrun; int seek_state; int seek_timeout; int seek (void) { float seek_u; float seek_e; int left_out,right_out; int sum; seek_e_old=seek_e; seek_e=(float)analog(LEFT_LIGHT)-(float)analog(RIGHT_LIGHT); sum=analog(LEFT_LIGHT)+analog(RIGHT_LIGHT); { { //Graduallyl increase the seek_K Gain as the robot moves // closer to the light to better increase accuracy. if (seek_state == 0){ seek_K=0.5; seek_state=1; } else if (seek_state == 1){ if(sum <=16){ seek_K=8.0; seek_state=2; } } else if (seek_state == 2){ if(sum <=12){ seek_K=20.0; seek_state=3; } } else if (seek_state == 3){ seek_K=70.0; seek_state=4; seek_timeout=SEEK_TIMEOUT; } else if (seek_state == 4){ if(seek_timeout >= 0){ seek_timeout--; }else{ return 1; } } else if (seek_state == 5){ } return 0; } } // Seek function for motor output. seek_u_old=seek_u; seek_u = seek_u_old + seek_K * (seek_e - seek_e_old); left_out =(int)(100.0+seek_u); right_out=(int)(100.0-seek_u); motor(LEFT_MOTOR, left_out); motor(RIGHT_MOTOR,right_out); } /********************************************************************* * Function: find_light(void) * Inputs: none * Global Variables: * int find_light_state * int find_light_max_count * int find_light_max * int find_light_min * int find_light_status * int find_direction *********************************************************************/ int find_light_state; int find_light_max_count; int find_light_max,find_light_min; int find_light_status=0; int find_direction=0; int find_light (void){ int my_sum; if(find_light_state==0){ find_light_status=0; find_light_max_count=500; find_light_max=0; find_light_min=10000; if(find_direction){ kill_servo_swing = 1; motor(LEFT_MOTOR, -100); motor(RIGHT_MOTOR,+100); motor(LEFT_TRACK, -100); find_direction=1; }else{ kill_servo_swing = 1; motor(LEFT_MOTOR, +100); motor(RIGHT_MOTOR,-100); motor(RIGHT_TRACK, -100); find_direction=1; } find_light_state=1; }else if(find_light_state==1){ if(find_light_max_count>0){ find_light_max_count--; my_sum=analog(LEFT_LIGHT)+analog(RIGHT_LIGHT); if(my_sum <= 20) { //beep();beep();beep();beep();beep();beep(); motor(RIGHT_MOTOR, 0); motor(LEFT_MOTOR, 0); motor(RIGHT_TRACK, 100); motor(LEFT_TRACK, 100); kill_servo_swing = 0; return 1; } if(my_sum>find_light_max) find_light_max=my_sum; if(my_sum0){ find_light_max_count--; my_sum=analog(LEFT_LIGHT)+analog(RIGHT_LIGHT); if(my_sum<=find_light_min+1){ //!!! find_light_state=4; if(my_sum <= 20) { //!!!mkw change me motor(RIGHT_TRACK, 100); motor(LEFT_TRACK, 100); kill_servo_swing = 0; find_light_status=1; } else { motor(RIGHT_TRACK, 100); motor(LEFT_TRACK, 100); kill_servo_swing = 0; find_light_status=2; } motor(LEFT_MOTOR, 0); motor(RIGHT_MOTOR,0); } } else { find_light_state = 4; find_light_status = 2; } } else if(find_light_state==4){ motor(LEFT_MOTOR, +100); motor(RIGHT_MOTOR,-100); find_light_state=5; } else if(find_light_state==5){ motor(LEFT_MOTOR, 0); motor(RIGHT_MOTOR,0); return find_light_status; } //return status; } /********************************************************************* * Function: find_bump(void) * Inputs: none * Global Variables: * none *********************************************************************/ int find_bump() { return digital(7); } /********************************************************************* * Function: find_bucket(void) * Inputs: none * Global Variables: * none *********************************************************************/ int find_bucket() { int lrange = analog(LEFT_RANGE); int rrange = analog(RIGHT_RANGE); if(lrange >= RANGE_THRESH) { //printf("\nlrange=%d", lrange); return DETECT_LEFT; } if(rrange >= RANGE_THRESH) { //printf("\nrrange=%d", rrange); return DETECT_RIGHT; } //c0cac0la //printf("\n"); return DETECT_NONE; } /********************************************************************* * Function: backup_and_turn(void) * Inputs: none * Global Variables: * int avoid_state * int backstate * int direction * int turncount * int forwardcount *********************************************************************/ int avoid_state; int backcount; int direction; int turncount; int forwardcount; int backup_and_turn() { if(avoid_state==0){ backcount = 25; forwardcount = 80; direction = analog(RIGHT_LIGHT)- analog(LEFT_LIGHT); motor(RIGHT_MOTOR, -100); motor(LEFT_MOTOR, -100); avoid_state=1; } else if(avoid_state==1){ if(backcount == 0) { avoid_state = 2; turncount = 10; } else { backcount--; } } else if(avoid_state==2){ if(turncount == 0) { avoid_state = 3; } else { if(direction >= 0) { motor(LEFT_MOTOR, -100); motor(RIGHT_MOTOR, 100); } else { motor(LEFT_MOTOR, 100); motor(RIGHT_MOTOR, -100); } turncount--; } } else if(avoid_state==3) { if(forwardcount == 0) { return 1; } else { if(!find_bump()) { motor(RIGHT_MOTOR, 100); motor(LEFT_MOTOR, 100); } else { avoid_state = 0; } forwardcount--; } } return 0; } /********************************************************************* * Function: avoid_bucket(void) * Inputs: none * Global Variables: * int ab_state * int ab_direction * int ab_turncount * int ab_pausecount * int ab_drivecount *********************************************************************/ int ab_state; int ab_direction; int ab_turncount; int ab_pausecount; int ab_drivecount; int avoid_bucket() { if(ab_state==0){ ab_pausecount = 50; motor(RIGHT_MOTOR, 0); motor(LEFT_MOTOR, 0); ab_state=1; } else if(ab_state==1){ if(ab_pausecount == 0) { ab_state = 2; ab_turncount = 25; } else { ab_pausecount--; } } else if(ab_state==2){ if(ab_turncount == 0) { ab_drivecount = 60; ab_state = 3; } else { if(ab_direction == DETECT_LEFT) { motor(LEFT_MOTOR, 100); motor(RIGHT_MOTOR, -100); } else if(ab_direction == DETECT_RIGHT) { motor(RIGHT_MOTOR, 100); motor(LEFT_MOTOR, -100); } ab_turncount--; } } else if(ab_state==3) { if(ab_drivecount == 0) { return 1; } else { if(find_bucket() != DETECT_NONE) { ab_state = 0; ab_direction = find_bucket(); return 0; } if(!find_bump()) { motor(RIGHT_MOTOR, 100); motor(LEFT_MOTOR, 100); } else { motor(RIGHT_MOTOR, -100); motor(LEFT_MOTOR, -100); } ab_drivecount--; } } return 0; } /********************************************************************* * Function: found_bucket(int frange) * Inputs: frange * Global Variables: * none *********************************************************************/ void found_bucket(int frange) { ab_state=0; ab_direction = frange; } /********************************************************************* * Function: touch_light(void) * Inputs: none * Global Variables: * int touch_light_state * int touch_light_timeout *********************************************************************/ int touch_light_state; int touch_light_timeout; int touch_light() { if((seek_state == 4)&&(analog(LEFT_LIGHT)+analog(RIGHT_LIGHT))>=LIGHT_THRESHOLD) { return 1; } return 0; } /********************************************************************* * Function: main(void) * Inputs: none * Global Variables: * none *********************************************************************/ int LIGHT_THRESHOLD; void main() { long time_old; int main_state; int prev_state; int countdown; int found_range; int skip_bump; int print_count; int servoRcount; int servoLcount; while(!start_button()){ LIGHT_THRESHOLD = 2 * knob(); printf("%d\n", 2* knob()); } printf("Press START\n"); while(!start_button()); printf("GO\n"); beep(); motor(RIGHT_TRACK,100); motor(LEFT_TRACK,100); init_expbd_servos(1); //center left_servo=LEFT_SERVO_CENTER; right_servo=RIGHT_SERVO_CENTER; time_old=mseconds(); main_state=0; move_servos_state=0; while(!stop_button()){ if(!kill_servo_swing) { move_servos(); } else { left_servo = 1000; right_servo = 4000; } if(main_state == 0){ // INIT find_light_state=0; main_state=1; seek_state = 0; //main_state=3; skip_bump=0; kill_servo_swing = 1; } else if(main_state == 1){ // FIND LIGHT int tmp; found_range = find_bucket(); if(found_range != DETECT_NONE) { found_bucket(found_range); main_state = 6; prev_state = 0; } tmp=find_light(); if(tmp==1){ main_state=2; countdown=100; beep(); } else if(tmp == 2) { countdown = 300; main_state=5; motor(RIGHT_MOTOR, 100); motor(LEFT_MOTOR, 100); } } else if(main_state == 2){ // PAUSE if(countdown == 0){ main_state=3; seek_state=0; touch_light_state = 0; touch_light_timeout = 10; motor(RIGHT_MOTOR, 100); motor(LEFT_MOTOR, 100); }else{ countdown--; } } else if(main_state == 3){ // SEEK skip_bump = 0; found_range = find_bucket(); if(found_range != DETECT_NONE) { found_bucket(found_range); main_state = 6; prev_state = 3; } if(touch_light()) { main_state = 0; skip_bump = 1; //beep();beep();beep();beep(); //motor(RIGHT_MOTOR, -100); //motor(LEFT_MOTOR, -100); //msleep(50l); //motor(RIGHT_MOTOR, 0); //motor(LEFT_MOTOR, 0); seek_state = 0; find_light_state=0; } if(find_bump() && !skip_bump) { main_state=4; prev_state=3; avoid_state = 0; } skip_bump = 0; if(seek()) { main_state=0; } //seek(); } else if(main_state == 4) { found_range = find_bucket(); if(found_range != DETECT_NONE) { found_bucket(found_range); main_state = 6; prev_state = 3; } else if(backup_and_turn()) { main_state = prev_state; } } else if(main_state == 5) { kill_servo_swing = 0; if(countdown == 0) { main_state = 0; } else { found_range = find_bucket(); if(found_range != DETECT_NONE) { found_bucket(found_range); main_state = 6; prev_state = 5; } if(find_bump()) { prev_state = 5; main_state = 4; avoid_state = 0; beep(); } countdown--; } } else if(main_state == 6) { kill_servo_swing = 0; if(avoid_bucket()) { motor(RIGHT_MOTOR, 100); motor(LEFT_MOTOR, 100); found_range = DETECT_NONE; main_state = prev_state; } } else if(main_state == 7) { motor(RIGHT_MOTOR, 100); motor(LEFT_MOTOR, 100); main_state = 0; } //printf("\nState=%d", main_state); if(print_count<10){ print_count++; }else{ printf("\nState=%d", main_state); print_count=0; } while((mseconds()-time_old) < 10L){} time_old=mseconds(); } init_expbd_servos(0); motor(LEFT_MOTOR, 0); motor(RIGHT_MOTOR,0); motor(2,0); motor(3,0); }