   Wire.onReceive(I2CReceive); 
   Serial.begin(115200);
   timeLastPacket = 0;
+  dataValid = false;
 }
 
 void loop() {
-  if(millis() - timeLastPacket < TIMEOUT) {
+  static boolean timedOut = false;
+  
+  if(millis() - timeLastPacket < TIMEOUT && dataValid) {
+    timedOut = false; 
+    
     // Map values from 5-bit char to int from full reverse to full forwards
     int leftSpeed = map(data.left_motor, 0, 32, MIN_FREQUENCY, MAX_FREQUENCY);
     int rightSpeed = map(data.right_motor, 0, 32, MIN_FREQUENCY, MAX_FREQUENCY);

  if(count % NECESSARY_PACKET_SIZE == 0) {
    timeLastPacket = millis();
    
    for(int i = 0; i < count / NECESSARY_PACKET_SIZE && Wire.available() >= 3; i++) {
      readBytes(&data, NECESSARY_PACKET_SIZE);
    }
  }
}

Servo leftMotor, rightMotor;

ChassisData data;
boolean dataValid;

void I2CReceive(int count);
boolean readBytes(void* dest, size_t len);

void setup() {
  // Attach the motors and set their starting speed to neutral/zero speed
