[ODRIVE] Initial Python Script for Controlling ODrive

odrive/Python/odrive_demo.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+"""
+Example usage of the ODrive python library to monitor and control ODrive devices
+"""
+
+from __future__ import print_function
+
+import odrive
+from odrive.enums import *
+import time
+import math
+
+# Find a connected ODrive (this will block until you connect one)
+print("finding an odrive...")
+my_drive = odrive.find_any()
+
+# # Calibrate motor and wait for it to finish
+# print("starting calibration...")
+# my_drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
+# while my_drive.axis0.current_state != AXIS_STATE_IDLE:
+#     time.sleep(0.1)
+
+my_drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+
+# my_drive.axis0.controller.config.control_mode = CONTROL_MODE_POSITION_CONTROL
+
+my_drive.axis0.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
+
+# To read a value, simply read the property
+print("Bus voltage is " + str(my_drive.vbus_voltage) + "V")
+
+# Or to change a value, just assign to the property
+my_drive.axis0.controller.input_pos = 3.14
+print("Position setpoint is " + str(my_drive.axis0.controller.pos_setpoint))
+
+# And this is how function calls are done:
+for i in [1,2,3,4]:
+    print('voltage on GPIO{} is {} Volt'.format(i, my_drive.get_adc_voltage(i)))
+
+# # A sine wave to test
+# t0 = time.monotonic()
+# while True:
+#     # setpoint = 4.0 * math.sin((time.monotonic() - t0)*2)
+#     setpoint = 10
+#     print("goto " + str(int(setpoint)))
+#     my_drive.axis0.controller.input_vel = setpoint
+#     time.sleep(0.01)
+
+# Some more things you can try:
+
+# Write to a read-only property:
+# my_drive.vbus_voltage = 11.0  # fails with `AttributeError: can't set attribute`
+
+# # Assign an incompatible value:
+# my_drive.motor0.pos_setpoint = "I like trains"  # fails with `ValueError: could not convert string to float`
+
+
+t0 = time.monotonic()
+while True:
+    # setpoint = 4.0 * math.sin((time.monotonic() - t0)*2)
+    my_drive.axis0.controller.input_vel = -20
+    time.sleep(0.01)

odrive/Python/odrive_teleop.py

@@ -0,0 +1,80 @@
+import rospy
+from geometry_msgs.msg import Twist
+import odrive
+from odrive.enums import *
+from odrive.utils import dump_errors
+import time
+import math
+
+# Constants
+eStopMultiplier = 1
+DAMPENING_THRESHOLD = 0.1
+WHEEL_BASE = 0.62
+WHEEL_DIAMETER = 0.3
+CONTROL_TIMEOUT = 1000  # milliseconds
+LEFT_POLARITY = 1
+RIGHT_POLARITY = -1
+VEL_TO_RPS = 1.0 / (WHEEL_DIAMETER * math.pi) * 98.0 / 3.0
+RPS_LIMIT = 20
+VEL_LIMIT = RPS_LIMIT / VEL_TO_RPS  # Speed limit
+
+# State variables
+is_autonomous = False
+mode_change = True
+left_vel = 0
+right_vel = 0
+left_vel_actual = 0
+right_vel_actual = 0
+dampening_on_l = False
+dampening_on_r = False
+wireless_stop = False
+lastData = 0
+
+
+# Initialize your ODrive
+print("Finding an odrive...")
+my_drive = odrive.find_any()
+print("\tDone")
+
+print("Starting calibration...")
+my_drive.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
+while my_drive.axis0.current_state != AXIS_STATE_IDLE:
+    time.sleep(0.1)
+print("\tDone")
+
+my_drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+my_drive.axis0.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
+
+
+# Dump current errors
+print("Current ODrive errors:")
+dump_errors(my_drive, True) # True clears errors
+print("Cleared all errors")
+
+
+def set_velocity(left_vel, right_vel):
+    """
+    Sets the velocity for the motors using ODrive.
+    """
+    my_drive.axis0.controller.input_vel = left_vel * VEL_TO_RPS * eStopMultiplier
+    # my_drive.axis1.controller.input_vel = right_vel * VEL_TO_RPS * eStopMultiplier
+    time.sleep(0.01)
+
+def vel_callback(twist_msg):
+    global dampening_on_l, dampening_on_r
+    left_vel = LEFT_POLARITY * (twist_msg.linear.x - WHEEL_BASE * twist_msg.angular.z / 2.0)
+    right_vel = RIGHT_POLARITY * (twist_msg.linear.x + WHEEL_BASE * twist_msg.angular.z / 2.0)
+
+    print(f"Left Velocity: {left_vel:.2f}, Right Velocity: {right_vel:.2f}")
+
+    set_velocity(left_vel, right_vel)
+
+
+def listener():
+    rospy.init_node('motor_controller')
+    rospy.Subscriber("/cmd_vel", Twist, vel_callback)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    listener()

odrive/Python/usbpermission

@@ -0,0 +1 @@
+sudo udevadm control --reload-rules && sudo service udev restart && sudo udevadm trigger