NASA-Big-Idea-Challenge Updates: got servo serial working

.gitignore

@@ -0,0 +1,3 @@
+.vscode
+
+**/_pycache_/

demos/servo_serial/demo_2DOF_duty_servo_serial.py

@@ -0,0 +1,23 @@
+from TMotorCANControl.servo_serial import *
+from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
+
+duty = 0.1
+
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev1:
+    with TMotorManager_servo_serial(port = '/dev/ttyUSB1', motor_params=Servo_Params_Serial['AK80-9']) as dev2:
+        loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
+        dev1.set_zero_position()
+        dev2.set_zero_position()
+        dev1.update()
+        dev2.update()
+
+        dev1.enter_duty_cycle_control()
+        dev2.enter_duty_cycle_control()
+        for t in loop:
+            dev1.set_duty_cycle_percent(duty)
+            dev2.set_duty_cycle_percent(-duty)
+            dev1.update()
+            dev2.update()
+            print(f"\r {dev1} {dev2}", end='')
+
+

demos/servo_serial/demo_2DOF_servo_serial.py

@@ -1,12 +1,10 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
 vel = 1
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev1:
-    with TMotorManager_servo_serial(port = '/dev/ttyUSB1', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev2:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev1:
+    with TMotorManager_servo_serial(port = '/dev/ttyUSB1', motor_params=Servo_Params_Serial['AK80-9']) as dev2:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev1.enter_position_control()
         dev2.enter_idle_mode()

demos/servo_serial/demo_PD_current_servo_serial.py

@@ -1,8 +1,7 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
-import numpy as np
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
+import numpy as np
+
 current = 0
 
 Pdes = 0
@@ -11,7 +10,7 @@
 P = 2
 D = 0.3
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.02, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()
@@ -20,7 +19,7 @@
 
         for t in loop:
             Pdes = 3*np.sin(t)
-            cmd = P*(Pdes - dev.θ ) + D*(Vdes - dev.θd)
+            cmd = P*(Pdes - dev.position ) + D*(Vdes - dev.velocity)
             dev.current_qaxis = cmd
             dev.update()
             print(f"\r {dev}", end='')

demos/servo_serial/demo_PD_duty_servo_serial.py

@@ -1,5 +1,3 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 import numpy as np
@@ -12,7 +10,7 @@
 P = 0.1
 D = 0.001
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()

demos/servo_serial/demo_ROS2_node_servo_serial.py

@@ -0,0 +1,141 @@
+# Example ROS2 Node that can switch between control modes via ros topics 
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32
+from std_msgs.msg import Float32MultiArray
+
+from TMotorCANControl.servo_serial import *
+
+CONTROL_MODES = {
+    "POSITION": 0,
+    "VELOCITY": 1,
+    "CURRENT":  2,
+    "DUTY":     3,
+}
+
+class MotorNode(Node):
+    def __init__(self, device):
+        super().__init__('motor_node')
+
+        self.device = device
+        self.dev_name = "dev"
+        self.mode = 10   
+        self.motor_data = 0
+
+        self.device.set_zero_position()
+        self.device.update()
+
+        self.velocity_sub = self.create_subscription(
+            Float32,
+            f"{self.dev_name}/velocity",
+            self.velocity_cb,
+            10
+        )
+
+        self.duty_sub = self.create_subscription(
+            Float32,
+            f"{self.dev_name}/duty",
+            self.duty_cb,
+            10
+        )
+
+        self.position_sub = self.create_subscription(
+            Float32,
+            f"{self.dev_name}/position",
+            self.position_cb,
+            10
+        )
+
+        self.current_sub = self.create_subscription(
+            Float32,
+            f"{self.dev_name}/current",
+            self.current_cb,
+            10
+        )
+
+        self.output_pub = self.create_publisher(
+            Float32MultiArray,
+            f"{self.dev_name}/output",
+            10
+        )
+
+        self.timer = self.create_timer(0.01, self.timer_callback)
+
+    def timer_callback(self):
+        if self.mode == CONTROL_MODES["VELOCITY"]:
+            self.device.set_output_velocity_radians_per_second(self.motor_data)
+            self.device.update()
+        elif self.mode == CONTROL_MODES["DUTY"]:
+            self.device.set_duty_cycle_percent(self.motor_data)
+            self.device.update()
+        elif self.mode == CONTROL_MODES["POSITION"]:
+            self.device.set_output_angle_radians(self.motor_data)
+            self.device.update()
+        elif self.mode == CONTROL_MODES["CURRENT"]:
+            self.device.current_qaxis = self.motor_data
+            self.device.update()
+
+        output_angle = self.device.get_output_angle_radians()
+        output_velocity = self.device.get_output_velocity_radians_per_second()
+        output_acceleration = self.device.get_output_acceleration_radians_per_second_squared()
+        output_torque = self.device.get_output_torque_newton_meters()
+
+        output_msg = Float32MultiArray()
+        output_msg.data = [output_angle, output_velocity, output_acceleration, output_torque]
+        self.output_pub.publish(output_msg)
+
+    def velocity_cb(self, msg):
+
+        if self.mode != CONTROL_MODES["VELOCITY"]:
+            self.mode = CONTROL_MODES["VELOCITY"]
+            self.device.enter_velocity_control()
+            self.device.update()
+            print("vel")
+
+        self.motor_data = msg.data
+
+    def duty_cb(self, msg):
+
+        if self.mode != CONTROL_MODES["DUTY"]:
+            self.mode = CONTROL_MODES["DUTY"]
+            self.device.enter_duty_cycle_control()
+            self.device.update()
+            print("duty")
+            print(msg.data)
+
+        self.motor_data = msg.data
+
+    def position_cb(self, msg):
+
+        if self.mode != CONTROL_MODES["POSITION"]:
+            self.mode = CONTROL_MODES["POSITION"]
+            self.device.enter_position_control()
+            self.device.update()
+            print("position")
+
+        self.motor_data = msg.data
+
+    def current_cb(self, msg):
+
+        if self.mode != CONTROL_MODES["CURRENT"]:
+            self.mode = CONTROL_MODES["CURRENT"]
+            self.device.enter_current_control()
+            self.device.update()
+            print("current")
+
+        self.motor_data = msg.data
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    with TMotorManager_servo_serial(port = "/dev/ttyUSB0") as dev:
+        motor_node = MotorNode(device = dev)
+        rclpy.spin(motor_node)
+
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

demos/servo_serial/demo_current_step_servo_serial.py

@@ -1,11 +1,9 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
 current = 1
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()

demos/servo_serial/demo_duty_servo_serial.py

@@ -1,11 +1,9 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
 duty = 0.1
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()

demos/servo_serial/demo_idle_servo_serial.py

@@ -1,13 +1,7 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
-baud=921600
-port='/dev/ttyUSB0'
-motor_params = Servo_Params_Serial['AK80-9']
-
-with TMotorManager_servo_serial(port=port, baud=baud, motor_params=motor_params) as dev:
+with TMotorManager_servo_serial(port='/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.01, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()

demos/servo_serial/check_motor_connection_servo_serial.py → demos/servo_serial/demo_motor_connection_servo_serial.py

@@ -1,10 +1,6 @@
-from TMotorCANControl.servo_serial import TMotorManager_servo_serial
+from TMotorCANControl.servo_serial import *
 
-# CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK10-9'
-ID = 0
-
-with TMotorManager_servo_serial(motor_type=Type, motor_ID=ID) as dev:
+with TMotorManager_servo_serial(port= '/dev/ttyUSB0', motor_params=Servo_Params_Serial["AK80-9"]) as dev:
     if dev.check_connection():
         print("\nmotor is successfully connected!\n")
     else:

demos/servo_serial/demo_position_step_servo_serial.py

@@ -1,11 +1,10 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 import numpy as np
+
 pos = 0
 
-with TMotorManager_servo_serial(port='/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port='/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
 
         dev.set_zero_position()

demos/servo_serial/demo_position_tracking_servo_serial.py

@@ -1,11 +1,10 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 import numpy as np
+
 pos = 0
 
-with TMotorManager_servo_serial(port='/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port='/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
 
         dev.set_zero_position()

demos/servo_serial/demo_velocity_servo_serial.py

@@ -1,11 +1,9 @@
-from sys import path
-path.append("/home/pi/TMotorCANControl/src/")
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 
-vel = 1
+vel = 9
 
-with TMotorManager_servo_serial(port = '/dev/ttyUSB0', baud=961200, motor_params=Servo_Params_Serial['AK80-9']) as dev:
+with TMotorManager_servo_serial(port = '/dev/ttyUSB0', motor_params=Servo_Params_Serial['AK80-9']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.enter_velocity_control()
 

src/TMotorCANControl.egg-info/requires.txt

@@ -1,4 +1,4 @@
-numpy
 NeuroLocoMiddleware
-python-can>=4.0.0
+numpy
 pyserial>=3.5
+python-can>=4.0.0