// ===================================================================================================================================
void setup()
{
for(byte i=0; i < numSensors; i++)
{ pinMode(sensor[i], INPUT); }
pinMode(emitter, OUTPUT);
pinMode(1, INPUT_PULLUP); // help pull up reset
pinMode(2, INPUT_PULLUP); // help pull up reset
pinMode(3, INPUT_PULLUP); // help pull up reset
delay(4000);
setupServos(); // It's not necessary to set the servo pins as OUTPUT, that's taken care of for you
}
// ===================================================================================================================================
void loop()
{
static byte state = STRAIGHT;
static byte strideCounter = straightenStrides;
if( millis() - sensorTimer > sensorInterval ) // time to take a sensor reading
{
sensorTimer += sensorInterval; // schedule next sensor reading
char sensorResult = readSensor();
switch(state)
{
case STRAIGHT:
if( sensorResult == 0 ) // we've detected an obstacle on the left
{ state = RIGHT; }
else if( sensorResult == 1 ) // we've detected an obstacle on the right
{ state = LEFT; }
break;
case STRAIGHTENLEFT:
case STRAIGHTENRIGHT:
if( strideCounter < 1 ) // we've been turning for staightenStride strides, so...
{ state = STRAIGHT; } // ...we're done turning and can go straight now
if( sensorResult > -1 ) // encountered a new obstacle while avoiding the last one
{ state++; } // push to full turn
break;
case LEFT:
case RIGHT:
strideCounter = straightenStrides;
if (sensorResult < 0) // we've cleared the obstacle
{ state--; } // fall back to straighten
break;
}
}
if( millis() > batteryTimer ) // time to check the battery
{
batteryTimer += 1000;
checkBattery();
}
if( millis() > strideTimer ) // time to take a new stride
{
static byte stridePhase = 0; // we start from center position, then go through a number of phases
if(state == STRAIGHT)
{
switch(stridePhase)
{
case 0: // lift body on one side
strideTimer += liftInterval;
moveServo(MIDDLESERVO, middleServoCenter - middleLiftHeight);
break;
case 1: // advance to extreme
strideTimer += strideInterval;
moveServo(LEFTSERVO, leftServoCenter + sideStrideLength);
moveServo(RIGHTSERVO, rightServoCenter + sideStrideLength);
break;
case 2: // lift other side of body
strideTimer += liftInterval;
moveServo(MIDDLESERVO, middleServoCenter + middleLiftHeight);
break;
case 3: // reverse to extreme
strideTimer += strideInterval;
moveServo(LEFTSERVO, leftServoCenter - sideStrideLength);
moveServo(RIGHTSERVO, rightServoCenter - sideStrideLength);
break;
}
}
else // turning
{
char turnDirection = 1;
if( state > LEFT ) // if turning or straightening right...
{ turnDirection = -1; } // ...reverse middle legs phase
switch(stridePhase)
{
case 0: // lift body on one side
strideTimer += liftInterval;
moveServo(MIDDLESERVO, middleServoCenter - (middleLiftHeight * turnDirection));
break;
case 1: // advance to extreme
strideTimer += strideInterval;
moveServo(LEFTSERVO, leftServoCenter + sideStrideLength);
moveServo(RIGHTSERVO, rightServoCenter - sideStrideLength);
break;
case 2: // lift other side of body
strideTimer += liftInterval;
moveServo(MIDDLESERVO, middleServoCenter + (middleLiftHeight * turnDirection));
break;
case 3: // reverse to extreme
strideTimer += strideInterval;
moveServo(LEFTSERVO, leftServoCenter - sideStrideLength);
moveServo(RIGHTSERVO, rightServoCenter + sideStrideLength);
break;
}
}
stridePhase++;
if( stridePhase > 3 ) // we've completed an entire stride
{
stridePhase = 0;
if( strideCounter > 0 ) // make sure we don't overflow
{ strideCounter--; }
}
}
}