absorb Arduino lib

Arduino/ODriveArduino/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2017 Oskar Weigl
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

Arduino/ODriveArduino/ODriveArduino.cpp

@@ -0,0 +1,116 @@
+
+#include "Arduino.h"
+#include "ODriveArduino.h"
+
+static const int kMotorOffsetFloat = 2;
+static const int kMotorStrideFloat = 28;
+static const int kMotorOffsetInt32 = 0;
+static const int kMotorStrideInt32 = 4;
+static const int kMotorOffsetBool = 0;
+static const int kMotorStrideBool = 4;
+static const int kMotorOffsetUint16 = 0;
+static const int kMotorStrideUint16 = 2;
+
+// Print with stream operator
+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; }
+
+ODriveArduino::ODriveArduino(Stream& serial)
+    : serial_(serial) {}
+
+void ODriveArduino::SetPosition(int motor_number, float position) {
+    SetPosition(motor_number, position, 0.0f, 0.0f);
+}
+
+void ODriveArduino::SetPosition(int motor_number, float position, float velocity_feedforward) {
+    SetPosition(motor_number, position, velocity_feedforward, 0.0f);
+}
+
+void ODriveArduino::SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward) {
+    serial_ << "$p " << motor_number  << " " << position << " " << velocity_feedforward << " " << current_feedforward << "!";
+}
+
+void ODriveArduino::SetVelocity(int motor_number, float velocity) {
+    SetVelocity(motor_number, velocity, 0.0f);
+}
+
+void ODriveArduino::SetVelocity(int motor_number, float velocity, float current_feedforward) {
+    serial_ << "$v " << motor_number  << " " << velocity << " " << current_feedforward << "!";
+}
+
+float ODriveArduino::getBusVoltage() {
+    serial_ << "$g 0 0!";
+    return readFloat();
+}
+
+float ODriveArduino::GetParameter(int motor_number, ParamNamesFloat parameter) {
+    int idx = kMotorOffsetFloat + kMotorStrideFloat * motor_number + parameter;
+    serial_ << "$g 0 " << idx << "!";
+    return readString().toFloat();
+}
+
+int32_t ODriveArduino::GetParameter(int motor_number, ParamNamesInt32 parameter) {
+    int idx = kMotorOffsetInt32 + kMotorStrideInt32 * motor_number + parameter;
+    serial_ << "$g 1 " << idx << "!";
+    return readString().toInt();
+}
+
+bool ODriveArduino::GetParameter(int motor_number, ParamNamesBool parameter) {
+    int idx = kMotorOffsetBool + kMotorStrideBool * motor_number + parameter;
+    serial_ << "$g 2 " << idx << "!";
+    return readString().toInt();
+}
+
+uint16_t ODriveArduino::GetParameter(int motor_number, ParamNamesUint16 parameter) {
+    int idx = kMotorOffsetUint16 + kMotorStrideUint16 * motor_number + parameter;
+    serial_ << "$g 3 " << idx << "!";
+    return readString().toInt();
+}
+
+
+void ODriveArduino::SetParameter(int motor_number, ParamNamesFloat parameter, float value) {
+    int idx = kMotorOffsetFloat + kMotorStrideFloat * motor_number + parameter;
+    serial_ << "$s 0 " << idx << " " << value << "!";
+}
+
+void ODriveArduino::SetParameter(int motor_number, ParamNamesInt32 parameter, int32_t value) {
+    int idx = kMotorOffsetInt32 + kMotorStrideInt32 * motor_number + parameter;
+    serial_ << "$s 1 " << idx << " " << value << "!";
+}
+
+void ODriveArduino::SetParameter(int motor_number, ParamNamesBool parameter, bool value) {
+    int idx = kMotorOffsetBool + kMotorStrideBool * motor_number + parameter;
+    serial_ << "$s 2 " << idx << " " << value << "!";
+}
+
+void ODriveArduino::SetParameter(int motor_number, ParamNamesUint16 parameter, uint16_t value) {
+    int idx = kMotorOffsetUint16 + kMotorStrideUint16 * motor_number + parameter;
+    serial_ << "$s 3 " << idx << " " << value << "!";
+}
+
+
+float ODriveArduino::readFloat() {
+    return readString().toFloat();
+}
+
+int32_t ODriveArduino::readInt() {
+    return readString().toInt();
+}
+
+String ODriveArduino::readString() {
+    String str = "";
+    static const unsigned long timeout = 1000;
+    unsigned long timeout_start = millis();
+    for (;;) {
+        while (!serial_.available()) {
+            if (millis() - timeout_start >= timeout) {
+                return str;
+            }
+        }
+        char c = serial_.read();
+        if (c == '\n')
+            break;
+        str += c;
+    }
+    return str;
+}

Arduino/ODriveArduino/ODriveArduino.h

@@ -0,0 +1,88 @@
+
+#ifndef ODriveArduino_h
+#define ODriveArduino_h
+
+#include "Arduino.h"
+
+class ODriveArduino {
+public:
+    enum ParamNamesFloat {
+        PARAM_FLOAT_POS_SETPOINT,
+        PARAM_FLOAT_POS_GAIN,
+        PARAM_FLOAT_VEL_SETPOINT,
+        PARAM_FLOAT_VEL_GAIN,
+        PARAM_FLOAT_VEL_INTEGRATOR_GAIN,
+        PARAM_FLOAT_VEL_INTEGRATOR_CURRENT,
+        PARAM_FLOAT_VEL_LIMIT,
+        PARAM_FLOAT_CURRENT_SETPOINT,
+        PARAM_FLOAT_CALIBRATION_CURRENT,
+        PARAM_FLOAT_PHASE_INDUCTANCE,
+        PARAM_FLOAT_PHASE_RESISTANCE,
+        PARAM_FLOAT_CURRENT_MEAS_PHB,
+        PARAM_FLOAT_CURRENT_MEAS_PHC,
+        PARAM_FLOAT_DC_CALIB_PHB,
+        PARAM_FLOAT_DC_CALIB_PHC,
+        PARAM_FLOAT_SHUNT_CONDUCTANCE,
+        PARAM_FLOAT_PHASE_CURRENT_REV_GAIN,
+        PARAM_FLOAT_CURRENT_CONTROL_CURRENT_LIM,
+        PARAM_FLOAT_CURRENT_CONTROL_P_GAIN,
+        PARAM_FLOAT_CURRENT_CONTROL_I_GAIN,
+        PARAM_FLOAT_CURRENT_CONTROL_V_CURRENT_CONTROL_INTEGRAL_D,
+        PARAM_FLOAT_CURRENT_CONTROL_V_CURRENT_CONTROL_INTEGRAL_Q,
+        PARAM_FLOAT_CURRENT_CONTROL_IBUS,
+        PARAM_FLOAT_ENCODER_PHASE,
+        PARAM_FLOAT_ENCODER_PLL_POS,
+        PARAM_FLOAT_ENCODER_PLL_VEL,
+        PARAM_FLOAT_ENCODER_PLL_KP,
+        PARAM_FLOAT_ENCODER_PLL_KI,
+    };
+
+    enum ParamNamesInt32 {
+        PARAM_INT_CONTROL_MODE,
+        PARAM_INT_ENCODER_ENCODER_OFFSET,
+        PARAM_INT_ENCODER_ENCODER_STATE,
+        PARAM_INT_ERROR,
+    };
+
+    enum ParamNamesBool{
+        PARAM_BOOL_THREAD_READY,
+        PARAM_BOOL_ENABLE_CONTROL,
+        PARAM_BOOL_DO_CALIBRATION,
+        PARAM_BOOL_CALIBRATION_OK,
+    };
+
+    enum ParamNamesUint16{
+        PARAM_UINT16_CONTROL_DEADLINE,
+        PARAM_UINT16_LAST_CPU_TIME,
+    };
+
+    ODriveArduino(Stream& serial);
+
+    // Get
+    float getBusVoltage();
+
+    float    GetParameter(int motor_number, ParamNamesFloat parameter);
+    int32_t  GetParameter(int motor_number, ParamNamesInt32 parameter);
+    bool     GetParameter(int motor_number, ParamNamesBool parameter);
+    uint16_t GetParameter(int motor_number, ParamNamesUint16 parameter);
+
+    // Set
+    void SetPosition(int motor_number, float position);
+    void SetPosition(int motor_number, float position, float velocity_feedforward);
+    void SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward);
+    void SetVelocity(int motor_number, float velocity);
+    void SetVelocity(int motor_number, float velocity, float current_feedforward);
+
+    void SetParameter(int motor_number, ParamNamesFloat parameter, float value);
+    void SetParameter(int motor_number, ParamNamesInt32 parameter, int32_t value);
+    void SetParameter(int motor_number, ParamNamesBool parameter, bool value);
+    void SetParameter(int motor_number, ParamNamesUint16 parameter, uint16_t value);
+private:
+    float readFloat();
+    int32_t readInt();
+    String readString();
+
+    Stream& serial_;
+};
+
+#endif //ODriveArduino_h

Arduino/ODriveArduino/README.md

@@ -0,0 +1,10 @@
+# ODriveArduino
+Arduino library for the ODrive
+
+To install the library, first clone this repository. In the Arduino IDE select: *Sketch -> Include Library -> Add .ZIP Library...*
+
+Select the enclosing folder (e.g. ODriveArduino) to add it. Restarting the Arduino IDE may be necessary to see the examples in the *File* dropdown. Check the included example *ODriveArduinoTest* for basic usage. 
+
+**Important Note: The Arduino library currently only supports the legacy UART protocol. This is selected in the firmware in [MotorControl/commands.h](https://github.com/madcowswe/ODrive/blob/master/Firmware/MotorControl/commands.h), with UART_PROTOCOL_LEGACY**
+
+For most applications, this protocol is sufficient. Future versions of the Arduino library will be upgraded to support the current binary protocol.

Arduino/ODriveArduino/examples/ODriveArduinoTest/ODriveArduinoTest.ino

@@ -0,0 +1,75 @@
+
+#include <SoftwareSerial.h>
+#include <ODriveArduino.h>
+
+// Printing with stream operator
+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)
+
+// ODrive object
+ODriveArduino odrive(odrive_serial);
+
+void setup() {
+  // ODrive uses 115200 baud
+  odrive_serial.begin(115200);
+
+  // Serial to PC
+  Serial.begin(115200);
+  while (!Serial) ; // wait for Arduino Serial Monitor to open
+
+  Serial.println("ODriveArduino alpha.");
+  Serial.println("Setting parameters...");
+
+  // In this example we set the same parameters to both motors.
+  // You can of course set them different if you want.
+  for (int motor = 0; motor < 2; ++motor) {
+    odrive.SetParameter(motor, odrive.PARAM_FLOAT_CURRENT_CONTROL_CURRENT_LIM, 10.0f);  // [A]
+    odrive.SetParameter(motor, odrive.PARAM_FLOAT_VEL_LIMIT, 20000.0f);                 // [counts/s]
+    odrive.SetParameter(motor, odrive.PARAM_FLOAT_POS_GAIN, 20.0f);                     // [(counts/s) / counts]
+    odrive.SetParameter(motor, odrive.PARAM_FLOAT_VEL_GAIN, 15.0f/10000.0f);            // [A/(counts/s)]
+    odrive.SetParameter(motor, odrive.PARAM_FLOAT_VEL_INTEGRATOR_GAIN, 0.0f/10000.0f); // [A/(counts/s * s)]
+  }
+
+  Serial.println("Ready!");
+  Serial.println("Send the character 's' to exectue test move");
+  Serial.println("Send the character 'b' to read bus voltage");
+  Serial.println("Send the character 'p' to read motor positions in a 10s loop");
+}
+
+void loop() {
+
+  if (Serial.available()) {
+    char c = Serial.read();
+
+    // Sinusoidal test move
+    if (c == 's') {
+      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);
+        odrive.SetPosition(0, pos_m0);
+        odrive.SetPosition(1, pos_m1);
+        delay(5);
+      }
+    }
+
+    // Read bus voltage
+    if (c == 'b') {
+      Serial << "Vbus voltage: " << odrive.getBusVoltage() << '\n';
+    }
+
+    // print motor positions in a 10s loop
+    if (c == 'p') {
+      static const unsigned long duration = 10000;
+      unsigned long start = millis();
+      while(millis() - start < duration) {
+        for (int motor = 0; motor < 2; ++motor) {
+          Serial << odrive.GetParameter(motor, odrive.PARAM_FLOAT_ENCODER_PLL_POS) << '\t';
+        }
+        Serial << '\n';
+      }
+    }
+  }
+}