From 68870c5bcebb0b81239b35fe9cbc16c1647f9f6e Mon Sep 17 00:00:00 2001 From: Jose Date: Sat, 9 Mar 2024 14:43:20 -0500 Subject: [PATCH 1/4] Added python script to demo odrive control. Imports remain a bit untested without entire Odrive repo --- odrive/Python/odrive_demo.py | 62 ++++++++++++++++++++++++++++++++++++ odrive/Python/usbpermission | 1 + 2 files changed, 63 insertions(+) create mode 100755 odrive/Python/odrive_demo.py create mode 100755 odrive/Python/usbpermission diff --git a/odrive/Python/odrive_demo.py b/odrive/Python/odrive_demo.py new file mode 100755 index 0000000..bca1d05 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/odrive/Python/usbpermission b/odrive/Python/usbpermission new file mode 100755 index 0000000..5ea6d8e --- /dev/null +++ b/odrive/Python/usbpermission @@ -0,0 +1 @@ +sudo udevadm control --reload-rules && sudo service udev restart && sudo udevadm trigger From 6b28300bea41e8ac98c39c99fc2b247a66016b48 Mon Sep 17 00:00:00 2001 From: jose Date: Sat, 23 Mar 2024 14:25:39 -0400 Subject: [PATCH 2/4] Added a test script that runs calibration and runs the motors from the teleop cmd_vel topic --- odrive/Python/odrive_teleop.py | 70 ++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 odrive/Python/odrive_teleop.py diff --git a/odrive/Python/odrive_teleop.py b/odrive/Python/odrive_teleop.py new file mode 100644 index 0000000..b501947 --- /dev/null +++ b/odrive/Python/odrive_teleop.py @@ -0,0 +1,70 @@ +import rospy +from geometry_msgs.msg import Twist +import odrive +from odrive.enums import * +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 + + +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) + + 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() From ae2127b4ea803dff4c6c8ca1dd4ea9bcc8e9b28f Mon Sep 17 00:00:00 2001 From: jose Date: Sat, 23 Mar 2024 14:30:03 -0400 Subject: [PATCH 3/4] added velocity print statement --- odrive/Python/odrive_teleop.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/odrive/Python/odrive_teleop.py b/odrive/Python/odrive_teleop.py index b501947..9e8854c 100644 --- a/odrive/Python/odrive_teleop.py +++ b/odrive/Python/odrive_teleop.py @@ -58,6 +58,8 @@ def vel_callback(twist_msg): 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) From eb0a29b062d73336bbe5c638c8294965afd20060 Mon Sep 17 00:00:00 2001 From: jose Date: Sat, 23 Mar 2024 14:51:01 -0400 Subject: [PATCH 4/4] Added basic code to print and clear errors at beginning of script --- odrive/Python/odrive_teleop.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/odrive/Python/odrive_teleop.py b/odrive/Python/odrive_teleop.py index 9e8854c..c40a21e 100644 --- a/odrive/Python/odrive_teleop.py +++ b/odrive/Python/odrive_teleop.py @@ -2,6 +2,7 @@ from geometry_msgs.msg import Twist import odrive from odrive.enums import * +from odrive.utils import dump_errors import time import math @@ -45,12 +46,18 @@ 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 + # my_drive.axis1.controller.input_vel = right_vel * VEL_TO_RPS * eStopMultiplier time.sleep(0.01) def vel_callback(twist_msg): @@ -68,5 +75,6 @@ def listener(): rospy.Subscriber("/cmd_vel", Twist, vel_callback) rospy.spin() + if __name__ == '__main__': listener()