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 }