RoboGolf

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:

<br />
// This program by Andrew Ferguson<br />
// Copyright 2003 Andrew Ferguson<br />
// The list below tells where the stuff should be plugged in<br />
//Begin<br />
int lt_bump = 7; //left bump sensor<br />
int rt_bump = 8; //right bump sensor</p>
<p>int left_motor = 0; //left motor<br />
int mill_motor = 1; //mill motor<br />
int right_motor = 3; //right motor</p>
<p>int rear_bump = 9; //rear bump sensor</p>
<p>int alpha_pol = 0; //1st polarized light sensor<br />
int beta_pol = 1; //2nd polarized light sensor, rotated 90 degrees</p>
<p>int lt_ir = 4; // first IR light sensor/emiter<br />
int rt_ir = 3;// second IR light sensor/emiter<br />
int break_beam_a = 15; //break beam sensor for detecting the balls<br />
int break_beam_b = 14; //break beam sensor for detecting the balls</p>
<p>int rotation_left = 6; //left rotation sensor<br />
int rotation_right = 5<br />
; //right rotation sensor<br />
//End</p>
<p>//The list below declares global variables, the varible does NOT need to be global, then do NOT put it here<br />
//Begin<br />
int foo_right = 0; // how many times bot has turned right<br />
int foo_left = 0; // how many times bot has turned left</p>
<p>int last_turn = 0; // Which way the bot turned last; 0 = Left and 1 = Right</p>
<p>int lt_speed = 25; //how fast the left side is going<br />
int rt_speed = 25; //how fast the right side is going</p>
<p>int ball_in_hopper = 1; //whether the a ball has been collected - 0 for no, 1 for yes</p>
<p>int pol_home = 1; //Which polorized light the sensor should look for/care about; possible values are 1 and -1</p>
<p>int sensor_value; //polarized sensor delta value</p>
<p>float system_time = seconds(); //time since epoch of system restart</p>
<p>float master_time = 0.; //This is the master start time, which is used to determine when it should start looking for home</p>
<p>int rt_cnt; //How many times the right rotation sensor has been hit<br />
int lt_cnt; //How many times the left rotation sensor has been hit</p>
<p>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</p>
<p>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)<br />
//End</p>
<p>void count_both (void) {<br />
    int lt_previous_hole; //if the previous check on the left side was hole or not<br />
    int rt_previous_hole; //if the previous check on the right side was a hole or not<br />
    while(1) {<br />
        if(lt_previous_hole == 0){ //the previous check was not a hole...<br />
            if (analog(rotation_left) &lt; 100){//... if the there is a hole now, then count that<br />
                lt_cnt++; //counting the hole<br />
                lt_previous_hole = 1; //setting the check to say there was a hole this time around<br />
            }<br />
        }<br />
        else{<br />
            if (analog(rotation_left) &gt;= 100){ //if the previous check was a hole, then check to see if it's not a hole<br />
                lt_cnt++;// counting the hole, or lack thereof<br />
                lt_previous_hole = 0; //setting the check to say there was not a hole this time<br />
            }<br />
        }<br />
        if(rt_previous_hole == 0){//the previous check was not a hole...<br />
            if (analog(rotation_right) &lt; 100){//... if the there is a hole now, then count that<br />
                rt_cnt++;//counting the hole<br />
                rt_previous_hole = 1;//setting the check to say there was a hole this time around<br />
            }<br />
        }<br />
        else{<br />
            if (analog(rotation_right) &gt;= 100){//if the previous check was a hole, then check to see if it's not a hole<br />
                rt_cnt++; //counting the hole, or lack thereof<br />
                rt_previous_hole = 0; //setting the check to say there was not a hole this time<br />
            }<br />
        }<br />
        defer();<br />
    }<br />
}</p>
<p>void stuck_master (void){<br />
    int i;<br />
    while(in_motion){<br />
        lt_cnt = 0; //reset the left count to zero<br />
        rt_cnt = 0; //reset the right count to zero<br />
        sleep(2.0); //wait 2 seconds...now we have fresh count<br />
        if ( (lt_cnt &lt; 20) || (rt_cnt &lt; 20) ) //if either the left or the right count is less then 10, then we just might be stuck<br />
          {<br />
            beep();<br />
            stop();<br />
            while ( (lt_cnt &lt; 50) &amp;&amp; (rt_cnt &lt; 50) ){ //reverse for 20 counts<br />
                lt_speed = -100;<br />
                rt_speed = -100;<br />
                defer();<br />
            }<br />
            rt_speed = 25;<br />
            lt_speed = 25;<br />
          }<br />
        defer();<br />
    }<br />
}</p>
<p>void go_home (void) //when time is up, try and find home!<br />
{<br />
    while(1) {<br />
        while (seconds() &lt;= 120.) { //if there are at least 2 minutes (120 seconds) left, we can go about our business<br />
            defer();<br />
            //beep();<br />
        }<br />
        while (seconds() &lt;= 150. &amp;&amp; (ball_in_hopper == 1) ) { //when there is a minute left, keep looking for balls<br />
            defer();<br />
            //beep();<br />
        }<br />
        while (seconds() &lt;= 150. &amp;&amp; (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<br />
            ir_sense();<br />
            //in_motion = 0;<br />
            defer();<br />
            //beep();<br />
        }<br />
        while (seconds() &lt;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<br />
            printf(&quot;Phoning Home\n&quot;);<br />
            //in_motion = 0;<br />
            beep();<br />
            pol_sensor();<br />
            defer();<br />
        }<br />
        while (seconds() &gt; 180.) {  //when the game is over, shut down all motors and beep...how nice<br />
            //printf(&quot;Shutting down\n&quot;);<br />
            //beep();<br />
            in_motion = 0;<br />
            beeper_on();<br />
            ao();<br />
            defer();<br />
        }<br />
        defer();<br />
    }<br />
}</p>
<p>/*void ball (void){ //This checks to see if a ball is in the hopper or not<br />
    while(1) {<br />
        if (_raw_analog(break_beam) &lt; 10)<br />
          {<br />
            ball_in_hopper = 0; // ball is NOT in hopper<br />
        }<br />
        else if (_raw_analog(break_beam) &gt; 10) //ball IS in hopper<br />
            {<br />
              ball_in_hopper = 1;<br />
              sleep(5.0);<br />
          }<br />
          sleep(.01);<br />
    }<br />
}<br />
*/<br />
void pol_sensor (void) // detects the polarization of the light source<br />
{<br />
    sensor_value = (_raw_analog(beta_pol) - _raw_analog(alpha_pol) ); // sets the delta value for the polarization<br />
    lt_speed = 100; //full speed, left side<br />
    rt_speed = 100; //full speed, right side<br />
    if ( (sensor_value &lt; -60) &amp;&amp; (pol_home == 1 ) ){ //if the sensor value is less than 60 and the light is NOT home, turn around<br />
        rt_speed = 100;<br />
        lt_speed = -100;//lt_speed - 15;<br />
    }<br />
    if ( (sensor_value &gt; -30) &amp;&amp; (pol_home == -1) ){//if the sensor value is greater than -30 and the light is NOT home, turn around<br />
        lt_speed = 100;<br />
        rt_speed = -100;//rt_speed - 15;<br />
    }<br />
    sleep(.2);<br />
    printf(&quot;%d\n&quot;, sensor_value);<br />
}</p>
<p>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<br />
    beep();<br />
    printf(&quot;%d\n&quot;, line_found);<br />
    while ( (analog(lt_ir) &gt; 100) &amp;&amp; (analog(rt_ir) &gt; 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<br />
        lt_speed = 20;<br />
        rt_speed = 20;<br />
        //sleep(2.0);<br />
        system_time = seconds();<br />
        defer();<br />
    }<br />
    while( (analog(lt_ir) &lt; 100) &amp;&amp; (analog(rt_ir) &lt; 100) &amp;&amp; (line_found == 0) ){ //If there are no black lines and the line has NOT been found before<br />
        lt_speed = 15;<br />
        rt_speed = 25;<br />
        defer();<br />
    }<br />
    while ( (analog(rt_ir) &gt; 100) &amp;&amp; (line_found == 0) ){ //if there is a black line under the right IR sensor and the line has not been found yet...<br />
        rt_speed = 20;<br />
        lt_speed = 20;//lt_speed - 15;<br />
        system_time = seconds();<br />
        line_found = 1;<br />
        while(analog(lt_ir) &lt; 100){ //...wait until the left IR sensor passes the line...<br />
            defer();<br />
        }<br />
        while(analog(lt_ir) &gt; 100){ //...then when it does hit the line, turn the other way<br />
            rt_speed = -20;<br />
            lt_speed = 20;<br />
            sleep(1.5);<br />
        }<br />
        defer();<br />
    }<br />
    while ( (analog(lt_ir) &gt; 100) &amp;&amp; (line_found == 0) ){ //if there is a black line under the left IR sensor and the line has not been found yet...<br />
        lt_speed = 20;<br />
        rt_speed = 20;//rt_speed - 15;<br />
        system_time = seconds();<br />
        line_found = 1;<br />
        while(analog(rt_ir) &lt; 100){//...wait until the right IR sensor passes the line...<br />
            defer();<br />
        }<br />
        while(analog(rt_ir) &gt; 100){//...then when it does hit the line, turn the other way<br />
            lt_speed = -20;<br />
            rt_speed = 20;<br />
            sleep(1.5);<br />
        }<br />
        defer();<br />
    }</p>
<p>    while ( (analog(rt_ir) &gt; 100) &amp;&amp; (line_found == 1) &amp;&amp; (analog(lt_ir) &lt; 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<br />
        rt_speed = 30;<br />
        lt_speed = -10;//lt_speed - 15;<br />
        system_time = seconds();<br />
        defer();<br />
    }<br />
    while ( (analog(lt_ir) &gt; 100) &amp;&amp; (line_found == 1) &amp;&amp; (analog(rt_ir) &lt; 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<br />
        lt_speed = 30;<br />
        rt_speed = -10;//rt_speed - 15;<br />
        system_time = seconds();<br />
        defer();<br />
    }<br />
    while( (analog(lt_ir) &lt; 100) &amp;&amp; (analog(rt_ir) &lt; 100) &amp;&amp; (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<br />
        if( (seconds() - system_time) &gt; 1.0) { // If more than 1 second has passed since the last course correction, start going in circles to try and reaquire the line<br />
            lt_speed = -20;<br />
            rt_speed = 20;<br />
            while(analog(rt_ir)&lt; 100) //while the right IR sensor doesn't seen the black line...<br />
              {<br />
                if( (seconds() - system_time) &gt; 6.0) {//...and six seconds have passed, stop looking for the line...it's probably gone...<br />
                    system_time = seconds();<br />
                    break;<br />
                }<br />
                if (analog(lt_ir)&gt; 100){//...or if the left line is seen, turn the other way :: the bot is really on track...just going straight<br />
                    lt_speed = 20;<br />
                    rt_speed = -20;<br />
                    system_time = seconds();<br />
                    while(analog(lt_ir)&gt; 100){ //...wait while the left IR sensor is still on the black line...<br />
                        defer();<br />
                    }<br />
                    while(analog(lt_ir)&lt; 100) { //...when the left IR sensor gets off the back line...<br />
                        if( (seconds() - system_time) &gt; 4.) { //...wait until four seconds have passed and then break out<br />
                            break;<br />
                        }<br />
                        defer();<br />
                    }<br />
                    break;<br />
                }<br />
                //system_time = seconds();<br />
                break;<br />
                defer();<br />
            }<br />
        }<br />
        defer();<br />
    }<br />
}</p>
<p>void turn_left(void){ //turn the robot left<br />
    //printf(&quot;Turning left:%d\n&quot;, foo_left);<br />
    fd(left_motor);<br />
    bk(right_motor);<br />
    sleep(0.3);<br />
    ao ();<br />
    fd(mill_motor);<br />
}</p>
<p>void turn_right(void){ //turn the robot right<br />
    //printf(&quot;Turning right:%d\n&quot;, foo_right);<br />
    bk(left_motor);<br />
    fd(right_motor);<br />
    sleep(0.3);<br />
    ao ();<br />
    fd(mill_motor);<br />
}</p>
<p>void go_forward(void){ //make the robot go forward<br />
    //printf(&quot;Going forward\n&quot;);<br />
    motor(left_motor, lt_speed);<br />
    motor(right_motor, rt_speed);<br />
}</p>
<p>void go_backward(void){ //make the robot go backwards<br />
    //printf(&quot;Going backwards\n&quot;);<br />
    bk(left_motor);<br />
    bk(right_motor);<br />
    sleep (.5);<br />
    ao ();<br />
    fd(mill_motor);<br />
}</p>
<p>void stop (void){ //make the robot stop<br />
    //printf(&quot;Stopped\n&quot;);<br />
    ao ();<br />
    fd(mill_motor);<br />
}</p>
<p>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<br />
{<br />
    while (in_motion)<br />
      {<br />
        go_forward();<br />
        //stuck_master(); //the ultimate in stuckness prevention!!<br />
        /*while ( (digital(lt_bump) == 0) &amp; (digital(rt_bump) == 0) )<br />
          {<br />
            //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<br />
            if( (seconds() - system_time) &gt; 10.0)<br />
              {<br />
                stop();<br />
                printf(&quot;10 second are up.  I'm going backwards&quot;);<br />
                go_backward();<br />
                printf(&quot;Turning %d\n&quot;, last_turn);<br />
                if (last_turn == 1)<br />
                  {<br />
                    turn_right();<br />
                    last_turn = 0;<br />
                    system_time = seconds();<br />
                }<br />
                else{<br />
                    turn_left();<br />
                    last_turn = 1;<br />
                    system_time = seconds();<br />
                }<br />
                go_forward();<br />
                system_time = seconds();<br />
            }<br />
            defer();<br />
        }<br />
*/<br />
        if ( (digital(lt_bump) == 1) ) //left turn<br />
          {<br />
            foo_left ++;<br />
            stop();<br />
            go_backward();<br />
            if (foo_left%3 == 0)<br />
              {<br />
                turn_right();<br />
                foo_right = 0;<br />
                foo_left = 0;<br />
            }<br />
            else{<br />
                turn_left();<br />
            }<br />
            last_turn = 0;<br />
            system_time = seconds();<br />
        }<br />
        if ( (digital(rt_bump) == 1) ) //right turn<br />
          {<br />
            foo_right ++;<br />
            stop();<br />
            go_backward();<br />
            if (foo_right%3 == 0)<br />
              {<br />
                turn_left();<br />
                foo_right = 0;<br />
                foo_left = 0;<br />
            }<br />
            else {<br />
                turn_right();<br />
                last_turn = 1;<br />
                system_time = seconds();<br />
            }<br />
        }<br />
        defer();<br />
    }<br />
}</p>
<p>void main() //Begin the main program<br />
{<br />
    printf(&quot;JCN v20.0 by Andrew and Mariah\n&quot;);<br />
    //sleep(1.0);<br />
    beep ();<br />
    fd(mill_motor); //start the mill motor thingy<br />
    start_process(main_movement() ); //start the main set of movements, including collision avoidance<br />
    start_process(go_home() ); //start the master clock, and prepare to go home<br />
    start_process(count_both() );<br />
    start_process(stuck_master() );</p>
<p>    while (1)<br />
      {</p>
<p>        line_found = 0;<br />
        while ( (in_motion) &amp;&amp; ( (digital(break_beam_a) == 0) || (digital(break_beam_b) == 0) )){<br />
            //printf(&quot;%d - %d\n&quot;, digital(14), digital(15) );<br />
            ir_sense(); //starts the black-line finding sub-routine<br />
            sleep(.01);<br />
        }<br />
        printf(&quot;%d - %d\n&quot;, digital(14), digital(15) );<br />
        //printf(&quot;Ball NOT in hopper\n&quot;);<br />
        sleep(.01);<br />
    }<br />
}<br />

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.