/*  **********************************************************************
    Interactive C program for controlling a robot

      Creator:  Susan Fox
      Date:  Apr 29, 1998
     
      Contents:  This file contains a program which moves the robot
                 forward a specified amount, turns a specified amount,
                 and moves forward again.  It then stops.

    **********************************************************************  */


/* --------------------------------------------------------------------- */
/*  This section defines general constants */

int true = 1;
int false = 0;


/*  This section defines constants for the digital inputs and the 
    motor inputs */

int leftMotor = 1;
int rightMotor = 3;


int leftTouch = 7;
int rightTouch = 8;


/* --------------------------------------------------------------------- */
/*  This section defines a few standard speeds for the robot */

int slow = 10;
int half = 50;
int full = 100;


/* --------------------------------------------------------------------- */
/*  This section defines helping procedures to interact with the touch 
    sensors */


/* return true if the given sensor is currently touched */
int touched(int sensor)
{
   return(digital(sensor));
}


/* return true if both sensors are currently being touched, false otherwise */
int bothTouched()
{
   return( touched(leftTouch) && touched(rightTouch));
}


/* --------------------------------------------------------------------- */
/* This section has helping procedures for moving in standard ways */


/* Move forward at the given speed, straight ahead */
void forwardForever(int speed)
{
   motor(leftMotor, speed);
   motor(rightMotor, speed);
}


/* Move forward at the given speed for the given amount of time, where
   time is given in milliseconds */
void forward(int speed, int time)
{
   motor(leftMotor, speed);
   motor(rightMotor, speed);
   msleep((long)time);
   ao();
}


/* Move straight backward... note speeds should be given as 
   positive values */
void backForever(int speed)
{
   motor(leftMotor, (0 - speed));
   motor(rightMotor, (0 - speed));
}


/* Move straight backward for the given number of milliseconds */
void backward(int speed, int time)
{
   motor(leftMotor, (0 - speed));
   motor(rightMotor, (0 - speed));
   msleep((long) time);
   ao();
}


/* Stop moving */
void stop()
{
   ao();
}


/* Turn right in place, forever, without moving forward */
void spinRight(int speed)
{
   motor(leftMotor, speed);
   motor(rightMotor, (0 - speed));
}


/* Turn right for the specified number of milliseconds */
void turnRight(int speed, int time)
{
   motor(leftMotor, speed);
   motor(rightMotor, (0 - speed));
   msleep((long) time);
   ao();
}

/* Turn left in place, forever, without moving forward */
void spinLeft(int speed)
{
   motor(leftMotor, (0 - speed));
   motor(rightMotor, speed);
}


/* Turn left for the specified number of milliseconds */
void turnLeft(int speed, int time)
{
   motor(leftMotor, (0 - speed));
   motor(rightMotor, speed);
   msleep((long) time);
   ao();
}

/* --------------------------------------------------------------------- */
/*  The following section contains the main program and any helping 
    procedures it needs.  THIS SECTION IS THE ONLY ONE THAT YOU SHOULD
    CHANGE!!!    */


void main()
{
   forward(half, 3000);     /* move forward at half speed for 3 seconds */
   turnRight(slow, 2500);   /* turn right slowly for 2.5 seconds */
   forward(full, 5000);     /* move forward at full speed for 5 seconds */
   spinLeft(full);          /* do a victory dance */
   msleep((long) 4000);            /*  for four seconds  */
   ao();                    /* then come to a stop  */
}  /* end of main */