Listing 2: The rest of the Bugbot code.
//behaviors
void do_wander(void)
{
//Does just the wander routine
if (priority == WANDER) {
dur—;
if (dur == 0)
priority = 0;}
if (priority < WANDER) {
priority = WANDER;
currDir = FWD;
PORTB = currDir;
sleft = MED;
sright = MED;
dur = 100;}
}
void do_avoid(void)
{ //Does the object avoidance
byte irpd;//IRPD inputs
if (priority == AVOID)
{
dur—;
if (dur == 0)
{
priority = 0;}
}
if (priority < AVOID)
{
currDir = FWD;
PORTB = currDir;
irpd = PORTA;
irpd &= 0x03;
if (irpd != 0)
{
//Note IRPD bits
priority = AVOID;
switch (irpd)
{
case 3: // directly ahead
currDir = REV;
PORTB = currDir;
sleft = FAST;
sright = SLOW;
dur = 3000;
break;
case 2: // something to the
// right
currDir = RTL;
PORTB = currDir;
sleft = FAST;
sright = FAST;
dur =300;
break;
case 1:
// something
// to the
// left
currDir = RTR;
PORTB = currDir;
sleft = FAST;
sright = FAST;
dur = 300;
break;}
}
}
}
16 SERVO 12.2010
void do_edge(void)
{
// Watches for the edge of the table
byte edet; //edge detector
if (priority == EDGE)
{
dur—;
if (dur < 6000)
{
currDir = RTR;
PORTB = currDir;}
if (dur == 0)
{
priority = 0;}
}
if (priority < EDGE)
{
edet = PORTB;
edet &= 0x10;
if (edet) //1 == edge of the world
{
priority = EDGE;
currDir = REV;
PORTB = currDir;
sleft = FAST;
sright = FAST;
dur = 9000;}
}
}
void Init(void)
{
// This starts it all off, set up the
// ports, IRQ and defaults
/*
PORTA 0 = Left IRPD
1 = Right IRPD
2 = Left PWM
3 = Right PWM
4 = N/C
5 = N/C
PORTB 0 = 1A Right Motor direction pin
1 = 2A Right Motor direction pin
2 = 3A Left Motor direction pin
3 = 4A Left Motor direction pin
4 = Down Sharp GP2Y0D340K
5 = Down IRPD enable N/C
6 = IRPD enable N/C
7 = N/C
*/
set_tris_A(0x03);//0,1 inputs, 2-4
//outputs
setup_comparator(NC_NC_NC_NC); //turn
// comparator
// off
//Set up PWM interrupt stuff
setup_timer_0( RTCC_INTERNAL | RTCC_DIV_2);
enable_interrupts(INT_TIMER0);
enable_interrupts(GLOBAL);
set_timer0(240);
set_tris_B(0x90);
}
void main(void)
{
Init();//0-3 outputs, 4&7
//inputs, 5&6
//outputs
