task main()
{
while(true)
{
readSensors();
if (touchL != 0 || touchR != 0)
{
avoidObstacle();
}
else if (lightDetected && facingLight)
{
driveForward();
}
else if (lightDetected && !facingLight)
{
turnTowardsLight();
}
else
{
//Default case
scanForLight();
}
}
}
void smartGoToGoal(float x, float y)
{
xpos=x;
ypos=y;
while((roboxpos<=(x-.25)||roboxpos>=(x+.25))&&(roboypos<=(y-.25)||roboypos>=(y+.25)))
{
avoidObstacle(7,5,5,5);
updateLCD();
}
}
void move() {
if(isTurning)
turn(speed);
else if(currentState == FORWARD_AVOIDANCE || currentState == COHERENCE_AVOIDANCE) {
avoidObstacle(speed, distances);
setSpeed(speed[0], speed[1]);
}
else {
// Just move forward
setSpeed(BIAS_SPEED, BIAS_SPEED);
}
}
