// SETUP void setup() { // For algorithm speed checking loopPeriod = 0; lastLoop = micros(); // Lap timer lapTimer.setInterval(100, IncrementTime); lapTimer.disable(0); // Initialize LCD lcd.begin(16, 2); lcd.setCursor(0,0); // Turn signals pinMode(LEFT_TURN_LED, OUTPUT); pinMode(RIGHT_TURN_LED, OUTPUT); digitalWrite(LEFT_TURN_LED, 0); digitalWrite(RIGHT_TURN_LED, 0); turnSignalTimer.setInterval(50, TurnSignal); // Force motors off by default MotorSpeed(LEFT_MOTOR, 0); MotorSpeed(RIGHT_MOTOR, 0); // Parameters need to be loaded from EEPROM LoadFromEEPROM(); // Intro text Clear(); Cursor(TOP, 0); Print("FAST ORANGE"); Cursor(BOTTOM, 0); #ifdef DEBUG_MODE Print("Debug Mode"); #else Print("Race Mode"); #endif delay(1000); currentState = menu; }
// Updates the sensors and machine state void Update() { if ((lcdRefreshCount == 0) && (currentState != menu) && (ReadButton() == SELECT)) { delay(750); if(ReadButton() == SELECT) // debounce MENU button { // Stop motors before entering menu MotorSpeed(LEFT_MOTOR, 0); MotorSpeed(RIGHT_MOTOR, 0); Clear(); Cursor(TOP, 0); Print("Entering menu"); currentState = menu; delay(1000); lapTimer.disable(0); return; } } // Read raw sensor values left = analogRead(LEFT_SENSOR); right = analogRead(RIGHT_SENSOR); leftDetected = left > thresholdLeft.Value; rightDetected = right > thresholdRight.Value; if (leftDetected || rightDetected) { CenterSmartUpdate(); } // Display values on LCD lcdRefreshCount = (lcdRefreshCount <= 0) ? lcdRefreshPeriod : (lcdRefreshCount - 1); batteryCount = (batteryCount <= 0) ? batteryPeriod : (batteryCount - 1); DisplaySensorValues(); }
// Function to protect bluetooth disconnection void timer_init(void){ BT_timer_id = timer.setTimeout(1000, terminate); // Stop all the motors in two seconds after bluetooth is disconnected, setTimeout function will return the ID of the timer. timer.disable(BT_timer_id); // Disable to timer before connected to Android phone }