 	if (m_automatic) {
 		steeringservo.write(drive_cmd.steering_pwm);
 		drivingservo.write(drive_cmd.driving_pwm);
-		toggle_led();
-	}
-
-	if (telemetry.time > last_time + 20) {
+		blink_automatic_mode();
+	}
+
+	if (m_time > m_last_telemetry_time + 20) {
 		send_telemetry();
-		last_time = telemetry.time;
-	}
-}
-
+		m_last_telemetry_time = m_time;
+	}
+
+	if (m_time > m_last_battery_check_time + BATTERY_CHECK_INTERVAL_MS) {
+		m_last_battery_check_time = m_time;
+		if (battery_voltage() < BATTERY_LOW_MILLI_VOLTAGE) {
+			digitalWrite(RED_LED, HIGH);
+		}
+		else {
+			digitalWrite(RED_LED, LOW);
+		}
+	}
+}
+

	analogReference (DEFAULT);
	analogReadAveraging(16);
	analogReadResolution(10);
	pinMode(TEENSY_LED, OUTPUT);

	pinMode(START_BUTTON, INPUT_PULLUP);
	attachInterrupt(START_BUTTON, button_service, FALLING);

	telemetry.header = BC_TELEMETRY;
}

#define SERIALDEV Serial
#endif

#define START_BUTTON_DEBOUNCE_TIME_MS       250
#define START_DELAY_MS                      5050

#define BATTERY_CHECK_INTERVAL_MS           1000
#define BATTERY_LOW_MILLI_VOLTAGE           7500

#define TEENSY_LED          13
#define START_BUTTON        14
#define GREEN_LED           16
#define RED_LED             17

#define BATTERY             A4

#define IR_LEFT             A9
#define IR_FRONT_LEFT       A8
#define IR_FRONT            A5
#define IR_FRONT_RIGHT      A7
#define IR_RIGHT            A6

#define STEERING_PWM_PIN    4
#define DRIVE_PWM_PIN       3
