 {
   p(F("MANUAL: %d, L %03d, F %03d, R %03d, dir %03d, speed %03d, left_mapped %03d, right_mapped %03d"),
     manual_control, dist_l, dist_f, dist_r, dir, speed, left_mapped, right_mapped);
-}+}

#include <Servo.h>
#include <Metro.h>
//#define Serial Serial1

#include <stdarg.h>
void p(char *fmt, ... ){

int dir_sign = 0;

bool manual_control = true;
int debug_light_on = 1;
byte byteRead;

Metro debug_out = Metro(10000);

void setup() {
  pinMode(13, OUTPUT);
