void wait_5s ( void )
{
int t;
for ( t = 0; t < 9; t++ )
{
delay10ktcy(225);
}
return;
}
void delay_msec(unsigned int ms)
{
unsigned char delay;
/* Should be adjusted to account for function call and loop overhead */
while ( ms > 0 )
{
delay = MIN(ms, 255);
delay10ktcy(delay);
ms -= delay;
}
}
// this callback is used when something wrong happened
// during communication between master and us
void i2c_hw_slave_on_error()
{
while(1) {LA7=!LA7; delay10ktcy(500);}
// Just tell user user something's got wrong
}
void do_autonomous(void)
{
if (!autonomous)
return;
get_sensors(); // Sensor sampling code
if ( Sumo.atLeftEdge || Sumo.atRightEdge )
Sumo.state = SURVIVE;
switch ( Sumo.state )
{
case SURVIVE:
if ( Sumo.atLeftEdge && Sumo.atRightEdge )
{
sumo_move(BACKWARD);
delay10ktcy(250);
delay10ktcy(250);
//rotate(180);
Sumo.searchClockwise = TRUE;
}
else if ( Sumo.atLeftEdge )
{
sumo_move(BACKWARD);
delay10ktcy(250);
delay10ktcy(250);
//rotate(-135);
//Add a Delay or create a function rotate(int degrees) instead
Sumo.searchClockwise = TRUE;
}
else if ( Sumo.atRightEdge )
{
sumo_move(BACKWARD);
delay10ktcy(250);
delay10ktcy(250);
//rotate(135);
//Add a Delay or create a function rotate(int degrees) instead
Sumo.searchClockwise = FALSE;
}
Sumo.state = HUNT;
break;
case HUNT:
if ( rangeAverage > mTargetThreshold )
Sumo.state = ATTACK;
else if ( Sumo.searchClockwise )
sumo_move(RIGHT);
//arc(mHuntPower, mHuntFactor);
else
sumo_move(LEFT);
//arc(mHuntPower, -mHuntFactor);
break;
case ATTACK:
sumo_move(FORWARD);
if ( rangeAverage < mTargetThreshold )
{
Sumo.state = HUNT;
}
break;
default:
Sumo.state = SURVIVE;
}
}
