From ed8705071f8a30c482e7808ea3cc0d6b68f4e04d Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Wed, 14 Oct 2020 19:32:39 -0400 Subject: [PATCH] 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 --- Arduino/ODriveArduino/ODriveArduino.cpp | 6 +-- Arduino/ODriveArduino/ODriveArduino.h | 2 +- .../ODriveArduinoTest/ODriveArduinoTest.ino | 49 ++++++++++++++----- 3 files changed, 41 insertions(+), 16 deletions(-) diff --git a/Arduino/ODriveArduino/ODriveArduino.cpp b/Arduino/ODriveArduino/ODriveArduino.cpp index 3f4eab5a..cd3fcd5e 100644 --- a/Arduino/ODriveArduino/ODriveArduino.cpp +++ b/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"; 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..e75a1862 100644 --- a/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino +++ b/Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino @@ -1,14 +1,39 @@ - +// includes +#include #include #include - -// Printing with stream operator +// Printing with stream operator helper functions template 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);