| |
//********************************************************************
/*
Description: TR1 Rover controlled by the Brainstem™ microcontroller.
The Rover will move forward until a barrier is detected.
It will then decide on its safest route and continue its progress.
Created:03/10/02 Revision 1.00
Written by:Total Robots (Jamie Finnan)
TR-010
Brainstem is a registered trademark of Acroname Inc.
*/
//********************************************************************
#include <aCore.tea> //Set up the tea files for use in the program.
#include <aPrint.tea>
#include <aServo.tea>
#define aSRF04_INIT 4 //Define Init and Echo before include.
#define aSRF04_ECHO 3
#include <aSRF04.tea>
//********************************************************************
void main()
{
aSRF04_Setup(); //Setup SRF04
setup(); //Setup Servos
stop(); //Stop Servos
aCore_Sleep(50000); //5 second set up delay.
while(1){
forward(); //Move rover forward
Check_Right(); //Check if right front is clear.
Check_Left(); //Check if left front is clear.
}
}
//********************************************************************
/* This section sets up the three HS-300 servos for use in the rover.
Servo 0
and 1 have different configurations. This can be changed to suit.*/
void setup()
{
aServo_SetConfig(2, (unsigned char)(SRV_ENA | 0));
aServo_SetLimits(2, 0x0046);
aServo_SetAbsolute(2, (unsigned char)-1);
aServo_SetAbsolute(0, (unsigned char)105);
aServo_SetConfig(0, (unsigned char)((SRV_ENA | SRV_INV) + 0));
aServo_SetAbsolute(1, (unsigned char)149);
aServo_SetConfig(1, (unsigned char)(SRV_ENA | 0));
aServo_SetLimits(0, 0x0046);
aServo_SetLimits(1, 0x0046);
aCore_Sleep(10000);
}
//********************************************************************
/* This routine checks the right side of the front sweep for obstacles.
It increments across its range by 20 each time. If an obstacle is found
an evade state occurs.*/
void Check_Right()
{
int reading ;
char incrr = -30;
aServo_SetAbsolute(2, (char)incrr);
aCore_Sleep(2000);
while (2){
reading = aSRF04_ReadInt();
incrr= incrr-20;
aServo_SetAbsolute(2, (char)incrr);
aCore_Sleep(2000);
if (reading < 1000) //If SRF04 < 1000 goto evade.
{
Evade();
stop();
break;
}
if (incrr < -120) break;//End of right sweep.
}
}
//********************************************************************
/* This routine checks the left side of the front sweep for obstacles.
It increments across its range by 20 each time. If an obstacle is found
an evade state occurs.*/
void Check_Left()
{
int reading ;
char incr = 127;
int num = 0;
while (3){
reading = aSRF04_ReadInt();
incr= incr -20;
aServo_SetAbsolute(2, (unsigned char)incr);
aCore_Sleep(2000);
if (reading < 1000) //If SRF04 < 1000 goto evade.
{
Evade();
stop();
break;
}
if (incr < 90) break;//End of left sweep.
}
}
//********************************************************************
/*This routine moves the right and left servos forward. The values can
be altered for different speeds and directions.*/void forward()
{
aServo_SetAbsolute(0, (unsigned char)0);
aServo_SetAbsolute(1, (unsigned char)45);
}
//********************************************************************
/*This routine moves the right and left servos backward. The values
can
be altered for different speeds and directions.*/
void backward()
{
aServo_SetAbsolute(0, (unsigned char)155);
aServo_SetAbsolute(1, (unsigned char)200);
aCore_Sleep(15000);
stop();
}
//********************************************************************
/* This routine stops the right and left servos. The value may be
different on different servos and should be tested.*/
void stop()
{
aServo_SetAbsolute(0, (unsigned char)104);
aServo_SetAbsolute(1, (unsigned char)149);
}
//********************************************************************
/*This is the main evade routine. If an obstacle has been detected,
the rover moves backward and decides whether to go left, right or
backward depending on further obstacles.*/
void Evade()
{
char ack = 0;
int right_scan;
int left_scan;
while(4){
stop();
backward();
aServo_SetAbsolute(2, (char)-10);
aCore_Sleep(10000);
right_scan = aSRF04_ReadInt();
if (right_scan >1500) //check if right is clear.If yes move right.
{
aServo_SetAbsolute(0, (unsigned char)155);
aServo_SetAbsolute(1, (unsigned char)45);
aCore_Sleep(10000);
ack = 1;
stop();
break;
}
aServo_SetAbsolute(2, (char)30);
aCore_Sleep(10000);
left_scan = aSRF04_ReadInt();
if (left_scan >1500) //check if left is clear. If yes move left.
{
aServo_SetAbsolute(0, (unsigned char)0);
aServo_SetAbsolute(1, (unsigned char)200);
aCore_Sleep(10000);
ack = 1;
stop();
break;
}
} //If both ways are obstructed, loop until clear.
}
//End of program
//********************************************************************
//********************************************************************
|
