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