// 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
}
