/* ------------------------------------------------------------------------
File: example.c
This program demonstrates motors, LEDs, and buzzer.

This program uses the Engr/CS 101 Version 3 robotics library for
Version 6 of the controller board.

Keil C headers are not guarded.  If robotics library is used, its header file 
should be the only included file.
---------------------------------------------------------------------------*/

#include "robotics6b.h"

/* Constant declarations */
#define DELAY_TIME 20   /* about 1 sec */
#define LIGHT_PULSE 20  /* About 1 sec */

void main (void)
{ 
   /* Variable declarations */
   unsigned char i;

   /* Initialization */
   InitializeMotors();
   InitializeIO();
   Delay (DELAY_TIME);  /* Wait about 1 sec */

   while (1)  /* Repeat forever */
   {
      /* Test LEDs */
      for (i = 11; i <= 14; i++)   /* 4 middle ports */
      {
         Beep(1);
         SetOutputPort(i, LOW);    /* Turn on LED i */
         Delay (LIGHT_PULSE);
         SetOutputPort (i, HIGH);  /* Turn off LED i */
         Delay (LIGHT_PULSE);
      }
   
      /* Forward */
      Beep (2);
      SetMotor (1, 255);
      SetMotor (2, -255);
      Delay (DELAY_TIME*5);
      AllStop();

      /* Left Turn */
      Beep(3);
      SetMotor (1, 255);
      SetMotor (2, 255);
      Delay (DELAY_TIME*2);
      AllStop();

      /* Reverse */;
      Beep(2);
      SetMotor (1, -255);
      SetMotor (2, 255);
      Delay (DELAY_TIME*5);
      AllStop();

      /* Right Turn */;
      Beep(3);
      SetMotor (1, -255);
      SetMotor (2, -255);
      Delay (DELAY_TIME*2);
      AllStop();

      Beep (5);
      Delay (DELAY_TIME*2);
   }
}