HallBot Interactive C code
#use "use-sonar.ic"
#use "explego.icb"

int sonar1 = 100000;
int done = 0;
#define desired_dist 8000.0

#define REAR_MOTOR 3
#define STEER_MOTOR 0
#define STEER_SPEED 10
#define BUMP 2

#define STEP 20

#define FL 2540
#define F  2040
#define FR 1540
#define RF 1040
#define R  540
#define RB 40
#define FRONT_THRESHOLD 6000.0

#define Kp .7
// proportional gain
#define Kd 0.0
// derivative gain

void ctr()
{
    int pos = (lego0_counts > 0);
    
    while(lego0_counts != 0)
    {
      if(pos)
        motor(STEER_MOTOR, -STEER_SPEED);
      else 
        motor(STEER_MOTOR, +STEER_SPEED);
      printf("center Angle: %d\n", lego0_counts);
    }
    if(pos)
      motor(STEER_MOTOR, +STEER_SPEED);
    else
      motor(STEER_MOTOR, -STEER_SPEED);
    sleep(0.07);
    off(STEER_MOTOR);
    
//    if(digital(BUMP))
//      lego0_counts = 0;
    
}

void l(int num)
{
    while(lego0_counts < num)
      {
        motor(STEER_MOTOR, STEER_SPEED);
        printf("left Angle: %d\n", lego0_counts);
    }        
    motor(STEER_MOTOR, -STEER_SPEED);
    sleep(0.07);
    off(STEER_MOTOR);
}

void r(int num)
{
    while(lego0_counts > -num)
    {
        motor(STEER_MOTOR, -STEER_SPEED);
        printf("right Angle: %d\n", lego0_counts);
        
    }
    motor(STEER_MOTOR, STEER_SPEED);
    sleep(0.07);
    off(STEER_MOTOR);
}

void main()
{
    int pid1;
    int pid2;
    int pid3;
    
    printf("Not started\n");
    while(!start_button());
    /*
    #if 1
    test_steering();
    #else
     */
    
    
    
    pid1 = start_process(moveServo());
    //    pid2 = start_process(detectSonar());
    pid3 = start_process(wallFollow());
    
    while(!stop_button());
    done = 1;
    
    alloff();
    kill_process(pid1);
    //    kill_process(pid2);  
    kill_process(pid3);  
//    #endif
    
}

void test_steering()
{
    int pid = start_process(wallFollow());
    
    printf("Left\n");
    turnLeft();
    sleep(2.0);
    
    printf("Right\n");
    turnRight();
    sleep(2.0);
    
    printf("Right\n");
    turnRight();
    sleep(2.0);
    
    printf("Left\n");
    turnLeft();
    sleep(2.0);
    
    printf("Center\n");
    center();
    sleep(2.0);
    
    /*printf("Right\n");
    turnRight();
    sleep(2.0);
    
    printf("Center\n");
    center();
    sleep(2.0);
    
    printf("Center");
    center();
    sleep(2.0);
    */
    //kill_process(pid);
}

void moveServo()
{
    int region[] = {RB, R, RF, FR, F, FL};
    float currMinRange[] = {desired_dist, desired_dist, desired_dist, 20000.0, 20000.0};
    float error[] = {desired_dist, desired_dist, desired_dist, 20000.0, 20000.0};
    float last_distance;
    init_expbd_servos(1);
    
    sonar_init();
    
    while (!done) {
        float distance;
        float last_distance;
        int i, r;
        servo0 = RB;
        sleep(0.7);
        beep();
        // Iterate through each sonar region
        for (r = 0; r < 5; ++r)
          {
            float min_side_error;
            float min_front_dist;
            int pos;
            int count = 0;
            // Save the last reading for comparison before writing
            // a new one.
            last_distance = currMinRange[r];
            currMinRange[r] = 0.0;
            
            // Iterate through all valid positions in the sonar region
            for (pos = region[r]; pos < region[r + 1]; pos+=STEP)
              {
                
                // Move sonar to new position
                servo0 = pos;
                
                // Check the sonar and update the minimum value.
                distance = (float)sonar_sample();
//                if (distance != -1L){// && distance < currMinRange[r]) {
                count += 1;
                currMinRange[r] += distance;
                //    }
            }

            //printf("currmin = %d\n", (int)(currMinRange[r]-32000L));
            //sleep(.5);
            currMinRange[r] = currMinRange[r]*(1.0/(float)count);

         //   printf("minrange = %f, count = %d\n", (float)currMinRange[r],(int)count);
          //  sleep(.5);
            /* NASTY HACK: in long hallways, we would read really low
             * values that we shouldn't see, ever.  Our hypothesis is
             * that this is caused by echoes that take longer
             * than the period of the sonar.  Our solution is to
             * change any suspiciously close value to be "really far."
             */
            if (currMinRange[r] < 2000.0)
              currMinRange[r] = 20000.0;    
            
            // Proportional Derivative Control
            //printf("Range: %d     Loc: %d\n", (int)currMinRange[r], r);
            
            // Use PD control on side sensor regions
            if (r < 4) {
                float p_error = (currMinRange[r] - desired_dist) * Kp;
                float d_error = (currMinRange[r] - last_distance)* Kd;
                error[r] = p_error + d_error;
            }
            
            if (currMinRange[4] > currMinRange[3])
              min_front_dist = currMinRange[4];
            else min_front_dist = currMinRange[3];
            
            
            // Compute minimum error over the side region
            // First, set error to really big numbers, because
            // we'll be finding lower numbers and want
            // to keep the smallest.
            
            min_side_error = 20000.0;
            for(i = 0 ; i < 4; ++i)
              if (error[i] < min_side_error)
                min_side_error = error[i];
            
            //            printf("front = %d ", min_front_dist);
            //           printf("side = %d\n", (min_side_error + desired_dist));
            
            // Act on errors:
            // If we're about to hit something, turn left.
            // Otherwise, align with right wall at desired dist.
            if (currMinRange[4] < FRONT_THRESHOLD)
              {printf("front =%f side_err=%f front-left\n",min_front_dist, (min_side_error)); turnLeft();}
            // turnLeft();
            else if (min_side_error > 1000.0)
                {printf("front =%f side_err=%f right\n", min_front_dist, (min_side_error)); turnRight();}
              // turnRight();
              else if (min_side_error < -500.0)
                  {printf("front =%f side_err=%f left\n", min_front_dist, (min_side_error)); turnLeft();}
                //  turnLeft();
                else
                  {printf("front =%f side_err=%f center\n", min_front_dist, (min_side_error)); center();}
            //  center();
        }
    }        
}

/*
 * STEERING CONTROL
 *
 * The turnRight, turnLeft, and Center functions set a global variable
 * to indicate to the wallFollow function which direction to turn the 
 * front wheels.
 */

int STEER_DIRECTION = 0;

void turnRight()
{
    STEER_DIRECTION = 1;
}

void center()
{
    STEER_DIRECTION = 0;
}

void turnLeft()
{
    STEER_DIRECTION = -1;
}

// This function follows walls on the right
void wallFollow()
{
    int prev_direction = 0;
    
    // The robot should start out with its steering centered.
    // We give a brief audio warning if this doesn't seem to be the case.
    if(!digital(BUMP)) beep();
    
    // Start moving!
    motor(3,60);
    
    // Turn vehicle based on global steering direction.
    // This variable is set by the turning commands above.
    while(!done){
        
        // turn the robot if it needs turning 
        float turn_time = .5;
        if (STEER_DIRECTION != prev_direction)
          {
            // Center wheels
            if (STEER_DIRECTION == 0)
              {
                ctr();
                if (prev_direction == 1)
                  l(1);
                else r(1);
                
                ctr();
                /*                
                // Bring motor back from previous direction.
                motor(STEER_MOTOR, -prev_direction * 4);
                while (!digital(BUMP));
                beep();
                // This will overshoot a bit.
                motor(STEER_MOTOR, -prev_direction * 5);
                sleep(turn_time + .5);
                motor(STEER_MOTOR, 0);
                printf("centering now");
                beep();
                // Return to center.
                motor(STEER_MOTOR, prev_direction * 4);
                while (!digital(BUMP));
                beep();              
                /*
              // Correct overshoot by moving wheels until
              // the bump sensor detects centering
  
              while(!digital(BUMP))
                  motor(STEER_MOTOR, -prev_direction * 2);
*/
/*                
                // Done turning
                motor(STEER_MOTOR, 0);
                
                printf("CENTER");
*/            }      
            // Turn left or right
            else
              {
                // If the motor wasn't centered, we have to recenter it
                // before we turn the other direction
                if (prev_direction != 0)
                  {
                    ctr();
                    
                    
                    /*
                    motor(STEER_MOTOR, STEER_DIRECTION * 4);
                    while (!digital(BUMP))
                      {
                        sleep(.05);
                        beep();
                    }   
                    //  sleep(0.05);
*/                 }
                
                if(STEER_DIRECTION == 1)
                  r(2);
                else
                  l(2);
                
                ctr();
                /*                
                motor(STEER_MOTOR, STEER_DIRECTION * 5);    
                sleep(turn_time);
                
                // Done turning
                motor(STEER_MOTOR, 0);
*/            }
            
            prev_direction = STEER_DIRECTION;
        }
    }
    
    // All done.
    alloff();
}