Fix ::run_state method (wait_for_idle instead of idle) and better vel_limit setting in .ino sketch example

Pull changes from arduino-improvements branch into master

Author: PAJohnson

Arduino/ODriveArduino/ODriveArduino.cpp

@@ -59,10 +59,10 @@ 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) {
+    if (wait_for_idle) {
         do {
             delay(100);
             serial_ << "r axis" << axis << ".current_state\n";

Arduino/ODriveArduino/ODriveArduino.h

@@ -35,7 +35,7 @@ class ODriveArduino {
     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();
 

Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino

@@ -1,14 +1,39 @@
-
+// includes
+#include <HardwareSerial.h>
 #include <SoftwareSerial.h>
 #include <ODriveArduino.h>
-
-// Printing with stream operator
+// Printing with stream operator helper functions
 template<class T> inline Print& operator <<(Print &obj,     T arg) { obj.print(arg);    return obj; }
 template<>        inline Print& operator <<(Print &obj, float arg) { obj.print(arg, 4); return obj; }
 
-// Serial to the ODrive
-SoftwareSerial odrive_serial(8, 9); //RX (ODrive TX), TX (ODrive RX)
-// Note: you must also connect GND on ODrive to GND on Arduino!
+
+////////////////////////////////
+// Set up serial pins to the ODrive
+////////////////////////////////
+
+// Below are some sample configurations.
+// You can comment out the default Teensy one and uncomment the one you wish to use.
+// You can of course use something different if you like
+// Don't forget to also connect ODrive GND to Arduino GND.
+
+// Teensy 3 and 4 (all versions) - Serial1
+// pin 0: RX - connect to ODrive TX
+// pin 1: TX - connect to ODrive RX
+// See https://www.pjrc.com/teensy/td_uart.html for other options on Teensy
+HardwareSerial& odrive_serial = Serial1;
+
+// Arduino Mega or Due - Serial1
+// pin 19: RX - connect to ODrive TX
+// pin 18: TX - connect to ODrive RX
+// See https://www.arduino.cc/reference/en/language/functions/communication/serial/ for other options
+// HardwareSerial& odrive_serial = Serial1;
+
+// Arduino without spare serial ports (such as Arduino UNO) have to use software serial.
+// Note that this is implemented poorly and can lead to wrong data sent or read.
+// pin 8: RX - connect to ODrive TX
+// pin 9: TX - connect to ODrive RX
+// SoftwareSerial odrive_serial(8, 9);
+
 
 // ODrive object
 ODriveArduino odrive(odrive_serial);
@@ -28,7 +53,7 @@ void setup() {
   // You can of course set them different if you want.
   // See the documentation or play around in odrivetool to see the available parameters
   for (int axis = 0; axis < 2; ++axis) {
-    odrive_serial << "w axis" << axis << ".controller.config.vel_limit " << 22000.0f << '\n';
+    odrive_serial << "w axis" << axis << ".controller.config.vel_limit " << 10.0f << '\n';
     odrive_serial << "w axis" << axis << ".motor.config.current_lim " << 11.0f << '\n';
     // This ends up writing something like "w axis0.motor.config.current_lim 10.0\n"
   }
@@ -52,23 +77,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);