 	}
 }
 
-void toggle_led() {
+void toggle_led(uint8_t led) {
 	static bool ledon = true;
 	if (ledon) {
 		ledon = false;
-		digitalWrite(TEENSY_LED, HIGH);
+		digitalWrite(led, HIGH);
 	}
 	else {
 		ledon = true;
-		digitalWrite(TEENSY_LED, LOW);
-	}
+		digitalWrite(led, LOW);
+	}
+}
+
+void blink_automatic_mode() {
+	static uint32_t last_blink;
+	if (millis() - last_blink > 125) {
+		toggle_led(TEENSY_LED);
+		last_blink = millis();
+	}
+}
+
+uint32_t battery_voltage() {
+	const uint32_t vref = 3600000; // 3.6 V
+	const uint32_t voltage_divider = 2710; // divides 2.7 times
+	return vref/1024 * analogRead(BATTERY) * (uint64_t)voltage_divider/1000000;
 }
 
 void loop() {
+	m_time = millis();
+
 	if (m_button_press) {
 		m_button_press = false;
+
+		m_countdown = true;
+		m_countdown_start_time = m_time;
+
+		digitalWrite(GREEN_LED, HIGH);
+	}
+
+	// handle button debounce
+	if (!m_button_active && (m_time > m_last_button_time +  START_BUTTON_DEBOUNCE_TIME_MS)) {
+		m_button_active = true;
+	}
+
+	// ready to race!?
+	if (m_countdown && (m_time > m_countdown_start_time +  START_DELAY_MS)) {
+		m_button_active = true;
 		m_automatic = true;
-	}
-
-	// handle debounce
-	if (!m_button_active && ((millis() - m_last_button_time) > START_BUTTON_DEBOUNCE_TIME_MS)) {
-		m_button_active = true;
 	}
 
 	while (SERIALDEV.available() > 0) {

	if (m_automatic) {
		steeringservo.write(drive_cmd.steering_pwm);
		drivingservo.write(drive_cmd.driving_pwm);
		toggle_led();
	}

	if (telemetry.time > last_time + 20) {
		send_telemetry();
		last_time = telemetry.time;
	}
}


Servo drivingservo;

bool m_automatic = false;
uint32_t m_last_telemetry_time = 0;

bool m_button_press = false;
bool m_button_active = true;
uint32_t m_last_button_time = 0;
bool m_countdown = false;
uint32_t m_countdown_start_time = 0;

uint32_t m_time;
uint32_t m_last_battery_check_time = 0;

MCDriver driver;

