University of Washington

Over the 2003 summer, I had the pleasure of completing a two-week course on robotics at the UW. During those two weeks, I developed the following code:

// This program by Andrew Ferguson
// Copyright 2003 Andrew Ferguson
// The list below tells where the stuff should be plugged in
//Begin
int lt_bump = 7; //left bump sensor
int rt_bump = 8; //right bump sensor

int left_motor = 0; //left motor
int mill_motor = 1; //mill motor
int right_motor = 3; //right motor

int rear_bump = 9; //rear bump sensor

int alpha_pol = 0; //1st polarized light sensor
int beta_pol = 1; //2nd polarized light sensor, rotated 90 degrees

int lt_ir = 4; // first IR light sensor/emiter
int rt_ir = 3;// second IR light sensor/emiter
int break_beam_a = 15; //break beam sensor for detecting the balls
int break_beam_b = 14; //break beam sensor for detecting the balls

int rotation_left = 6; //left rotation sensor
int rotation_right = 5
; //right rotation sensor
//End

//The list below declares global variables, the varible does NOT need to be global, then do NOT put it here
//Begin
int foo_right = 0; // how many times bot has turned right
int foo_left = 0; // how many times bot has turned left

int last_turn = 0; // Which way the bot turned last; 0 = Left and 1 = Right

int lt_speed = 25; //how fast the left side is going
int rt_speed = 25; //how fast the right side is going

int ball_in_hopper = 1; //whether the a ball has been collected - 0 for no, 1 for yes

int pol_home = 1; //Which polorized light the sensor should look for/care about; possible values are 1 and -1

int sensor_value; //polarized sensor delta value

float system_time = seconds(); //time since epoch of system restart

float master_time = 0.; //This is the master start time, which is used to determine when it should start looking for home

int rt_cnt; //How many times the right rotation sensor has been hit
int lt_cnt; //How many times the left rotation sensor has been hit

int line_found; //This determines if the bot has detected the line for a given set of balls trying to be dropped...please no jokes

int in_motion = 1; //If the bot is in motion. This is normal set to 1 (i.e. Yes) until the game is over (180 seconds after boot), in which case the value is changed to 0 (no)
//End

void count_both (void) {
    int lt_previous_hole; //if the previous check on the left side was hole or not
    int rt_previous_hole; //if the previous check on the right side was a hole or not
    while(1) {
        if(lt_previous_hole == 0){ //the previous check was not a hole...
            if (analog(rotation_left) < 100){//... if the there is a hole now, then count that
                lt_cnt++; //counting the hole
                lt_previous_hole = 1; //setting the check to say there was a hole this time around
            }
        }
        else{
            if (analog(rotation_left) >= 100){ //if the previous check was a hole, then check to see if it's not a hole
                lt_cnt++;// counting the hole, or lack thereof
                lt_previous_hole = 0; //setting the check to say there was not a hole this time
            }
        }
        if(rt_previous_hole == 0){//the previous check was not a hole...
            if (analog(rotation_right) < 100){//... if the there is a hole now, then count that
                rt_cnt++;//counting the hole
                rt_previous_hole = 1;//setting the check to say there was a hole this time around
            }
        }
        else{
            if (analog(rotation_right) >= 100){//if the previous check was a hole, then check to see if it's not a hole
                rt_cnt++; //counting the hole, or lack thereof
                rt_previous_hole = 0; //setting the check to say there was not a hole this time
            }
        }
        defer();
    }
}

void stuck_master (void){
    int i;
    while(in_motion){
        lt_cnt = 0; //reset the left count to zero
        rt_cnt = 0; //reset the right count to zero
        sleep(2.0); //wait 2 seconds...now we have fresh count
        if ( (lt_cnt < 20) || (rt_cnt < 20) ) //if either the left or the right count is less then 10, then we just might be stuck
          {
            beep();
            stop();
            while ( (lt_cnt < 50) && (rt_cnt < 50) ){ //reverse for 20 counts
                lt_speed = -100;
                rt_speed = -100;             
                defer();
            }
            rt_speed = 25;
            lt_speed = 25;
          }
        defer();
    }
}

void go_home (void) //when time is up, try and find home!
{
    while(1) {
        while (seconds() <= 120.) { //if there are at least 2 minutes (120 seconds) left, we can go about our business
            defer();
            //beep();
        }
        while (seconds() <= 150. && (ball_in_hopper == 1) ) { //when there is a minute left, keep looking for balls
            defer();
            //beep();
        }       
        while (seconds() <= 150. && (ball_in_hopper == 0) ) { //when there is a minute left, start looking for the hole to drop of whatever balls we have...assuming we have balls
            ir_sense();
            //in_motion = 0;
            defer();
            //beep();
        }
        while (seconds() <180.) {  // when there are a 30 seconds left, start looking for the polarized light...note: there is no defer() or sleep(x.x) becuase this is a priority 1 process
            printf("Phoning Home\n");
            //in_motion = 0;
            beep();
            pol_sensor();
            defer();
        }
        while (seconds() > 180.) {  //when the game is over, shut down all motors and beep...how nice
            //printf("Shutting down\n");
            //beep();
            in_motion = 0;
            beeper_on();
            ao();
            defer();
        }
        defer();
    }
}

/*void ball (void){ //This checks to see if a ball is in the hopper or not
    while(1) {
        if (_raw_analog(break_beam) < 10)
          {
            ball_in_hopper = 0; // ball is NOT in hopper
        }
        else if (_raw_analog(break_beam) > 10) //ball IS in hopper
            {
              ball_in_hopper = 1;
              sleep(5.0);
          }
          sleep(.01);
    }
}
*/
void pol_sensor (void) // detects the polarization of the light source
{
    sensor_value = (_raw_analog(beta_pol) - _raw_analog(alpha_pol) ); // sets the delta value for the polarization
    lt_speed = 100; //full speed, left side
    rt_speed = 100; //full speed, right side
    if ( (sensor_value < -60) && (pol_home == 1 ) ){ //if the sensor value is less than 60 and the light is NOT home, turn around
        rt_speed = 100;
        lt_speed = -100;//lt_speed - 15;
    }
    if ( (sensor_value > -30) && (pol_home == -1) ){//if the sensor value is greater than -30 and the light is NOT home, turn around
        lt_speed = 100;
        rt_speed = -100;//rt_speed - 15;
    }
    sleep(.2);
    printf("%d\n", sensor_value);
}

void ir_sense(void) { //this tries to find the black line, which hopefully will lead to the hole. If not, the robot will turn around and go the other way, which HAS to lead to the black hole
    beep();
    printf("%d\n", line_found);
    while ( (analog(lt_ir) > 100) && (analog(rt_ir) > 100) ){ //if there are black lines under both the IR sensors :: this would happen if the robot approaced the line at a 90 degree angle
        lt_speed = 20;
        rt_speed = 20;
        //sleep(2.0);
        system_time = seconds();
        defer();
    }
    while( (analog(lt_ir) < 100) && (analog(rt_ir) < 100) && (line_found == 0) ){ //If there are no black lines and the line has NOT been found before
        lt_speed = 15;
        rt_speed = 25;
        defer();
    }
    while ( (analog(rt_ir) > 100) && (line_found == 0) ){ //if there is a black line under the right IR sensor and the line has not been found yet...
        rt_speed = 20;
        lt_speed = 20;//lt_speed - 15;
        system_time = seconds();
        line_found = 1;
        while(analog(lt_ir) < 100){ //...wait until the left IR sensor passes the line...
            defer();
        }
        while(analog(lt_ir) > 100){ //...then when it does hit the line, turn the other way
            rt_speed = -20;
            lt_speed = 20;
            sleep(1.5);
        }
        defer();
    }
    while ( (analog(lt_ir) > 100) && (line_found == 0) ){ //if there is a black line under the left IR sensor and the line has not been found yet...
        lt_speed = 20;
        rt_speed = 20;//rt_speed - 15;
        system_time = seconds();
        line_found = 1;
        while(analog(rt_ir) < 100){//...wait until the right IR sensor passes the line...
            defer();
        }
        while(analog(rt_ir) > 100){//...then when it does hit the line, turn the other way
            lt_speed = -20;
            rt_speed = 20;
            sleep(1.5);
        }
        defer();
    }
   
    while ( (analog(rt_ir) > 100) && (line_found == 1) && (analog(lt_ir) < 100) ){ //if the right IR sensor sees the black line, but the left one doesn't and the line has been found before, turn a bit the other way to get back on track
        rt_speed = 30;
        lt_speed = -10;//lt_speed - 15;
        system_time = seconds();
        defer();
    }
    while ( (analog(lt_ir) > 100) && (line_found == 1) && (analog(rt_ir) < 100) ){//if the left IR sensor sees the black line, but the right one doesn't and the line has been found before, turn a bit the other way to get back on track
        lt_speed = 30;
        rt_speed = -10;//rt_speed - 15;
        system_time = seconds();
        defer();
    }
    while( (analog(lt_ir) < 100) && (analog(rt_ir) < 100) && (line_found == 1) ) { // if both IR sensors don't see the black line :: this would happen if: A) The robot was on track or B) if the robot reached the end of the black line
        if( (seconds() - system_time) > 1.0) { // If more than 1 second has passed since the last course correction, start going in circles to try and reaquire the line
            lt_speed = -20;
            rt_speed = 20;
            while(analog(rt_ir)< 100) //while the right IR sensor doesn't seen the black line...
              {
                if( (seconds() - system_time) > 6.0) {//...and six seconds have passed, stop looking for the line...it's probably gone...
                    system_time = seconds();
                    break;
                }
                if (analog(lt_ir)> 100){//...or if the left line is seen, turn the other way :: the bot is really on track...just going straight
                    lt_speed = 20;
                    rt_speed = -20;
                    system_time = seconds();
                    while(analog(lt_ir)> 100){ //...wait while the left IR sensor is still on the black line...
                        defer();
                    }
                    while(analog(lt_ir)< 100) { //...when the left IR sensor gets off the back line...
                        if( (seconds() - system_time) > 4.) { //...wait until four seconds have passed and then break out
                            break;
                        }
                        defer();
                    }
                    break;
                }
                //system_time = seconds();
                break;
                defer();
            }
        }
        defer();
    }
}

void turn_left(void){ //turn the robot left
    //printf("Turning left:%d\n", foo_left);
    fd(left_motor);
    bk(right_motor);
    sleep(0.3);
    ao ();
    fd(mill_motor);
}

void turn_right(void){ //turn the robot right
    //printf("Turning right:%d\n", foo_right);
    bk(left_motor);
    fd(right_motor);
    sleep(0.3);
    ao ();
    fd(mill_motor);
}

void go_forward(void){ //make the robot go forward
    //printf("Going forward\n");
    motor(left_motor, lt_speed);
    motor(right_motor, rt_speed);
}

void go_backward(void){ //make the robot go backwards
    //printf("Going backwards\n");
    bk(left_motor);
    bk(right_motor);
    sleep (.5);
    ao ();
    fd(mill_motor);
}

void stop (void){ //make the robot stop
    //printf("Stopped\n");
    ao ();
    fd(mill_motor);
}

void main_movement (void) //main set of robot commands, pretty much just make sure it doesn't hit and thing and what to do if it does
{
    while (in_motion)
      {
        go_forward();
        //stuck_master(); //the ultimate in stuckness prevention!!
        /*while ( (digital(lt_bump) == 0) & (digital(rt_bump) == 0) )
          {
            //This section is commented out becuase it will be really hard for the robot get stuck on anything, esp. with the new physical collision avoidance system
            if( (seconds() - system_time) > 10.0)
              {
                stop();
                printf("10 second are up.  I'm going backwards");
                go_backward();
                printf("Turning %d\n", last_turn);
                if (last_turn == 1)
                  {
                    turn_right();
                    last_turn = 0;
                    system_time = seconds();
                }
                else{
                    turn_left();
                    last_turn = 1;
                    system_time = seconds();
                }
                go_forward();
                system_time = seconds();
            }
            defer();
        }
*/
        if ( (digital(lt_bump) == 1) ) //left turn
          {
            foo_left ++;
            stop();
            go_backward();
            if (foo_left%3 == 0)
              {
                turn_right();
                foo_right = 0;
                foo_left = 0;
            }
            else{
                turn_left();
            }
            last_turn = 0;
            system_time = seconds();
        }
        if ( (digital(rt_bump) == 1) ) //right turn
          {
            foo_right ++;
            stop();
            go_backward();
            if (foo_right%3 == 0)
              {
                turn_left();
                foo_right = 0;
                foo_left = 0;
            }
            else {
                turn_right();
                last_turn = 1;
                system_time = seconds();
            }
        }
        defer();
    }
}

void main() //Begin the main program
{
    printf("JCN v20.0 by Andrew and Mariah\n");
    //sleep(1.0);
    beep ();
    fd(mill_motor); //start the mill motor thingy
    start_process(main_movement() ); //start the main set of movements, including collision avoidance
    start_process(go_home() ); //start the master clock, and prepare to go home
    start_process(count_both() );
    start_process(stuck_master() );
   
    while (1)
      {
       
        line_found = 0;
        while ( (in_motion) && ( (digital(break_beam_a) == 0) || (digital(break_beam_b) == 0) )){
            //printf("%d - %d\n", digital(14), digital(15) );
            ir_sense(); //starts the black-line finding sub-routine
            sleep(.01);
        }
        printf("%d - %d\n", digital(14), digital(15) );
        //printf("Ball NOT in hopper\n");
        sleep(.01);
    }
}

I really like this part:
//stuck_master(); //the ultimate in stuckness prevention!!

Note: This was code that I originally had in a ‘Code’ page, but I’m moving it to a post now. This code is probably really old (years and years old). I enjoy laughing at myself and so should you.