| |
//********************************************************************
/*
WCMMain.tea
Description: TR2 Rover with manual override using the DS-WHC hand held
remote, 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. If the rover receives the
correct command it goes into manual mode and waits for further instruction.
This program is loaded into file slot 0 on module 2. This is also the
program that is launched at start up.
Created:18/11/02 Revision 2.00
Written by: Total Robots (Jamie Finnan)
Adapted from TR-010
TR-017
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>
#include <aMulti.tea>
#define aSRF04_INIT 4 //Define Init and Echo before include.
#define aSRF04_ECHO 3
#include <aSRF04.tea>
//********************************************************************
void main()
{
aSRF04_Setup(); //Set-up SRF04
setup(); //Setup Servos
stop(); //Stop Servos
aCore_Sleep(50000); //5 second set up delay.
aMulti_Run00(1,3); //Jump to file location 1 (WCM set-up)
while(1){
aMulti_Run00(2,3); //Jump to file location 2 (Read WCM)
forward(); //Move rover forward
aMulti_Run00(2,3); //Jump to file location 2 (Read WCM)
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.
}
}
//********************************************************************
void drive(int a, int b)
{
aServo_SetAbsolute(0,(unsigned char)a);
aServo_SetAbsolute(1,(unsigned char)b);
}
//********************************************************************
/*This routine moves the right and left servos forward. The values can
be altered for
different speeds and directions.*/
void forward()
{
drive(0,45);
}
//********************************************************************
/*This routine moves the right and left servos backward. The values
can
be altered for different speeds and directions.*/
void backward()
{
drive(155,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()
{
drive(105,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.
{
drive(155,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.
{
drive(0,200);
aCore_Sleep(10000);
ack = 1;
stop();
break;
}
} //If both ways are obstructed, loop until clear.}
//End of program
//********************************************************************
//********************************************************************
/********************************************************************
*********************************************************************
WCMRead.tea
Description: This .tea program is loaded into BrainstemTM file slot
2 on module 2.
This program reads the value from the remote DS-WHC or WCM.( In this
application
the command bytes and nodes have been set up to use the DS-WHC).
Created: 18/11/02 Revision: 1.00
Written by: Total Robots (Jamie Finnan)
TR-019
Brainstem is a registered Trademark of Acroname Inc
*********************************************************************
********************************************************************/
#include <aCore.tea>
#include <aPrint.tea> //Write progress onto console.
#include <aMulti.tea> //Used to jump from one file location to
another.
#include <aServo.tea> //Servo control.
#define WCM_ADDR (unsigned char)0xC0 // WCM Address
/********************************************************************
This routine reads the desired register from the local WCM. First a
write is
undertaken then a read from the requested register.
********************************************************************/
int CheckRead(char wcmADDR)
{
int value=0;
asm
{
/* write register address */
pushsb 5 // push address
pushlb 1 // packet will have one byte
pushlb 4 // point to register 4
pushlb 3 // total packet byte count
popcmd // send command via I2C
/* then read two bytes */
pushsb 5 // push address again
pushlb 1 // set read bit
orb
pushlb 2 // will read 2 bytes
popsm aPortIICRead // begin I2C read
popss 2 // store result as int
}
return value; //return with read value
}
/********************************************************************
This section sets up the two HS-300 servos used for direction on the
rover.
Servo 0 and 1 have different configurations. This can be changed to
suit.
********************************************************************/
void setup()
{
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(5000);
}
/********************************************************************
This routine checks the byte command that has been received by the local
WCM.
If 0xFD04 is received (node 253, command data byte 4) the manual routine
is called.
********************************************************************/
void WCM_Check()
{
int status;
while(1)
{
status=CheckRead(WCM_ADDR); //Read the node value and data.
if (status == 0xFD04) //If 0xFD04 is received call manual.
{
Manual(); //Manual routine
}
else break; //exit routine
}
}
/********************************************************************
This routine checks the local WCM's received data byte and compares
it against
its look up table. Certain bytes represent different servo moves. By
changing these
values, different data bytes can be used.
********************************************************************/
void Manual()
{
int status2;
setup(); //setup servos ready for use.
status2=CheckRead(WCM_ADDR); //Read the node value and data.
aPrint_String("Welcome to Manual");
aPrint_IntHex(status2);
aPrint_Char('\n');
while(1)
{
status2=CheckRead(WCM_ADDR); //Look up what value represents.
if (status2 == 0xFD05)
{
forward();
}
if (status2 == 0xFD06)
{
backward();
}
if (status2 == 0xFD07)
{
left();
}
if (status2 == 0xFD08)
{
right();
}
if (status2 == 0xFD09)
{
stop();
}
if (status2 == 0xFD0E)
{
break;
}
aPrint_IntHex(status2);
aPrint_Char('\n');
aCore_Sleep(5000);
}
}
/********************************************************************
Set the servos direction.
********************************************************************/
void drive(int a, int b)
{
aServo_SetAbsolute(0,(unsigned char)a);
aServo_SetAbsolute(1,(unsigned char)b);
}
/********************************************************************
This routine moves the right and left servos forward. The values can
be altered for
different speeds and directions.
********************************************************************/
void forward()
{
drive(0,45);
}
/********************************************************************
This routine moves the right and left servos backward. The values can
be altered for
different speeds and directions.
********************************************************************/
void backward()
{
drive(155,200);
}
/********************************************************************
This routine stops the right and left servos. The value may vary on
different servos
and should be tested.
********************************************************************/
void stop()
{
drive(105,149);
}
/********************************************************************
This routine turns the rover right. The value may vary on different
servos.
********************************************************************/
void right()
{
drive(155,45);
}
/********************************************************************
This routine turns the rover left. The value may vary on different servos.
********************************************************************/
void left()
{
drive(0,200);
}
/********************************************************************
This is the initial routine. When finished, it exits back to the main
program.
********************************************************************/
void main(char callingProcID)
{
aCore_Sleep(1000);
WCM_Check();
aMulti_Halt(callingProcID,100); //Exit from this program.
}
/********************************************************************/
/********************************************************************/
/********************************************************************
*********************************************************************
WCMSetup.tea
Description: This .tea program is loaded into BrainstemTM file slot
1 on module 2. This program sets up the local WCM ready to receive data
from the remote.
Created: 18/11/02 Revision: 1.00
Written by: Total Robots (Jamie Finnan)
TR-018
Brainstem is a registered Trademark of Acroname Inc
*********************************************************************
********************************************************************/
#include <aCore.tea>
#include <aMulti.tea> //Used to jump file locations.
#define WCM_ADDR (unsigned char)0xC0 // WCM Address
/********************************************************************
This routine sets up the local WCM ready for use.
********************************************************************/
void SetUpWCM(char wcmADDR, char localNode)
{
asm
{
pushsb 4 // push address
pushlb 2 // packet will have 2 data bytes
pushlb 0 // start writing at register 0
pushsb 6 // push local node ID
pushlb 4 // total packet byte count
popcmd // send command via I2C
}
}
/********************************************************************
This is the main routine. When it is finished, it exits back to the
program in file slot 0.
********************************************************************/
void main(char callingProcID)
{
SetUpWCM(WCM_ADDR,1);
aCore_Sleep(500);
aMulti_Halt(callingProcID,100);
}
/********************************************************************/
/********************************************************************/
|
_200.jpg)