 		}
 	}
 
-	telemetry.time = millis();
+	telemetry.time = m_time;
 	telemetry.ir_left = sharp_left.distance();
 	telemetry.ir_right = sharp_right.distance();
 	telemetry.ir_front_left = sharp_front_left.distance();

#define SERIALDEV Serial
#endif

#define START_BUTTON_DEBOUNCE_TIME_MS        250

#define TEENSY_LED          13
#define START_BUTTON        14

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

#define STEERING_PWM_PIN    4
#define DRIVE_PWM_PIN       3

	analogReference (DEFAULT);
	analogReadAveraging(16);
	analogReadResolution(10);

	pinMode(TEENSY_LED, OUTPUT);
	digitalWrite(TEENSY_LED, HIGH);
	pinMode(GREEN_LED, OUTPUT);
	pinMode(RED_LED, OUTPUT);
	pinMode(BATTERY, INPUT);

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

	telemetry.header = BC_TELEMETRY;
}
