update arduino demo

ArduinoI2C/ArduinoI2C.ino

@@ -28,35 +28,159 @@ bool I2C_transaction(uint8_t slave_addr, const uint8_t * tx_buffer, size_t tx_le
 }
 
 
+int set_and_save_configuration(uint8_t odrive_num, uint8_t axis_num) {
+  bool success;
+  success = odrive::clear_errors(odrive_num, axis_num);
+  if (!success)
+    return __LINE__;
+
+  // select hall effect mode
+  success = odrive::write_axis_property<odrive::AXIS__ENCODER__CONFIG__MODE>(odrive_num, axis_num, 1);
+  if (!success)
+    return __LINE__;
+
+  // configure encoder counts per revolution (6 hall effect states * 12 pole pairs)
+  success = odrive::write_axis_property<odrive::AXIS__ENCODER__CONFIG__CPR>(odrive_num, axis_num, 72);
+  if (!success)
+    return __LINE__;
+
+  // disable velocity integrator
+  success = odrive::write_axis_property<odrive::AXIS__CONTROLLER__CONFIG__VEL_INTEGRATOR_GAIN>(odrive_num, axis_num, 0);
+  if (!success)
+    return __LINE__;
+
+  // select velocity control
+  success = odrive::write_axis_property<odrive::AXIS__CONTROLLER__CONFIG__CONTROL_MODE>(odrive_num, axis_num, 2);
+  if (!success)
+    return __LINE__;
+
+  // set velocity controller P-gain
+  success = odrive::write_axis_property<odrive::AXIS__CONTROLLER__CONFIG__VEL_GAIN>(odrive_num, axis_num, 0.005f);
+  if (!success)
+    return __LINE__;
+
+  // request state: motor calibration
+  success = odrive::write_axis_property<odrive::AXIS__REQUESTED_STATE>(odrive_num, axis_num, 4);
+  if (!success)
+    return __LINE__;
+
+  delay(6000);
+
+  // check if the axis is in idle and no errors occurred
+  if (!odrive::check_axis_state(odrive_num, axis_num, 1))
+    return __LINE__;
+
+  // ensure that the motor calibration is considered valid after power cycle
+  success = odrive::write_axis_property<odrive::AXIS__MOTOR__CONFIG__PRE_CALIBRATED>(odrive_num, axis_num, true);
+  if (!success)
+    return __LINE__;
+
+  // request state: encoder calibration
+  success = odrive::write_axis_property<odrive::AXIS__REQUESTED_STATE>(odrive_num, axis_num, 7);
+  if (!success)
+    return __LINE__;
+
+  delay(12000);
+
+  // check if the axis is in idle and no errors occurred
+  if (!odrive::check_axis_state(odrive_num, axis_num, 1))
+    return __LINE__;
+
+  // ensure that the encoder calibration is considered valid after power cycle
+  success = odrive::write_axis_property<odrive::AXIS__ENCODER__CONFIG__PRE_CALIBRATED>(odrive_num, axis_num, true);
+  if (!success)
+    return __LINE__;
+
+  // store the configuration to NVM
+  // Caution: this operation is usually instantaneous but after every couple of hundred calls it will
+  // take around 1 second (because a flash page needs to be erased).
+  success = odrive::trigger<odrive::SAVE_CONFIGURATION>(odrive_num);
+  if (!success)
+    return __LINE__;
+  return 0;
+}
+
+
+byte odrive_num = 7;
+byte axis_num = 0;
+bool do_setup = true;
+
 void setup() {
   Wire.begin(); // join i2c bus (address optional for master)
   Serial.begin(9600);
+
+  if (do_setup) {
+    Serial.println("Starting ODrive setup...");
+    int error_line = set_and_save_configuration(odrive_num, axis_num);
+    if (error_line != 0) {
+      Serial.print("ODrive setup failed at line ");
+      Serial.print(error_line);
+      Serial.println();
+      return;
+    }
+    Serial.println("ODrive setup succeeded!");
+    do_setup = false;
+  }
 }
 
-byte odrive_num = 7;
-byte x;
 
 void loop() {
   bool success;
+  delay(500);
 
-  float val;
-  success = odrive::read_property<odrive::VBUS_VOLTAGE>(odrive_num, &val);
-  if (success) {
-    Serial.println(val, HEX);
-  } else {
-    Serial.println("error");
+  success = odrive::check_axis_state(odrive_num, axis_num, 8);
+  if (!success) {
+    Serial.println("not in closed loop control - entering closed loop control");
+
+    // clear previous error state
+    success = odrive::clear_errors(odrive_num, axis_num);
+    if (!success) {
+      Serial.println("could not enter closed loop control");
+      return;
+    }
+
+    // request velocity 0
+    success = odrive::write_axis_property<odrive::AXIS__CONTROLLER__VEL_SETPOINT>(odrive_num, axis_num, 0);
+    if (!success) {
+      Serial.println("could not enter closed loop control");
+      return;
+    }
+    
+    // request state: closed loop control
+    success = odrive::write_axis_property<odrive::AXIS__REQUESTED_STATE>(odrive_num, axis_num, 8);
+    if (!success) {
+      Serial.println("could not enter closed loop control");
+      return;
+    }
+
+    success = odrive::check_axis_state(odrive_num, axis_num, 8);
+    if (!success) {
+      Serial.println("could not enter closed loop control");
+      return;
+    }
   }
 
-  success = odrive::write_property<odrive::TEST_PROPERTY>(odrive_num, x++);
-  if (!success)
+  success = odrive::write_axis_property<odrive::AXIS__CONTROLLER__VEL_SETPOINT>(odrive_num, axis_num, 72 * 5);
+  if (!success) {
     Serial.println("error");
-
-  success = odrive::trigger<odrive::SAVE_CONFIGURATION>(odrive_num);
-  if (!success)
-    Serial.println("error");
-  else
-    Serial.println("saved config");
+    return;
+  }
 
   delay(500);
+
+  success = odrive::write_axis_property<odrive::AXIS__CONTROLLER__VEL_SETPOINT>(odrive_num, axis_num, -72 * 5);
+  if (!success) {
+    Serial.println("error");
+    return;
+  }
+
+  // print Vbus to show liveness
+  float vbus;
+  success = odrive::read_property<odrive::VBUS_VOLTAGE>(odrive_num, &vbus);
+  if (!success) {
+    Serial.println("error");
+    return;
+  }
+  Serial.println(vbus);
 }
 

ArduinoI2C/odrive.h

@@ -101,7 +101,8 @@ namespace odrive {
         write_le<T_Int>(buffer, *reinterpret_cast<T_Int*>(&value));
     }
 
-    /* @brief Read from an endpoint on the ODrive
+    /* @brief Read from an endpoint on the ODrive.
+    * To read from an axis specific endpoint use read_axis_property() instead.
     *
     * Usage example:
     *   float val;
@@ -112,9 +113,9 @@ namespace odrive {
     * @return true if the I2C transaction succeeded, false otherwise
     */
     template<int IPropertyId>
-    bool read_property(uint8_t num, endpoint_type_t<IPropertyId>* value) {
+    bool read_property(uint8_t num, endpoint_type_t<IPropertyId>* value, uint16_t address = IPropertyId) {
         uint8_t i2c_tx_buffer[4];
-        write_le<uint16_t>(i2c_tx_buffer, IPropertyId);
+        write_le<uint16_t>(i2c_tx_buffer, address);
         write_le<uint16_t>(i2c_tx_buffer + sizeof(i2c_tx_buffer) - 2, json_crc);
         uint8_t i2c_rx_buffer[byte_width<endpoint_type_t<IPropertyId>>::value];
         if (!I2C_transaction(i2c_addr + num,
@@ -126,25 +127,26 @@ namespace odrive {
         return true;
     }
 
-    /* @brief Write to an endpoint on the ODrive
+    /* @brief Write to an endpoint on the ODrive.
+    * To write to an axis specific endpoint use write_axis_property() instead.
     *
     * Usage example:
-    *   success = odrive::write_property<odrive::AXIS0__CONTROLLER__VEL_SETPOINT>(0, 10000);
+    *   success = odrive::write_property<odrive::TEST_PROPERTY>(0, 42);
     *
     * @param num Selects the ODrive. For instance the value 4 selects
     * the ODrive that has [A2, A1, A0] connected to [VCC, GND, GND].
     * @return true if the I2C transaction succeeded, false otherwise
     */
     template<int IPropertyId>
-    bool write_property(uint8_t num, endpoint_type_t<IPropertyId> value) {
+    bool write_property(uint8_t num, endpoint_type_t<IPropertyId> value, uint16_t address = IPropertyId) {
         uint8_t i2c_tx_buffer[4 + byte_width<endpoint_type_t<IPropertyId>>::value];
-        write_le<uint16_t>(i2c_tx_buffer, IPropertyId);
+        write_le<uint16_t>(i2c_tx_buffer, address);
         write_le<endpoint_type_t<IPropertyId>>(i2c_tx_buffer + 2, value);
         write_le<uint16_t>(i2c_tx_buffer + sizeof(i2c_tx_buffer) - 2, json_crc);
         return I2C_transaction(i2c_addr + num, i2c_tx_buffer, sizeof(i2c_tx_buffer), nullptr, 0);
     }
 
-    /* @brief Write to an endpoint on the ODrive
+    /* @brief Trigger an parameter-less function on the ODrive
     *
     * Usage example:
     *   success = odrive::trigger<odrive::SAVE_CONFIGURATION>(0);
@@ -155,11 +157,43 @@ namespace odrive {
     */
     template<int IPropertyId,
              typename = typename std::enable_if<std::is_void<endpoint_type_t<IPropertyId>>::value>::type>
-    bool trigger(uint8_t num) {
+    bool trigger(uint8_t num, uint16_t address = IPropertyId) {
         uint8_t i2c_tx_buffer[4];
-        write_le<uint16_t>(i2c_tx_buffer, IPropertyId);
+        write_le<uint16_t>(i2c_tx_buffer, address);
         write_le<uint16_t>(i2c_tx_buffer + sizeof(i2c_tx_buffer) - 2, json_crc);
         return I2C_transaction(i2c_addr + num, i2c_tx_buffer, sizeof(i2c_tx_buffer), nullptr, 0);
     }
 
+    template<int IPropertyId>
+    bool read_axis_property(uint8_t num, uint8_t axis, endpoint_type_t<IPropertyId>* value) {
+        return read_property<IPropertyId>(num, value, IPropertyId + axis * AXIS_ENDPOINT_COUNT);
+    }
+
+    template<int IPropertyId>
+    bool write_axis_property(uint8_t num, uint8_t axis, endpoint_type_t<IPropertyId> value) {
+        return write_property<IPropertyId>(num, value, IPropertyId + axis * AXIS_ENDPOINT_COUNT);
+    }
+
+
+    /* @brief Checks if the axis is in the requested state and the error register is clear */
+    bool check_axis_state(uint8_t num, uint8_t axis, uint8_t state) {
+        endpoint_type_t<odrive::AXIS__CURRENT_STATE> observed_state = 0;
+        endpoint_type_t<odrive::AXIS__ERROR> observed_error = 0;
+        if (!read_axis_property<odrive::AXIS__CURRENT_STATE>(num, axis, &observed_state))
+            return false;
+        if (!read_axis_property<odrive::AXIS__ERROR>(num, axis, &observed_error))
+            return false;
+        return (observed_error == 0) && (observed_state == state);
+    }
+
+    /* @brief Clears any error state of the specified axis */
+    bool clear_errors(uint8_t num, uint8_t axis) {
+        if (!write_axis_property<odrive::AXIS__ERROR>(num, axis, 0))
+            return false;
+        if (!write_axis_property<odrive::AXIS__MOTOR__ERROR>(num, axis, 0))
+            return false;
+        if (!write_axis_property<odrive::AXIS__ENCODER__ERROR>(num, axis, 0))
+            return false;
+        return true;
+    }
 }