Welcome to my personal webpage. | home
NavigationCode
ED & G 100
Penn State Abington
Spring 2001
Professor R. Avanzato
Robot : Z1380
Programming team :
Antonio Marzullo
John Pawlowski
Nima Moshtagh
CODE:
#define LIGHT 0 /* port 0 for candle light sensor */
#define RM 1 /* port 1 for right motor */
#define FAN 2 /* port 2 for fan */
#define LM 3 /* port 3 for left motor */
#define LINE 4 /* port 4 for line sensor */
#define RENCODER 1 /* port 7 for right encoders */
#define LENCODER 0 /* port 8 for left encoders */
#define ON 1;
#define OFF 0;
int candle = OFF;
/*------------------------------------------------------------------------------*/
int initialize()
{
servo_on(); /* TURN ON THE SERVO MOTOR,LEFT AND RIGHT ENCODERS */
enable_encoder(LENCODER); /* TURN ON THE ENCODERS */
enable_encoder(RENCODER);
reset_encoder(LENCODER); /* RESET BOTH ENCODERS TO ZERO(0) */
reset_encoder(RENCODER);
}
/*-----------------------------------------------------------------------------*/
/* TURN OFF BOTH RIGHT AND LEFT MOTORS */
void stop()
{
off(LM);
off(RM);
}
/*----------------------------------------------------------------------------*/
/* DIRECT THE ROBOT OUT OF THE ROOMS ,WORKS WITH THE CANDLE STATUS AND ROOM NUMBER */
void goout(int room)
{
straight(80.0,-100); /* IT BACKS UP TO HIT THE WALL AND MAKES SURE THE ROBOT IS STRAIGHT */
straight(15.0,100);
if(candle == ON) /* IF YOU FOUND THE CANDLE */
{
if(room == 1 || room == 4)
turn (-90); /* AND YOU WERE IN ROOM 1 OR TOOM 4,THEN TURN LEFT */
}
else
turn (90); /* IN ALL OTHER CASES TURN RIGHT */
}
/*-----------------------------------------------------------------------------*/
/* RUN THE FAN IN 3 CLOSE DIRECTION TO PUT OUT THE CANDLE */
void extinguish()
{
motor(FAN,100);
sleep(2.0);
servo_deg(80.0); /* TRY 10 DEGREE TO THE LEFT */
sleep(2.0);
servo_deg(100.0); /* TRY 10 DEGREE TO THE RIGHT */
sleep(2.0);
off(FAN); /* TURN OFF THE FAN */
}
/*-----------------------------------------------------------------------------*/
/* BY HAVING THE LENGHT AND THE POWER OF MOTORS AS TWO PRAMETERS RUNS THE ROBOT STRAIGHT.
IN THIS METHOD WE COMPARE THE READINGS OF TWO ENCODERS.
IF THE LEFT MOTOR IS GOING FASTER, THE READING OF THE LEFT ENCODER WOULD BE
GREATER THAN THAT OF THE RIGHT ENCODER.THEN WE SLOW THAN THE LEFT MOTOR
SO THAT THE RIGHT ONE CATCHES UP THE LEFT ONE.AND VISE VERSA FOR THE OTHER CASE.*/
void straight(float length,int power)
{
float num = length*12.0/(4.191*3.14); /* CONVERT THE LENGHT TO THE NUMBER OF HOLES REQUIREDB */
int r,l;
reset_encoder(RENCODER); /* SET THE ENCODERS TO ZERO(0) */
reset_encoder(LENCODER);
while (1)
{
r=read_encoder(RENCODER);
l=read_encoder(LENCODER);
if(power > 0) /* IF THE POWER WAS POSITIVE GO FORWARD */
{
if(r > l) /* IF RIGHT GOES FASTER */
{
motor(RM,power - 50); /* SLOW DOWN THE RIGHT MOTOR */
motor(LM,power);
}
else /* IF LEFT MOTOR GOES FASTER */
{
motor(LM,power - 50); /* THEN SLOW DOWN THE LEFT MOTOR */
motor(RM,power);
}
}
else /* IF THE POWER WAS NEGATINE GO BACKWARD */
{
if(r > l) /* AND IF RIGHT GOES FASTER, */
{
motor(RM,power + 50); /* THEN SLOW DOWN THE RIGHT MOTOR */
motor(LM,power);
}
else /* IF LEFT MOTOR GOES FASTER */
{
motor(LM,power + 50); /* THEN SLOW DOWN THE LEFT MOTOR */
motor(RM,power);
}
}
/* IF THE RIGHT ENCODER READING WAS GREATER THAN NUMBER OF HOLES, FINISH IT !! */
if(r > (int)num)
break;
}
/*-----------------------------------------------------------------------------*/
/* THIS FUNCTION TURNS THE ROBOT BY THE ANGLE PASSED TO FUNCTION.
POSITIVE ANGLE MEANS RIGHT TURN AND NEGATIVE ANGLE MEANS LEFT TURN. */
void turn(int deg)
{
int num;
enable_encoder(RENCODER);
reset_encoder(RENCODER);
if(deg < 0) /* TURN LEFT */
{
deg = deg*(-1);
motor(LM,-100);
motor(RM,100);
num = (int)8*deg/90; /* CONVER THE ANFLE TO THE NUMBER OF HOLES FOR ENCODER */
}
else{ /* TURN RIGHT */
motor(LM,100);
motor(RM,-100);
num = (int)8*deg/90; /* CONVERT THE ANGLE OF ROTATION TO THE NUMBER OF HOLES FOR ENCODER */
}
/* TURN UNTIL THE READING OF THE RIGHT ENCODER EQUAL THE NUMBER OF HOLES REQUIRED */
while(read_encoder(RENCODER) < num)
{}
}
/*---------------------------------------------------------------------------*/
/* THIS FUNCTION RUNS THE ROBOT TOWARD THE CANDLE AND WILL STOP IT BY THE WHITE
LINE AND IT RETURNS THE DISTANCE BETWEEN THE WHITE LINE AND ENTERANCE OF THE ROOM */
float findline()
{
int num,r,l;
float length;
reset_encoder(RENCODER);
reset_encoder(LENCODER);
/* THIS WHILE() LOOP KEEPS THE ROBOT GO STRAIGHT
(SAME ALGORITHEM IS USED IN THE STRAIGHT() FUNCTION ).*/
while (1)
{
r=read_encoder(RENCODER); /* CHECK THE VALUES OF THE LEFT AND RIGHT ENCODERS */
l=read_encoder(LENCODER);
if(r>l)
{
motor (RM, 50);
motor (LM, 100);
}
else
{
motor (LM, 50);
motor(RM, 100);
}
if(analog (LINE) < 230) /* IF YOU SAW A WHITE LINE, STOP */
break;
}
stop();
num = read_encoder(LENCODER); /* READ THE LEFT ENCODER */
length = (float)num*3.14*4.191/12.0; /* CONVERT THE READING OF ENCODER TO LENGHT */
return length;
}
/*----------------------------------------------------------------------------*/
/*AFTER FINDING THE CANDLE IN A ROOM, THE ROBOT SHOULD TURN AND FIND THE CANDLE AGAIN
BECAUSE SERVO IS NOT ACURATE ENOUGH TO BE RELIABLE */
void gotowards(int room)
{
/* TURN AROUND TO FIND THE CANDLE AGAIN */
while(analog(LIGHT) > 30) /* AS LONG AS LIGHT IS DARKER THAN 30, KEEP ON TURNING */
{
motor(RM,-50);
motor(LM,50);
}
}
/*----------------------------------------------------------------------------*/
/* THIS FUNCTION RUNS THE SERVO MOTOR TO LOOK FOR THE CANDLE
AND IT RETURNS THE AMOUNT OF DEGREE THAT THE CANDLE STANDS FROM THE FRONT OF THE ROBOT */
float searchcandle(int room)
{
float limit;
float deg;
if(room == 1) /* BECAUSE THE RANGE OF SWEEPING FOR THE SERVO IS DIFFERENT FOR ROOM 1 */
{
deg = 25.0; /* THR ANGLE OF 25.0 DEGREE SETS THE SERVO AT ZERO(0) DEGREE POSITION. */
limit = 150.0;
}
else /* FOR OTHER ROOMS WE DON'T NEED TO SWEEP ANGLES LESS THAN 60
BECAUSE THE CANDLE IS NOT GOING TO BE THERE. */
{
deg = 60.0;
limit = 175.0;
}
stop();
/* INCREASE THE ANGLE BY STEPS OF 3 DEGREE UNTILL THE LIMIT DEGREE IS REACHED */
while(deg <= limit)
{
servo_deg(deg);
sleep(.2);
if(analog(LIGHT) < 50) /* IF YOU SEE A LIGHT BRIGHTER THAN 50 */
{
servo_deg(90.0); /* PUT THE SERVO AND THE FAN IN THE STRAIGHT DIRECTION */
candle = ON;
break; /* AND THEN YOU DON'T NEED TO TURN ANY MORE !! */
}
deg = deg + 3.0; /* INCREASE THE ANGLES BY 3.0 DEGREE AT A TIME */
}
return deg;
}
/*-----------------------------------------------------------------------------*/
/* THIS FUNCTION CONTAINS ALL THE CODES REQUARED TO FIND,FOLLOW AND EXTINGUISH THE CANDLE */
void checkroom(int room)
{
float length;
float degree;
degree = searchcandle(room); /* LOOK FOR CANDLE */
if(candle == ON) /* IF YOU FOUND IT */
{
gotowards(room); /* GET THE ORIENTATION OF THE CANDLE */
length = findline(); /* GO TOARD IT TO FIND THE WHITE LINE */
extinguish(); /* PUT OUT THE CANDLE */
straight(length,-100); /* BACK UP TO THE INITIAL POSITION IN THE ROOM */
/* IF THE CANDLE WAS AT ANGLE LESS THAN 90.0, (90.0- degree) WOULD BE POSITIVE,
AND AT THE END ROBOT SHOULD TURN RIGHT TO GET THE INITIAL ORIENTATION.*/
/*IF THE CANDLE WAS AT ANGLE GREATER THAN 90.0, (90.0-degree) WOULD BE NEGATIVE,
AND AT THE END ROBOT SHOULD TURN LEFT TO GET THE INITIAL ORIENTATION.*/
turn(90-(int)degree);
}
goout(room); /* GET OUT OF THE ROOM */
}
/*---------------------------------------------------------------------------*/
/* START FROM "HOME" AND GO TO "ROOM 1" */
void gotoroom1()
{
straight(105.0,100);
turn(-90);
straight(60.0,100);
turn(-90);
straight(26.0,100);
checkroom(1);
}
/*----------------------------------------------------------------------------*/
/* START FROM "ROOM 1" AND GO TO "ROOM 2" */
void go1to2()
{
straight(46.0,100);
turn(-90);
straight(55.0,100);
turn(90);
straight(79.0,100);
turn(90);
straight(30.0,100);
checkroom(2);
}
/*----------------------------------------------------------------------------*/
/* START FROM "ROOM 2" AND GO TO "ROOM 3 " */
void go2to3()
{
straight(79.0,100);
turn(90);
straight(153.0,100);
turn(90);
straight(23.0,100);
checkroom(3);
}
/*-----------------------------------------------------------------------------*/
/* START FROM "ROOM 3" AND GO TO "ROOM 4" */
void go3to4()
{
straight(46.0,100);
turn(90);
straight(23.0,100);
checkroom(4);
}
/*-----------------------------------------------------------------------------*/
/* START FROM "ROOM 1" AND GO TO "HOME" */
void go1tohome()
{
straight(65.0,100);
turn(90);
straight(100.0,100);
stop();
}
/*-----------------------------------------------------------------------------*/
/* START FROM "ROOM 2" AND GO TO "HOME" */
void go2tohome()
{
straight(79.0,100);
turn(90);
straight(46.0,100);
turn(-90);
straight(133.0,100);
stop();
}
/*-----------------------------------------------------------------------------*/
/* START FROM "ROOM 3" AND GO TO "HOME" */
void go3tohome()
{
straight(90.0,100);
turn(70);
straight(133.0,100);
stop();
}
/*-----------------------------------------------------------------------------*/
/* START FROM "ROOM 4" AND GO TO "HOME" */
void go4tohome()
{
straight(46.0,100);
turn(90);
straight(133.0,100);
stop();
}
/*-----------------------------------------------------------------------------*/
void main()
{
/* WAIT TILL SOMEBODY THE PRESS THE START BOTTOM */
start_press(); /* INITIALIZE THE ROBOT, TURN ON THE SERVO, AND ENABLE THE ENCODERS */
initialize();
gotoroom1(); /* START FROM HOME AND GO TO ROOM 1 */
if(candle == ON )
go1tohome(); /* IF YOU PUT OUT THE CANDLE, RETURN TO HOME */
else
go1to2(); /* IF THE CANDLE WASN'T IN ROOM 1, GO TO ROOM 2 */
if( candle == ON) /* IF YOU PUT OUT THE CANDLE, RETURN TO HOME */
go2tohome();
else
go2to3(); /* IF THE CANDLE WASN'T IN ROOM 2, GO TO ROOM 3 */
if( candle == ON)
go3tohome(); /* IF YOU PUT OUT THE CANDLE, RETURN TO HOME */
else
go3to4(); /* IF THE CANDLE WASN'T IN ROOM 3, GO TO ROOM 4 */
go4tohome(); /* RETURN TO HOME*/
stop(); /* TURN OFF BOTH MOTORS */
}
/*-----------------------------------------------------------------------------*/
Home
|