diff --git a/Arduino/ODriveArduino/ODriveArduino.cpp b/Arduino/ODriveArduino/ODriveArduino.cpp
index 3f4eab5a..bca507f2 100644
--- a/Arduino/ODriveArduino/ODriveArduino.cpp
+++ b/Arduino/ODriveArduino/ODriveArduino.cpp
@@ -59,8 +59,8 @@ int32_t ODriveArduino::readInt() {
     return readString().toInt();
 }
 
-bool ODriveArduino::run_state(int axis, int requested_state, bool wait) {
-    int timeout_ctr = 100;
+bool ODriveArduino::run_state(int axis, int requested_state, bool wait_for_idle, float timeout) {
+    int timeout_ctr = (int)(timeout * 10.0f);
     serial_ << "w axis" << axis << ".requested_state " << requested_state << '\n';
     if (wait) {
         do {
diff --git a/Arduino/ODriveArduino/ODriveArduino.h b/Arduino/ODriveArduino/ODriveArduino.h
index 8e39b244..6620f859 100644
--- a/Arduino/ODriveArduino/ODriveArduino.h
+++ b/Arduino/ODriveArduino/ODriveArduino.h
@@ -35,7 +35,7 @@ public:
     int32_t readInt();
 
     // State helper
-    bool run_state(int axis, int requested_state, bool wait);
+    bool run_state(int axis, int requested_state, bool wait_for_idle, float timeout = 10.0f);
 private:
     String readString();
 
diff --git a/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino b/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino
index 1e835026..c2cc8b77 100644
--- a/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino
+++ b/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino
@@ -52,23 +52,23 @@ void loop() {
 
       requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION;
       Serial << "Axis" << c << ": Requesting state " << requested_state << '\n';
-      odrive.run_state(motornum, requested_state, true);
+      if(!odrive.run_state(motornum, requested_state, true)) return;
 
       requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
       Serial << "Axis" << c << ": Requesting state " << requested_state << '\n';
-      odrive.run_state(motornum, requested_state, true);
+      if(!odrive.run_state(motornum, requested_state, true, 25.0f)) return;
 
       requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL;
       Serial << "Axis" << c << ": Requesting state " << requested_state << '\n';
-      odrive.run_state(motornum, requested_state, false); // don't wait
+      if(!odrive.run_state(motornum, requested_state, false /*don't wait*/)) return;
     }
 
     // Sinusoidal test move
     if (c == 's') {
       Serial.println("Executing test move");
       for (float ph = 0.0f; ph < 6.28318530718f; ph += 0.01f) {
-        float pos_m0 = 20000.0f * cos(ph);
-        float pos_m1 = 20000.0f * sin(ph);
+        float pos_m0 = 2.0f * cos(ph);
+        float pos_m1 = 2.0f * sin(ph);
         odrive.SetPosition(0, pos_m0);
         odrive.SetPosition(1, pos_m1);
         delay(5);