inline void CenterSmartUpdate()
{
center = analogRead(CENTER_SENSOR);
if (centerTimeoutCount)
centerTimeoutCount += 1;
if (!centerOldHigh && (center > centerSchmittHigh.Value)) // On switch-to-high
{
centerOldHigh = true; // Switch current center state to high
if((intersectionSignalRecent + 1) == INTERSECTION_COUNT_ORIGIN) // If the start/stop signal is encountered, toggle clock.
// This is done on when it switches high (instead of waiting for timeout) to save a bit of clock-time.
{
lapTimer.toggle(0);
if (!lapTimer.isEnabled(0))
{
// Stop motors before entering menu
MotorSpeed(LEFT_MOTOR, 0);
MotorSpeed(RIGHT_MOTOR, 0);
Clear();
Cursor(TOP, 0);
Print("Finish!");
currentState = menu;
delay(800);
return;
}
}
switch(intersectionTurnFlag)
{
case INTERSECTION_COUNT_LEFT: // First intersection after left signal
currentState = turnLeft;
break;
case INTERSECTION_COUNT_RIGHT: // First intersection after right signal
currentState = turnRight;
break;
default: // Else
currentState = moveStraight;
intersectionSignalRecent += 1;
break;
}
centerTimeoutCount = 0; // Stop timeout counter
intersectionTurnFlag = 0; // Reset flag
}
else if (centerOldHigh && (center < centerSchmittLow.Value)) // On switch-to-low
{
centerOldHigh = false; // Swtich current center state to low
centerTimeoutCount = 1; // Start timeout counter
}
if (centerTimeoutCount >= (centerTimeoutLimit.Value * 2)) // Timeout
{
if (intersectionTurnFlag < intersectionSignalRecent) // Do not let false signals effect the flag
intersectionTurnFlag = intersectionSignalRecent; // Set flag
intersectionSignalRecent = 0; // Reset signal counter
centerTimeoutCount = 0; // Stop timeout counter
}
}
void startTimer(void){
if(!timer.isEnabled(BT_timer_id)){
timer.enable(BT_timer_id); // Enable the timer when bluetooth connection is established
}
timer.restartTimer(BT_timer_id); // Restart the timer
}
