Drinks vending robot sketch - alanesq.com

/*
  
  drinks robot - robot to fetch drinks from the vending machine 
  May 2012 - v 25/6/12   v2.0
  Pins used:
      Motor control on pins 4-7
      LCD display on pins 36, 34, 32, 30, 28 and 26
      Onboard LED on pin 13
      3 x ultrasonic range finders on pins front=1,2 left=45,44  right=43,42
      4 button Radio remote receiver on pins 8-11
      Sounder on pin 49
      PS2 mouse on 68,69 (analog pins)
      Cup platform sensor on pin 23
      Tri colour LEDs on pins 19-21
      under body lights on pins 62-67
      Front bumper sensor on pin 18
      Servo for front ultrasonic sensor on pin 3
      Battery voltage sensor on pin A0
      
      
  Radio buttons:
      top button = 1, bottom button = 3, ringer = 4
      radiobutton stores what button has been pressed
      button 3 = emerency stop
      button 4 = call robot to collect cup
 */
 
#include <LiquidCrystal.h> // LCD display
#include "Ultrasonic.h" // ultrasonic range finders
#include <MemoryFree.h>       // discover free ram
#include <ps2.h> // ps2 mouse
#include <Servo.h> // servo for front ultrasonic sensor
#include <TimerOne.h>         // interrupts using timer1
//  note - to use timer1 in this sketch you have to comment out "#define _useTimer1" in the servo.h file (in the servo library)
// Misc settings
     int mturn = 25000; // ps2 mouse pixels per 360 turn
     int mcm = 180; // ps2 mouse pixels per cm of travel forward or back
     
     // delay between switching relays (to stop spikes on power line upsetting the Arduino)
     const int switch_delay = 100;
unsigned long mtime = 0; // timer to cut power to motors if they are running too long
int led = 13; // onboard LED
int bumper = 18; // front bumper (fender)
int batvol = A0; // Battery voltage sensor
int cup = 23; // cup on platform sensor
String location = "parked"; // current location of robot (parked, table, vender)
int ledr = 19; // led warning lights (on front risers)
int ledg = 20; int ledb = 21; int uleds[6] = {62,63,64,65,66,67}; // under body leds
int sounder = 49; // sounder
int radioa = 10; // 4 button radio remote
int radiob = 9; int radioc = 11; int radiod = 8; int radiobutton = 0; // button pressed
int range; // current reading on ultrasound range finder
int left_on = 5; // left motor on/off
int right_on = 6; // right motor on/off
int left_dir = 4; // left motor direction select
int right_dir = 7; // right motor direction select
boolean robot_error = false; // flag of error occured
int qtemp = 0; // general temp variable
// initialize the lcd display
  LiquidCrystal lcd(36, 34, 32, 30, 28, 26);
// initialise ultrasonic range finder
  Ultrasonic ultrasonic_front(2,1);
  Ultrasonic ultrasonic_left(44,45);
  Ultrasonic ultrasonic_right(43,42);
// initialise PS2 mouse
  PS2Mouse mouse(68, 69);
// initialise servo
  Servo myservo; // create servo object to control a servo
// motor control 
  const boolean forward = LOW;
  const boolean reverse = HIGH;
  
//  ---------------------------------------------------------
void setup() { // set up servo (front ultrasonic sensor)
  myservo.attach(3); // attaches the servo on pin 9 to the servo object
  myservo.write(90); // point forward
  
  
// set up the LCD's number of columns and rows: 
   lcd.begin(16, 2);
// initialise PS2 mouse
  mouse.init();
//set pin modes
  pinMode(left_on, OUTPUT);   pinMode(right_on, OUTPUT);   pinMode(left_dir, OUTPUT);   pinMode(right_dir, OUTPUT);   pinMode(led, OUTPUT);   pinMode(radioa, INPUT);   pinMode(radiob, INPUT);   pinMode(radioc, INPUT);   pinMode(radiod, INPUT);   pinMode(sounder, OUTPUT);   pinMode(cup, INPUT);   pinMode(bumper, INPUT);   pinMode(ledr, OUTPUT);   pinMode(ledg, OUTPUT);   pinMode(ledb, OUTPUT);   for ( int i = 0 ; i < 6 ; i++) pinMode(uleds[i], OUTPUT); // Make sure led warning lights are all off
  digitalWrite(ledr, LOW);   digitalWrite(ledg, LOW);   digitalWrite(ledb, LOW);   for ( int i = 0 ; i < 6 ; i++) digitalWrite(uleds[i],LOW); // Print a message to the LCD.
   // read battery voltage and pull out 1st dec place
      float tvolts = ((float)analogRead(batvol) / 24.5 ); // read voltage
      float qvolts = tvolts * 10.0;
      float wvolts = (float)int(tvolts) * 10.0;
   lcdshow ( "Venderbot 1.0" , "Mem" + String(freeMemory()) + " Bat" + String(int(tvolts)) + "." + String(int(qvolts)-int(wvolts)) + "v" );
   digitalWrite(ledr,HIGH);
   delay(200);
   digitalWrite(ledr,LOW);
   digitalWrite(ledg,HIGH);
   delay(200);
   digitalWrite(ledg,LOW);
   digitalWrite(ledb,HIGH);
   delay(200);
   digitalWrite(ledb,LOW);
   delay(600);
   underlights ( 2 , 50 );   // flash under body lights
   lcdshow ( " " , " " );
   
//setup radio keyfob button receiver
  digitalWrite(radioa, HIGH); //turn on pullup resistor on radio pin
  digitalWrite(radiob, HIGH); //turn on pullup resistor on radio pin
  digitalWrite(radioc, HIGH); //turn on pullup resistor on radio pin
  digitalWrite(radiod, HIGH); //turn on pullup resistor on radio pin
//turn on internal pullup resistor on cup sensor pin
  digitalWrite(cup, HIGH); //turn on pullup resistor on radio pin
//turn on internal pullup resistor on front bumper sensor pin
  digitalWrite(bumper, HIGH); //turn on pullup resistor on radio pin
//make sure motors are off
  digitalWrite(left_on, LOW);   digitalWrite(right_on, LOW); //make sure sounder is off
  digitalWrite(sounder, LOW); // Interrupt stuff - initialize Timer1 to update every half second
  Timer1.initialize(500000); // set a timer of length in microseconds ( 1,000,000 = 1 second )
  Timer1.attachInterrupt( RoboInter ); // attach the service routine here
}    // end of setup
//  ---------------------------------------------------------
void loop(){ //loop0
// make sure no cup on platform
  if ( readcup() == "on" ) {
     lcdshow( "Cup on platform" , "please remove" );
     warning_sound(1);            // sound warning
     while ( readcup() == "on" ) delay(200);
  }
// clear lcd screen
  lcdshow( " " , " ");
// wait for a radio remote button to be pressed or cup on platform 
    lcdshow ( "Waiting...." , "location=" + location );
    int qtmp=0; // counter
    while ( radiobutton == 0 && readcup() == "off" ) {
      qtmp = qtmp + 1;   // Increment counter
      if ( qtmp > 10 ) {
        qtmp = 0;
        digitalWrite(ledb, HIGH); // flash blue led
        delay(3);
        digitalWrite(ledb, LOW); // flash blue led
      }
      delay(300);
    }
    lcdshow( " " , " " );
    warning_sound(1);            // sound warning
// button 1 - Move
  if ( radiobutton == 1 ) {      radiobutton = 0;   // clear radio button pressed flag
     if ( location == "table" ) {
       //table to park
       lcdshow ( "Going to park" , "Bye..." );
       move_TableToLib();       // move from table to library
       move_LibDoorToPark();    // move from library to park
       warning_sound(1);
       underlights ( 1 , 5 );   // flash under body lights
     }
     else if ( location == "vender" ) {
        // Move from vending machine to Parlour table
           move_VendToLib();       // move from vending machine to Library Door
           move_LibToTable();      // move from Library Door to table
        // finished
           digitalWrite(ledr, LOW); // warning led off
           warning_sound(2);
           lcdshow( "Finished" , " " );
     }
     else {
         // wrong location
         lcdshow( "Error" , "wrong location" );
         warning_sound(3);
     }
  }  // if button 1
// button 2 - change location
  if ( radiobutton == 2 ) {       radiobutton = 0;   // clear radio button pressed flag
      if ( location == "parked" ) location = "table"; // set location
      else if ( location == "table" ) location = "vender"; // set location
      else if ( location == "vender" ) location = "parked"; // set location
      lcdshow( "Location set to" , location );
      warning_sound(1);
  } // if button 2
// button 4 (bell) - come to table (from parked)
  if ( radiobutton == 4 ) {      radiobutton = 0;   // clear radio button pressed flag
     if ( location == "parked" ) move_ParkToTable(); // move from vending machine to parlour table
     else {
         // wrong location
         lcdshow( "Error" , "wrong location" );
         warning_sound(3);
     }
  }  // if button 4 (bell)
// If cup on tray collect a drink
  if ( readcup() == "on" ) { // in case of false alarm - chance to change mind
     lcdshow( "Cup on platform" , " :-)");
     underlights ( 1 , 8 );   // flash under body lights
  }
  if ( readcup() == "on" ) { // if still on platform then fetch a coffee
     if ( location == "table" ) {
       collectDrink();    // collect a drink
     }
     else {
         // wrong location
         lcdshow( "Error" , "wrong location" );
         warning_sound(3);
     }
  }
delay(1000);
}   // end of loop
//  ---------------------------------------------------------
// move robot (mode0)
// inputs are;   mode , direction , time to run motors or distance to find , sensor to use
//     note: negative NUM means very slow
//     modes
//     1 = set time
//     2 = find a distance (moving towards)
//     3 = find a distance (moving away from)
//     4 = until distance greater than
//     5 = until distance less than
//     6 = follow wall using sensor for set time
//     7 = guided by PS2 mouse 
//     direction =  forward , reverse , left or right
//     sensor to use = front left or right
void move_robot ( int mode , String direct , int num , String sensor ) {
  
  // check a valid mode was supplied
  if ( mode < 1 || mode > 7 ) error( "ERROR (move)" , "invalid mode" , "mode=" + String(mode) );
  // check a valid sensor was supplied
  if ( mode > 1 && mode < 7 ) { // i.e. sensor is used
    if ( sensor != "left" && sensor != "right" && sensor != "front" ) {
      // invalid sensor supplied
      error( "ERROR (move)" , "invalid sensor" , "sensor=" + String(sensor) );
    }
  }
  // set motor directions
    set_direction( direct );
  // turn motors on
    mtime = millis(); // log time motors were turned on (interupt cuts power if they are on too long)
    // take up slack in drive train
      analogWrite(right_on, 15);       analogWrite(left_on, 15);       delay(100);
      analogWrite(right_on, 50);       analogWrite(left_on, 50);       delay(50);
    analogWrite(right_on, 100); //slow
    analogWrite(left_on, 100); //slow
    if ( (analogRead(batvol) / 24.5) < 12 ){ // if battery low then run motors a bit faster
      analogWrite(right_on, 110); //slow
      analogWrite(left_on, 110); //slow
    }
    delay(100);
    // if not turning go to full speed
    if ( direct == "forward" || direct == "reverse" ) {
      if ( num > 0 ) { // negative means very slow
        analogWrite(right_on, 150);         analogWrite(left_on, 150);         delay(100);
        digitalWrite(right_on, HIGH); //full on
        digitalWrite(left_on, HIGH); //full on
      }
    }
    num = abs(num); // remove negative
  // -----------------------
  // mode1 - move set time
  if ( mode == 1 ) {
    //time setting supplied so run motors for set time
    lcdshow ( "Move for" , String(int(num / 1000)) + " seconds" );     digitalWrite(uleds[0],HIGH); // under body light
    delay(num); // wait
    digitalWrite(uleds[0],LOW); // under body light
  }
  // -----------------------
  // mode2/mode3 - find set distance
  else if ( mode == 2 || mode == 3 ) {
     // distance setting supplied so run until this distance from object using ultrasound
     digitalWrite(uleds[1],HIGH); // under body light
     // seperate lines as the LCD locks for some reason if I try to just use the variable direct ??
       if ( direct == "forward" ) lcdshow ( "FORWARD to " + String(num) + "cm " , " " );        else if ( direct == "reverse" ) lcdshow ( "REVERSE to " + String(num) + "cm " , " " );        else if ( direct == "left" ) lcdshow ( "LEFT to " + String(num) + "cm " , " " );        else if ( direct == "right" ) lcdshow ( "RIGHT to" + String(num) + "cm " , " " );      int tcount=0; // temp counter (timeout if motors running too long)
     int us = read_ultrasonics( sensor , 90 ); // get readings from ultrasonic sensor
      
     int temp = 0 ; // temp flag
     int slow = 50; // slow speed when close
     while ( tcount < 200 && temp == 0 ) {
       delay (300);
       tcount = tcount + 1;  // increment counter
       int us = read_ultrasonics( sensor , 90 ); // get readings from ultrasonic sensor
       // check if in correct position
       if ( mode == 2 && us != 0 ) { // if moving towards or away from obsticle
         // moving towards
         if ( us < (num + 10) ) { // if close slow down
            analogWrite(right_on, slow); //slow
            analogWrite(left_on, slow); //slow
         }
         if ( us < (num + 1) ) temp = 1; // correct possition so stop
       }
       else if ( mode == 3 && us != 0 ) {
         // moving away from
         if ( us > (num - 10) ) { // if close slow down
            analogWrite(right_on, slow); //slow
            analogWrite(left_on, slow); //slow
         }
         if ( us > (num - 1) ) temp = 1; // correct possition so stop
       }
     } 
     digitalWrite(uleds[1],LOW); // under body light
     //check there was no problem
     if ( tcount >= 200 ) {
        //robot timed out
        error( "ERROR (move 2/3)" , "timed out" , " " );
     }
  }
  // -----------------------
  // mode4/mode5 - move until distance less than or greater than
  else if ( mode == 4 || mode == 5 ) {
     // distance setting supplied so run until this distance from object using ultrasound
     digitalWrite(uleds[2],HIGH); // under body light
     // seperate lines as the LCD locks for some reason if I try to just use the variable direct ??
       if ( direct == "forward" ) lcdshow ( "FORWARD til " + String(num) + "cm " , " " );        else if ( direct == "reverse" ) lcdshow ( "REVERSE til " + String(num) + "cm " , " " );        else if ( direct == "left" ) lcdshow ( "LEFT til " + String(num) + "cm " , " " );        else if ( direct == "right" ) lcdshow ( "RIGHT til " + String(num) + "cm " , " " );      int tcount=0; // temp counter (timeout if motors running too long)
     int us = read_ultrasonics( sensor , 90 ); // get readings from ultrasonic sensor
      
     int temp = 0 ; // temp flag
     while ( tcount < 200 && temp == 0 ) {
       delay (300);
       tcount = tcount + 1;  // increment counter
       int us = read_ultrasonics( sensor , 90 ); // get readings from ultrasonic sensor
       // check if in correct position
       if ( mode == 4 && us != 0 ) { // until distance greater than
         if ( us > num ) temp = 1; // correct possition so stop
       }
       if ( mode == 5 && us != 0 ) { // until distance less than
         if ( us < num ) temp = 1; // correct possition so stop
       }
       
     } 
     digitalWrite(uleds[2],LOW); // under body light
     //check there was no problem
     if ( tcount >= 200 ) {
        //robot timed out
        error( "ERROR (move 3-4)" , "timed out" , " " );
     }
  }
  // -----------------------
  // mode6 - follow wall
  else if ( mode == 6 ) {
     // follow a wall ( i.e. run parallel to it )
     // this is done by varying the speed of one motor if the distance to wall is increasing
    digitalWrite(uleds[3],HIGH); // under body light
    int mod = 90; // amount to vary speed by
    int tdelay = 500; // time between checking course
    int tadj = 500; // time to adjust course
    int utemp = read_ultrasonics( sensor , 90 ); // get initial reading from sensor
    int tcnt = 0; // time counter
 
    while (tcnt < num) {
      tcnt = tcnt + tdelay;    // increment counter
      // adjust steering if distance from wall is changing
      int us = read_ultrasonics( sensor , 90 ); // get readings from ultrasonic sensor
      lcdshow ( "follow wall" , " " ) ;
      // set new motor speeds (depending on which side sensor is being used and distance from wall)
      if ( sensor == "left" ) {
        if ( us == utemp ) {
          lcdshow( "" , "course ok" ) ;
          analogWrite(right_on, 150);           analogWrite(left_on, 150);
        }
        else if ( us > utemp ) { // distance increased
          lcdshow( "" , "turning left" ) ;
          analogWrite(right_on, 150 + mod); // turn left
          analogWrite(left_on, 150 - mod );
          delay(tadj);
          analogWrite(right_on, 150);           analogWrite(left_on, 150);
          tcnt = tcnt + tadj;    // increment counter
          utemp = read_ultrasonics( sensor , 90 );   // get readings from ultrasonic sensor
        }
        else if ( us < utemp ) { // distance decreased
          lcdshow( "" , "turning right" ) ;
          analogWrite(right_on, 150 - mod ); // turn right
          analogWrite(left_on, 150 + mod );
          delay(tadj);
          analogWrite(right_on, 150);           analogWrite(left_on, 150);
          tcnt = tcnt + tadj;    // increment counter
          utemp = read_ultrasonics( sensor , 90 );   // get readings from ultrasonic sensor
        }
      }
      if ( sensor == "right" ) {
        if ( us == utemp ) {
          lcdshow( "" , "course ok" ) ;
          analogWrite(right_on, 150);           analogWrite(left_on, 150);
        }
        else if ( us > utemp ) { // distance increased
          lcdshow( "" , "turning right" ) ;
          analogWrite(right_on, 150 - mod); // turn right
          analogWrite(left_on, 150 + mod );
          delay(tadj);
          analogWrite(right_on, 150);           analogWrite(left_on, 150);
          tcnt = tcnt + tadj;    // increment counter
          utemp = read_ultrasonics( sensor , 90 );   // get readings from ultrasonic sensor
        }
        else if ( us < utemp ) { // distance decreased
          lcdshow( "" , "turning left" ) ;
          analogWrite(right_on, 150 + mod ); // turn left
          analogWrite(left_on, 150 - mod );
          delay(tadj);
          analogWrite(right_on, 150);           analogWrite(left_on, 150);
          tcnt = tcnt + tadj;    // increment counter
          utemp = read_ultrasonics( sensor , 90 );   // get readings from ultrasonic sensor
        }
      }
      delay(tdelay);
    }
    digitalWrite(uleds[3],LOW); // under body light
  }
  
  // -----------------------
  
  // mode7 - guided by PS2 mouse
  else if ( mode == 7 ) {
     // guided by PS2 mouse
     digitalWrite(uleds[4],HIGH); // under body light
     // disable interrupt as it upsets the PS2 mouse - (called manually during this proc)
        Timer1.detachInterrupt(); //disable the timer1 interrupt
     // seperate lines as the LCD locks for some reason if I try to just use the variable direct ??
       if ( direct == "forward" ) lcdshow ( "FORWARD by ps2" , "to " + String(num) );        else if ( direct == "reverse" ) lcdshow ( "REVRSE by ps2" , "to " + String(num) );        else if ( direct == "right" ) lcdshow ( "RIGHT by ps2" , "to " + String(num) );        else if ( direct == "left" ) lcdshow ( "LEFT by ps2" , "to " + String(num) );       int tcount=0; // temp counter (timeout if motors running too long)
      int mxpos = 0; // current mouse data
      int mypos = 0; // current mouse data
    
      char mstat;
      char mx;
      char my;
    
      qtemp = 0;   // reset temp flag
      num = num - 300;    // to compensate for motor over run
      MouseInfo mouseInfo;  // call the mouse data structure
      // convert direct to a integer to make comparing faster below?
        int ndirect = 0;
        if ( direct == "forward" ) ndirect = 1;
        if ( direct == "reverse" ) ndirect = 2;
        if ( direct == "right" ) ndirect = 3;
        if ( direct == "left" ) ndirect = 4;
        if ( ndirect == 0 ) {
           // error
           error( "ERROR (mode7)" , "invalid direct" , String(direct) );
           while(1==1); // stop
      }      
      while ( qtemp == 0 ) { // keep checking mouse reading
          
          mouse.getData(&mouseInfo);      // get a reading from the mouse
      
          mxpos = mxpos + mouseInfo.x;    // update current mouse data
          mypos = mypos + mouseInfo.y;    // update current mouse data
    
          // check if target reached yet
    
          if ( ndirect == 1 ) { // forward
             // moving forward so y will be increasing
             if ( mypos > num ) qtemp = 1;           }
        
          else if ( ndirect == 2 ) { // reverse
             // moving backwards so y will be decreasing
             if ( abs(mypos) > num ) qtemp = 1;           }
    
          else if ( ndirect == 3 ) { // right
             // moving right so x will be decreasing
             if ( abs(mxpos) > num ) qtemp = 1;           }
    
          else if ( ndirect == 4 ) { // left
             // moving left so x will be increasing
             if ( mxpos > num ) qtemp = 1;           }
          //call interrupt routine manually
          RoboInter();
          
    }    //while
        
    digitalWrite(uleds[4],LOW); // under body light off
    // turn interrupt back on
        Timer1.attachInterrupt( RoboInter );   }   //if
  
  
  // -----------------------
  stop_robot();                       // stop
  lcdshow ( " " , " " ); // clear lcd
}  //proc
//  ---------------------------------------------------------
// stop moving    (stop0)
void stop_robot() {
  digitalWrite(right_on, LOW); //turn motor off
  digitalWrite(left_on, LOW); //turn motor off
  //display on lcd
    lcdshow( "stop moving" , " " );
    mtime = 0;   // log motors off (interrupt cuts power if they are on too long)
    // all under body lights off
    for ( int i = 0 ; i < 6 ; i++) digitalWrite(uleds[i],LOW);     underlights ( 2 , 8 );   // flash under body lights
}   //proc
//  ---------------------------------------------------------
// Timed Interrupt (inter1) 
void RoboInter() {
    if ( digitalRead(radioa) == LOW ) radiobutton = 1;     
    else if ( digitalRead(radiob) == LOW ) radiobutton = 2;     
    else if ( digitalRead(radioc) == LOW ) {
        // EPO button pressed
        radiobutton = 3;   
        stop_robot();
        digitalWrite(sounder,LOW); // sounder off
        digitalWrite(ledr, HIGH); // warning led on
        lcdshow( "STOPPED !" , "EPO button" );
        while (1); // stop
    }
    
    else if ( digitalRead(radiod) == LOW ) radiobutton = 4;     
    // stop robot if bumper activated (i.e. it has hit something)
    if ( digitalRead(bumper) == LOW ) {         // bumper activated
        stop_robot();
        digitalWrite(sounder,LOW); // sounder off
        digitalWrite(ledr, HIGH); // warning led on
        lcdshow( "STOPPED !" , "bumper hit !" );
        while (1==1); // stop
    }
    // stop robot if motors have been running too long
    if ( mtime != 0 && millis() > ( mtime + 25000 ) ) {
        stop_robot();
        digitalWrite(sounder,LOW); // sounder off
        digitalWrite(ledr, HIGH); // warning led on
        lcdshow( "STOPPED !" , "motor timeout" );
        while (1==1); // stop
    }      
}
//  ---------------------------------------------------------
// read ultrasonic range finders
// input = front, left or right sensor , angle in degrees (0-180) of front sensor
int read_ultrasonics( String sel , int angle ) {
  
    digitalWrite(uleds[5],HIGH); // under body light
    // verify valid sensor requested
    if ( sel != "front" && sel != "left" && sel != "right" ) {
        // invalid sensor
       error( "ERROR (UltraSnd)" , "invalid sensor" , String(sel) );
       while(1==1); // stop
    } 
    // verify valid sensor angle
    if ( sel == "front" ) {
         if ( angle < 0 || angle > 180 ) {          // invalid angle
         error( "ERROR (UltraSnd)" , "invalid angle" , String(angle) );
         while(1==1); // stop
         }
       } 
  
    // get readings from the ultrasound range finders
      int qtemp = 0;
      if ( sel == "front" ) {
        // front sensor so need to adjust sensor angle first
        myservo.write(angle);
        delay(400); // wait for sero to move
        qtemp = ultrasonic_front.convert(ultrasonic_front.timing(), Ultrasonic::CM);   
      }
      else if ( sel == "left" ) {
        qtemp = ultrasonic_left.convert(ultrasonic_left.timing(), Ultrasonic::CM);
      }
      else if ( sel == "right" ) {
        qtemp = ultrasonic_right.convert(ultrasonic_right.timing(), Ultrasonic::CM);   
      }
    
    
    // display results on LCD
      lcdshow ( "" , sel + " = " + String(qtemp) );
    // if front sensor has been turned, put it back to the front
      if ( sel == "front" && angle != 90 ) {
            //myservo.write(90);
      }
    if ( qtemp > 300 ) qtemp=0; // if over 3m then must be a false reading
    digitalWrite(uleds[5],LOW); // under body light
    return qtemp;
}
//  ---------------------------------------------------------
// display on LCD 
// inputs = line1 , line2
// if a line is "" then leave existing content
void lcdshow ( String line1 , String line2 ) {
 
   if ( line1 != "" ) {
     lcd.setCursor(0, 0);
     lcd.print(line1);
     lcd.print(" ");
   }
   
   
   if ( line2 != "" ) {
     lcd.setCursor(0, 1);
     lcd.print(line2);
     lcd.print(" ");
   }
   
}
//  ---------------------------------------------------------
// sound a warning
void warning_sound( int cnt ) {
  
     for ( int i = 0 ; i < cnt ; i++) {
       digitalWrite(sounder,HIGH);
       digitalWrite(ledb, HIGH); // warning led on
       delay(300);
       digitalWrite(sounder,LOW);
       digitalWrite(ledb, LOW); // warning led off
       delay(300);
     }
}
//  ---------------------------------------------------------
// set motor directions
// input = forward , reverse , left or right
void set_direction( String dir ) {
  
  int flag = 0; // temp flag
  
  if ( dir == "forward" ) {
    flag = 1;
    digitalWrite(right_dir, forward); //set right wheel for forward
    delay(switch_delay); //delay between switching to reduce spikes in power line
    digitalWrite(left_dir, forward); //set left wheel for forward
    delay(switch_delay); //delay between switching to reduce spikes in power line
  }
  
  if ( dir == "reverse" ) {
    flag = 1;
    digitalWrite(right_dir, reverse); //set right wheel for forward
    delay(switch_delay); //delay between switching to reduce spikes in power line
    digitalWrite(left_dir, reverse); //set left wheel for forward
    delay(switch_delay); //delay between switching to reduce spikes in power line
  }
  if ( dir == "right" ) {
    flag = 1;
    digitalWrite(right_dir, reverse); //set right wheel for forward
    delay(switch_delay); //delay between switching to reduce spikes in power line
    digitalWrite(left_dir, forward); //set left wheel for forward
    delay(switch_delay); //delay between switching to reduce spikes in power line
  }
  if ( dir == "left" ) {
    flag = 1;
    digitalWrite(right_dir, forward); //set right wheel for forward
    delay(switch_delay); //delay betwen switching to reduce spikes in power line
    digitalWrite(left_dir, reverse); //set left wheel for forward
    delay(switch_delay); //delay betwen switching to reduce spikes in power line
  }
  
  // check a valid direction was supplied
  if ( flag == 0 ) {
    error( "ERROR (set dir)" , "invalid directn" , "dir=" + String(dir) );
  }
}
//  ---------------------------------------------------------
//  correct any error in going stright by comparing a measurement with expected (correcte)
//  input = expected distance , sensor , amount to adjust by per cm out
void correct_e ( int dist , String sensor , int adj ) {
     int us = read_ultrasonics( sensor , 90 ); // get readings from ultrasonic sensor
     lcdshow( "Adjusting" , "1=" + String(dist) + " - 2=" + String(us) );
     delay (1000);
     // check adj is not too far out to be corrected
     if ( us > (dist+20) || us < (dist-20) ) error( "ERROR (correct)" , "Adjust too big" , "1=" + String(dist) + " - 2=" + String(us) );
     // if dist is greater
     if ( us > dist ) {
        // turn slightly
        lcdshow( "" , "dist. greater" );
        delay(1000);
        if ( sensor == "right" ) move_robot( 1 , "right" , adj*(us - dist) , "" );
        if ( sensor == "left" ) move_robot( 1 , "left" , adj*(us - dist) , "" );
     }       
  
     // if dist is less
     if ( us < dist ) {
        // turn slightly
        lcdshow( "" , "dist. less" );
        delay(1000);
        if ( sensor == "right" ) move_robot( 1 , "left" , adj*(dist - us) , "" );
        if ( sensor == "left" ) move_robot( 1 , "right" , adj*(dist - us) , "" );
     }       
}
//  ---------------------------------------------------------
//  correct any error in going stright by using two ultrasound sensors (correctu)
//  input = sensor (i.e. front left right) , correction (distance difference between sensors)
void correct_u ( String sensor , int corectn ) {
 
//     ------------------------------  
  
  if ( sensor == "front" ) {
       // measure front by varying angle of front sensor
       int lcamnt = 40; // amount to adjust turning left
       int rcamnt = 40; // amount to adjust turning right
       // adjust angle for distance
           int fang = 25; // angle from 90 degrees to use for front readings
           int tempus = read_ultrasonics( "front" , 90 ); // get readings from ultrasonic sensor
           if ( tempus < 25 ) fang = 30; // close work
           if ( tempus > 60 ) fang = 15; // distant work
       String result = ""; // result of check
       int tcnt = 0; // temp counter
       while ( result != "straight" && tcnt < 5 ) { // adjust several times if required
           tcnt = tcnt +1;   // increment timout counter
           if ( tcnt > 1 ) delay (200); // wait for robot to stop turning
           // read the two measurements
           int usfr = read_ultrasonics( "front" , 90-fang ); // get readings from ultrasonic sensor
           delay(20);
           int usfl = read_ultrasonics( "front" , 90+fang ); // get readings from ultrasonic sensor
           usfl = usfl + corectn;   // adjust for correction
           if ( usfr == 0 ) usfr = 999; // if zero returned make it very large instead
           if ( usfl == 0 ) usfl = 999; // if zero returned make it very large instead
           result = "straight"; // result of check
    
           int amount = 0; //amount out of square
    
           if ( usfr > usfl) {
             // robot pointing right
             result = "right";
             amount = usfr - usfl;
           }
    
           if ( usfl > usfr) {
             // robot pointing left
             result = "left";
             amount = usfl - usfr;
           }
         
  
          if ( amount > 3 ) amount = 3; // Make sure amount isn't too great
             
          lcdshow( "correct_u front" , "l=" + String(usfl) + " r=" + String(usfr) );
          delay(200);
        
          // make adjustment
             if ( result == "right" ) move_robot( 1 , "left" , ( lcamnt * amount ) , "" );
             if ( result == "left" ) move_robot( 1 , "right" , ( rcamnt * amount ) , "" );
     }   // end of while
       
   }     // end of proc
//     ------------------------------  
   
  if ( sensor == "left" ) {
       // measure side with 2 sensors
       int lcamnt = 40; // amount to adjust turning left
       int rcamnt = 40; // amount to adjust turning right
       String result = ""; // result of check
       int tcnt = 0; // temp counter
       
       while ( result != "straight" && tcnt < 5 ) { // adjust several times if required
           tcnt = tcnt +1;   // increment timout counter
           if ( tcnt > 1 ) delay (200); // wait for robot to stop turning
           // read the two measurements
           int usff = read_ultrasonics( "front" , 180 ); // get readings from ultrasonic sensor
           delay(20);
           int usfr = read_ultrasonics( "left" , 0 ); // get readings from ultrasonic sensor
           usff = usff + corectn;   // adjust for correction
           if ( usff == 0 ) usff = 999; // if zero returned make it very large instead
           if ( usfr == 0 ) usfr = 999; // if zero returned make it very large instead
           result = "straight"; // result of check
    
           int amount = 0; //amount out of square
           usfr = usfr + 19;   // adjust for different locations
    
           if ( usff > usfr) {
             // robot pointing right
             result = "right";
             amount = usff - usfr;
           }
    
           if ( usfr > usff) {
             // robot pointing left
             result = "left";
             amount = usfr - usff;
           }
    
        if ( amount > 3 ) amount = 3; // Make sure amount isn't too great
           
        lcdshow( "correct_u left" , "f=" + String(usff) + " r=" + String(usfr) );
        delay(200);
      
        // make adjustment
           if ( result == "right" ) move_robot( 1 , "left" , ( lcamnt * amount ) , "" );
           if ( result == "left" ) move_robot( 1 , "right" , ( rcamnt * amount ) , "" );
    
       }   // end of while
    }     // end of left
//     ------------------------------  
  if ( sensor == "right" ) {
       // measure side with 2 sensors
       int lcamnt = 40; // amount to adjust turning left
       int rcamnt = 40; // amount to adjust turning right
       String result = ""; // result of check
       int tcnt = 0; // temp counter
       while ( result != "straight" && tcnt < 5 ) { // adjust several times if required
           tcnt = tcnt +1;   // increment timout counter
           if ( tcnt > 1 ) delay (200); // wait for robot to stop turning
           // read the two measurements
           int camnt = 100; // amount to adjust per cm out
           int usff = read_ultrasonics( "front" , 5 ); // get readings from ultrasonic sensor
           delay(20);
           int usfr = read_ultrasonics( "right" , 0 ); // get readings from ultrasonic sensor
           usff = usff + corectn;   // adjust for correction
           if ( usff == 0 ) usff = 999; // if zero returned make it very large instead
           if ( usfr == 0 ) usfr = 999; // if zero returned make it very large instead
           result = "straight"; // result of check
    
           int amount = 0; //amount out of square
           usfr = usfr + 16;   // adjust for different locations
    
           if ( usfr > usff) {
             // robot pointing right
             result = "right";
             amount = usfr - usff;
           }
    
           if ( usff > usfr) {
             // robot pointing left
             result = "left";
             amount = usff - usfr;
           }
    
        if ( amount > 3 ) amount = 3; // Make sure amount isn't too great
           
        lcdshow( "correct_u right" , "f=" + String(usff) + " r=" + String(usfr) );
        delay(200);
      
        // make adjustment
           if ( result == "right" ) move_robot( 1 , "left" , ( lcamnt * amount ) , "" );
           if ( result == "left" ) move_robot( 1 , "right" , ( rcamnt * amount ) , "" );
      
       }  //while
      
    }  //if-right
  
}  //correct_u
//  ---------------------------------------------------------
// debounce cup on platform sensor
// returns either on or off
String readcup() {
  String res = "off"; // default result
   // see if cup is on platform
   if ( digitalRead(cup) == LOW ) {
        // cup on platform so debounce to make sure
        delay(20);
        if ( digitalRead(cup) == LOW ) res = "on";
   }
   return res;
}
//  ---------------------------------------------------------
// display error message on lcd and stop
// input is 3 strings - title for line 1 plus 2 strings to be displayed on line 2
void error( String title , String linea , String lineb ) {
      robot_error = true; // flag error state
      stop_robot();           // stop
      lcdshow( title , "" );
      warning_sound (3);
      while ( 1 == 1 ) { // stop
         // display info on line 2
        lcdshow( "" , linea );
        delay (800);
        lcdshow( "" , lineb );
        delay (800);
      }
  
}
//  ---------------------------------------------------------
//  scan left or right with front ultrasound sensor to find an object or gap (scan0)
//     inputs:    direction to scan  (left or right) , gap or object , min distance to find , scan steps
int scan ( String direct , String goro , int dist , int sstep) {
  
    int spos = 50; // starting position
    if ( direct == "right" ) {
        lcdshow ( "scanning right" , " " );
        spos = 130;
        sstep = -sstep;
    }
    if ( direct = "left" ) lcdshow ( "scanning left" , " " );
 
    // scan until distance less than "dist"
    int qtemp=999; // temp store of reading
    int tflag = 0; // flag
    while ( spos <= 130 && spos >= 50 && tflag == 0 ) {
      myservo.write(spos);
      delay(400); // wait for sero to move
      qtemp = ultrasonic_front.convert(ultrasonic_front.timing(), Ultrasonic::CM);   
      if ( goro == "gap" and qtemp > dist ) tflag = 1; // found
      if ( goro == "object" and qtemp < dist ) tflag = 1; // found
      spos = spos + sstep;   // increment position
    }   // end of while
    myservo.write(90); //return ultrasound sensor to front
    // return result
      spos = spos - sstep;   
      if ( spos < 50 ) spos = 50;
      if ( spos > 130 ) spos = 130;
      lcdshow ( "" , "result=" + String(spos) );
      return spos; // return position
  
}  // end of scan
//  ---------------------------------------------------------
//  scan left or right with front ultrasound sensor to find edge of a gap  (scan1)
//     inputs:    direction to scan  (left or right) , min distance to find , scan steps
int cscan ( String direct , int dist , int sstep) {
  
    int spos = 90; // starting position
    if ( direct == "right" ) lcdshow ( "Gscanning right" , " " );
    if ( direct = "left" ) lcdshow ( "Gscanning left" , " " );
 
    // scan until distance less than "dist"
    int qtemp=999; // temp store of reading
    while ( spos <= 180 && spos >= 0 && qtemp > dist ) {
      myservo.write(spos);
      delay(400); // wait for sero to move
      qtemp = ultrasonic_front.convert(ultrasonic_front.timing(), Ultrasonic::CM);   
      if ( direct == "right" ) spos = spos - sstep; // increment position
      if ( direct == "left" ) spos = spos + sstep; // increment position
    }   // end of while
    myservo.write(90); //return ultrasound sensor to front
    // return result
      spos = spos - sstep;   
      if ( spos < 0 ) spos = 0;
      if ( spos > 180 ) spos = 180;
      lcdshow ( "" , "result=" + String(spos) );
      return spos; // return position
  
}  // end of scan
//  ---------------------------------------------------------
//  Check distance left or right is correct after a turn and adjust if not (tadjust1)
//  inputs = direction , expected distance )
void tadjust ( String direct , int dist ) {
    lcdshow ( "adjust dist" , String (dist) ) ;
    for ( int i = 0 ; i < 4 ; i++) { // check 3 times
        delay(300); // wait for robot to stop moving
        int tempus = read_ultrasonics( direct , 90 ); // get readings from ultrasonic sensor
        if ( tempus < dist && direct == "left" ) move_robot( 7 , "left" , 20 , "" ); // kick left
        if ( tempus > dist && direct == "left" ) move_robot( 7 , "right" , 20 , "" ); // kick right
        if ( tempus < dist && direct == "right" ) move_robot( 7 , "right" , 20 , "" ); // kick left
        if ( tempus > dist && direct == "right" ) move_robot( 7 , "left" , 20 , "" ); // kick right
        if ( tempus < dist && direct == "front" ) move_robot( 7 , "reverse" , 20 , "" ); // kick back
        if ( tempus > dist && direct == "front" ) move_robot( 7 , "forward" , 20 , "" ); // kick forward
      }
}   // end of tadjust
//  ---------------------------------------------------------
// move from table to Library door (TTL)
void move_TableToLib() {
    int tempus = 0; // store temp ultrasound readings
  
    // make sure location is correct
    if ( location != "table" ) {
        // not at table
        error( "ERROR (ttld)" , "not at table" , " " );
    }
    // check the robot is in the correct starting position
    tempus = read_ultrasonics( "front" , 90 ); // get readings from ultrasonic sensor
    if ( tempus > 14 ) error( "ERROR (tab2vend)" , "not at table" , "front=" + String(tempus) + "cm" );
  
    lcdshow( "About to move!" , "leaving table" );
    warning_sound(2);            // sound warning
    digitalWrite(ledr, HIGH); // warning led on
    
    // from coffee table to vending machine
      move_robot( 7 , "reverse" , mcm * 30 , "" ); // reverse away from table
      correct_u( "front" , 0 ); // make sure parallel to coffee table
      move_robot( 7 , "right" , mturn / 2 , "" ); // about turn
      move_robot( 7 , "forward" , mcm * 50 , "left" ); // forward towards wall
      correct_u( "front" , 0 ); // make sure square on to wall
      move_robot( 2 , "forward" , 30 , "front" ); // move to wall
      move_robot( 7 , "right" , mturn / 4 , "" );
      move_robot( 6 , "forward" , mcm * 75 , "left" ); // follow wall out of parlour
    // out of parlour and down corridor
      warning_sound(2);            // sound warning
      move_robot( 2 , "forward" , 35 , "front" ); // out of parlour door
      move_robot( 7 , "left" , (mturn / 4) - 300 , "" );
      correct_u( "right" , 0 ); // make sure parallel to wall
      move_robot( 7 , "forward" , mcm * 20 , "" );       move_robot( 5 , "forward" , 30 , "left" ); // forward until left is less than 30 (get fix at door frame)
      // check door frame is expcted distance
          tempus = read_ultrasonics( "left" , 0 ); // get readings from ultrasonic sensor
          if ( tempus <= 7 ) move_robot( 1 , "right" , 30 , "" );
      move_robot( 7 , "forward" , mcm * 40 , "" ); // down corridor
      correct_u( "right" , -1 ); // make sure parallel to wall
      move_robot( 7 , "left" , mturn / 4 , "" ); // left to face library
     // into Library
      move_robot( 5 , "forward" , 35 , "left" ); // get fix at door frame
      move_robot( 7 , "forward" , mcm * 5 , "" );       correct_u( "left" , -1 ); // make sure parallel to wall
      // If too close to the wall give a kick to the right
          tempus = read_ultrasonics( "left" , 0 ); // get readings from ultrasonic sensor
          if ( tempus < 17 ) move_robot( 1 , "right" , 210 , "" );
      move_robot( 7 , "forward" , mcm * 35 , "" ); //into library
      // If too close to the wall give a kick to the right
          tempus = read_ultrasonics( "left" , 0 ); // get readings from ultrasonic sensor
          if ( tempus < 24 ) move_robot( 1 , "right" , 130 , "" );
          if ( tempus < 14 ) move_robot( 1 , "right" , 130 , "" );
      move_robot( 2 , "forward" , 55 , "front" ); // up to side of vending machine
      correct_u( "left" , 0 ); // make sure parallel to wall
      move_robot( 2 , "forward" , 45 , "front" ); // up to side of vending machine
      
     // finished
     digitalWrite(ledr, LOW); // warning led off
}
//  ---------------------------------------------------------
// move from Library to vending machine (ltv)
void move_LibToVend() {
    int tempus = 0; // store temp ultrasound readings
  
    lcdshow( "About to move!" , "Into Lib." );
    digitalWrite(ledr, HIGH); // warning led on
    
      // turn amount depending on distance from wall
        tempus = read_ultrasonics( "left" , 0 ); // get readings from ultrasonic sensor
        int tmes = ( mturn / 5.6 ); // amount to turn
        if ( tempus < 23 ) tmes = tmes + 600; // turn a bit more if close to wall
        else if ( tempus > 35 ) tmes = tmes - 600; // turn a bit less if far from wall
        move_robot( 7 , "right" , tmes , "" );       move_robot( 7 , "forward" , mcm * ( 105 - tempus ) , "" ); // adjust for distance from back wall
      move_robot( 7 , "left" , tmes , "" ); // turn back to parallel to back wall
      move_robot( 7 , "forward" , mcm * 20 , "" );       move_robot( 2 , "forward" , 140 , "front" ); // in front of vending machine
      // make sure correct distance from wall
        int qdis = 121; // distance to find
        int qtime = 0; // timeout
        tempus = read_ultrasonics( "front" , 90 ); // get readings from ultrasonic sensor
        while ( ( tempus < (qdis - 1) || tempus > (qdis) ) && qtime < 30 ) {
           qtime = qtime + 1;    // increment timeout counter
           if ( tempus > qdis ) move_robot( 7 , "forward" , -10 , "" );
           if ( tempus < qdis ) move_robot( 7 , "reverse" , -10 , "" );
           delay(300);
           tempus = read_ultrasonics( "front" , 90 ); // get readings from ultrasonic sensor
        } 
        // check for failure
        if ( qtime >= 30 ) {
           error( "ERROR (find)" , "timed out" , "vender find" );
        }
      move_robot( 7 , "left" , (mturn / 4) - 500 , "" );       delay (500); // wait for robot to stop before taking readings
      correct_u( "front" , 0 ); // make sure square on to vending machine
      myservo.write(90); // make sure front servo pointing forward to avoid hitting machine
      move_robot( 2 , "forward" , -4 , "front" ); // move forward into vending machine
      move_robot( 1 , "forward" , -500 , "" ); // into vending machine
     location = "vender"; // flag new location
      
     // finished
     digitalWrite(ledr, LOW); // warning led off
}   // end of proc
//  ---------------------------------------------------------
// move from vending machine to library door (vtld)
void move_VendToLib() {
    int tempus = 0; // store temp ultrasound readings
  
    // check the robot is in the correct starting position
    tempus = read_ultrasonics( "front" , 90 ); // get readings from ultrasonic sensor
    if ( tempus > 10 ) error( "ERROR (vend2tab)" , "not at vender" , "front=" + String(tempus) + "cm" );
  
    // make sure location is correct
    if ( location != "vender" ) {
        // not at table
        error( "ERROR (vtld)" , "not at vender" , " " );
    }
    lcdshow( "About to move!" , "leaving vender" );
    warning_sound(2);            // sound warning
    digitalWrite(ledr, HIGH); // warning led on
    
    // from vending machine out of library
      move_robot( 3 , "reverse" , -20 , "front" );
      correct_u( "front" , 0 ); // make sure square square to vending machine
      move_robot( 3 , "reverse" , -57 , "front" );
      move_robot( 7 , "left" , mturn / 5 , "" );
      move_robot( 2 , "left" , 58 , "right" ); // turn away from vending machine
      move_robot( 7 , "forward" , mcm * 20 , "" );
      move_robot( 2 , "forward" , 68 , "front" );
      move_robot( 7 , "right" , mturn / 5 , "" );
      // check position of gap
        int qtemp = 71; // distance wanted
        tempus = scan ( "left" , "gap" , qtemp , 3 );         if ( tempus > qtemp ) move_robot( 7 , "left" , 60 , "" );
        if ( tempus < qtemp ) move_robot( 7 , "right" , 60 , "" );
        delay(300);
        if ( tempus > ( qtemp + 5 ) ) move_robot( 7 , "left" , 60 , "" );
        if ( tempus < ( qtemp - 6 ) ) move_robot( 7 , "right" , 60 , "" );
      move_robot( 7 , "forward" , mcm * 70 , "" ); // go between door and vending machine
      move_robot( 2 , "forward" , 35 , "front" );       move_robot( 7 , "left" , mturn / 5.3 , "" );
      move_robot( 7 , "forward" , mcm * 5 , "" ); // move farward slightly to get better ultrasound signal
    // finished
    digitalWrite(ledr, LOW); // warning led off
}    // end of proc
//  ---------------------------------------------------------
// move from library to table (ltt)
void move_LibToTable() {
    int tempus = 0; // store temp ultrasound readings
    lcdshow( "About to move!" , "leaving Library" );
    digitalWrite(ledr, HIGH); // warning led on
    
    // out of library, down corridor and into parlour
      correct_u( "right" , 0 ); // make sure parallel to wall
      move_robot( 6 , "forward" , mcm * 32 , "right" ); // follow wall out of library
      warning_sound(2);            // sound warning
      move_robot( 2 , "forward" , 50 , "front" );       move_robot( 7 , "right" , mturn / 4 , "" );
      correct_u( "left" , 0 ); // make sure parallel to wall
      move_robot( 7 , "forward" , mcm * 10 , "" ); // move forward a bit to avoid rear door frame
      move_robot( 5 , "forward" , 35 , "right" ); // get fix at door frame
      correct_u( "left" , 12 ); // make sure parallel to wall
      // If too close to the wall give a kick to the right
          tempus = read_ultrasonics( "left" , 0 ); // get readings from ultrasonic sensor
          if ( tempus < 26 ) move_robot( 7 , "right" , 110 , "" );
      move_robot( 7 , "forward" , mcm * 22 , "" );       // move 20cm and check going parallel to wall as not enough wall to follow
          tempus = read_ultrasonics( "left" , 0 ); // get readings from ultrasonic sensor
          move_robot( 7 , "forward" , mcm * 20 , "" );
          correct_e( (tempus) , "left" , 30 ); // adjust for any change
      move_robot( 7 , "right" , (mturn / 4) , "" ); // facing into parlour
    // into parlour and upto coffee table
      move_robot( 7 , "forward" , mcm * -65 , "" ); // move through doorway
      correct_u( "right" , 0 ); // make sure parallel to wall
      // check wall is expected distance, if not give kick to the left
          tempus = read_ultrasonics( "right" , 0 ); // get readings from ultrasonic sensor
          if ( tempus <= 10 ) move_robot( 1 , "left" , 30 , "" );
      move_robot( 4 , "forward" , -40 , "left" ); // get past speaker
      // move away from wall now in open space
        move_robot( 7 , "left" , (mturn / 12) , "" );         move_robot( 7 , "forward" , mcm * 32 , "" );
        move_robot( 7 , "right" , (mturn / 12) , "" );       move_robot( 6 , "forward" , mcm * 29 , "right" ); // follow parlour wall
      correct_u( "right" , 0 ); // make sure parallel to wall
      move_robot( 7 , "left" , (mturn / 4) - 500 , "" );
      move_robot( 2 , "forward" , 70 , "front" );       correct_u( "front" , 0 ); // make sure parallel to box
      move_robot( 2 , "forward" , 12 , "front" );     location = "table"; // flag new location
    underlights ( 1 , 8 );   // flash under body lights
    // finished
    digitalWrite(ledr, LOW); // warning led off
}   // end of proc
//  ---------------------------------------------------------
// move from library door to parking space
void move_LibDoorToPark() {
    int tempus = 0; // store temp ultrasound readings
    lcdshow( "About to move!" , "parking" );
    digitalWrite(ledr, HIGH); // warning led on
    
      // Into Library
      move_robot( 2 , "forward" , 14 , "front" ); // close to vender side
      move_robot( 1 , "forward" , -400 , "front" ); // park
    location = "parked"; // flag new location
    // finished
    digitalWrite(ledr, LOW); // warning led off
}   // end of proc
//  ---------------------------------------------------------
//  collect a drink
void collectDrink() {
  
      radiobutton = 0;   // clear radio button pressed flag
 
      lcdshow( "Off to collect" , "a drink...." );
      // move from Parlour table to vending machine
        move_TableToLib();       // move from table to Library Door
        move_LibToVend();       // move from Library Door to Vender
      // wait for drink to dispense (24 seconds)
        lcdshow( "Waiting for" , "drink...." );
        for ( int i = 0 ; i < 47 ; i++) {
          digitalWrite(ledg, !digitalRead(ledg)); // flash green led
          delay(500);
        }
        digitalWrite(ledg,LOW); // make sure led is off
        lcdshow( " " , " " );
      // Move from vending machine to Parlour table
        move_VendToLib();       // move from vending machine to Library Door
        move_LibToTable();      // move from Library Door to table
      // finished
        digitalWrite(ledr, LOW); // warning led off
        warning_sound(2);
        lcdshow( "Finished" , " " );
}   // end of proc
//  ---------------------------------------------------------
// Move from parked to Parlour Table
void move_ParkToTable() {
      radiobutton = 0;   // clear radio button pressed flag
 
      lcdshow( "Off to Parlour" , "a drink !!!" );
      warning_sound(2);            // sound warning
      // get to correct starting position
      move_robot( 7 , "reverse" , mcm * 30 , "" ); // reverse away from table
      correct_u( "front" , 0 ); // make sure parallel to coffee table
      move_robot( 7 , "right" , mturn / 2 , "" ); // about turn
      move_LibToTable();         
}  // end of proc
//  ---------------------------------------------------------
// sequence on the under body lights
// input = mode to use , duration 
//            1 = kitt,  2 = random 
void underlights ( int sel , int dur ) {
  // Kitt lights
  if ( sel == 1 ) {
    int stp = 30; // delay
    for ( int j = 0 ; j < dur ; j++ ) {
       for ( int i = 0 ; i < 6 ; i++) {
         digitalWrite(uleds[i],HIGH);          delay(stp);
         digitalWrite(uleds[i],LOW);        }   // end for i
       for ( int i = 5 ; i > 0 ; i=i-1) {
         digitalWrite(uleds[i],HIGH);          delay(stp);
         digitalWrite(uleds[i],LOW);        }   // for i
    }   // for j
  }  // if
  // random lights
  if ( sel == 2 ) {
    int stp = 30; // delay
    for ( int i = 0 ; i < dur ; i++ ) {
      for ( int j = 0 ; j < 6 ; j++) digitalWrite(uleds[j],LOW);       digitalWrite(uleds[random(6)],HIGH);       delay(stp);
    }  // for i
  }  // if
  // all off
  for ( int i = 0 ; i < 6 ; i++) digitalWrite(uleds[i],LOW);   
  
}   // end of proc
//  ---------------------------------------------------------
//  the end