simplify setup sequence

src/main.cpp

@@ -17,8 +17,6 @@ void setup()
 
     initEeprom(HARDWARE_ID);
 
-    delay(2000);
-
     initXBee(HARDWARE_ID);
     unpacker.attach(XBEE_SERIAL);
 
@@ -47,6 +45,14 @@ void setup()
     WHILL_SERIAL_R.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // R
     whill.attach(WHILL_SERIAL_L, WHILL_SERIAL_R);
 
+#if 1
+    whill.stop();
+    delay(500);
+    whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
+    delay(500);
+    whill.speed(SPEEDMODE::RS232C, 60, 90, 90, 60, 90, 90, 60, 90, 90); // for new whill firmware
+    delay(500);
+#else
     whill.streaming(false, WHILL_INFO_INTERVAL_MS, DATASET::SPEED_PROFILE);
     whill.stop();
 
@@ -67,6 +73,7 @@ void setup()
 
     whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
     delay(1000);
+#endif
 
     if (digitalRead(PIN_EMERGENCY) == LOW) b_motor_active = true;
     else                                   b_motor_active = false;