A Sample Control Program

Here is a sample program for a robot that follows the bright light source, and turns around any obstacles it runs into. The program also demonstrates how to make use of the START and STOP buttons on the Handy Board. The START button is used to "aclimatize" the robot to ambient light. The STOP button is used to stop the robot's program.

/* Control program for a LEGO Bug with two touch sensors and two light sensors*/

/* Touch sensors */

int LAntler = 15;               /* Left touch sensor in digital port 15 */
int RAntler = 7;                /* Right touch sensor in digital port 7 */

/* Photo sensors */

int LLight = 6;                 /* Left sensor in analog port 6 */
int RLight = 0;                 /* Right sensor in analog port 0 */

/* Photo sensor caliberations (initial) */
int Shadow = 240;               /* Readings in shadows are >= 240 */
int Ambient = 210;              /* Readings in ambient light are between 210 and 240 */
int Midrange = 150;             /* Readings in midrange are between 150 and 210 */
int Bright = 0;                 /* Readings for bright light are between 0 and 150 */

/* motors */
int Lmotor = 0;                                 /* Left motor */
int Rmotor = 3;                                 /* Right motor */

int FullForward = 100;
int SlowForward = 20;
int FullBackward = -100;
int SlowBackward = 20;
int STOP = 0;

void TurnLeft() {               /* Causes the robot to turn left */
                tone(880.0, 0.5);
                motor(Rmotor, FullForward);
                motor(Lmotor, STOP);
} /* TurnLeft */

void TurnRight() {
                tone(440.0, 0.5);
                motor(Lmotor, FullForward);
                motor(Rmotor, STOP);
} /* TurnRight */

void AllStop() {
                alloff();
                msleep(1000L);
} /* AllStop */

void GoStraight() {
                motor(Rmotor, FullForward);
                motor(Lmotor, FullForward);
} /* GoStraight */

void BackUp() {
                tone(100.0, 1.0);
                motor(Rmotor, FullBackward);
                motor(Lmotor, FullBackward);
                msleep(1000L);
} /* BackUp */

void SetAmbient() {             /* Measures the amount of ambient light */
          int L = analog(LLight), R = analog(RLight);
                
                if (L < R)
                                Ambient = L;
                else
                                Ambient = R;
                Ambient -= 20;
                printf("%d>Press START...\n", Ambient);
                start_press();
}       /* SetAmbient */

void main() {             /* The main control program */

        int LeftTouch, RightTouch;                                                                      /* Touch flags */
        int LeftReading, RightReading;              /* Left and right photo readings */

        SetAmbient();

        while (1) {                                                                                                                                                                     /* The main loop */

               /* Get readings from antlers*/
               LeftTouch = digital(LAntler);
               RightTouch = digital(RAntler);
               printf("Left=%d, Right=%d  ", LeftTouch, RightTouch);

               if (! (LeftTouch || RightTouch)) {/* Not touching anything, go with the light */
                   /* Get readings from photosensors */
                      LeftReading = analog(LLight);
                      RightReading = analog(RLight);
                      printf("R=%d, L=%d\n", LeftReading, RightReading);

                      if (LeftReading < Ambient)
                           TurnLeft();
                      else if (RightReading < Ambient)        
                           TurnRight();
                      else if ((LeftReading > Shadow) && (RightReading > Shadow))
                           AllStop();
                      else
                           GoStraight();
                }
                else {
                    AllStop();
                    BackUp();
                    if (LeftTouch)
                          TurnRight();
                    else if (RightTouch)
                                TurnLeft();
                    msleep(500L);
                }
                                                
                if (stop_button()) {
                         printf("Good Bye...");
                         AllStop();
                         break;
                }
        }       /* while */

}       /* main */

Previous: Touch & Light Sensors

Back to Using Robots in an AI Course