Corrected temperature reading from CAN in MIT mode

demos/mit_can/check_motor_connection_mit_can.py

@@ -1,12 +1,12 @@
 from TMotorCANControl.mit_can import TMotorManager_mit_can
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK80-9'
+Type = 'AK60-6'
 ID = 1
 
 with TMotorManager_mit_can(motor_type=Type, motor_ID=ID) as dev:
     if dev.check_can_connection():
         print("\nmotor is successfully connected!\n")
     else:
         print("\nmotor not connected. Check dev power, network wiring, and CAN bus connection.\n")
-
+

demos/mit_can/demo_current_chirp_mit_can.py

@@ -3,14 +3,14 @@
 from TMotorCANControl.mit_can import TMotorManager_mit_can
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK80-9'
+Type = 'AK60-6'
 ID = 1
 
 
 def chirp_demo(dev, amp=1.0, dt=0.001):
     print("Chirping ActPackA. Press CTRL-C to finish.")
     chirp = Chirp(250, 25, 1)
-    dev.set_set_current_gains()
+    dev.set_current_gains()
 
     print("Starting current chirp demo. Press ctrl+C to quit.")
 

demos/mit_can/demo_full_state_feedback_mit_can.py

@@ -5,19 +5,19 @@
 from TMotorCANControl.mit_can import TMotorManager_mit_can
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK80-9'
+Type = 'AK60-6'
 ID = 1
 
 def full_state_feedback(dev):
     dev.set_zero_position() # has a delay!
     time.sleep(1.5)
-    dev.set_impedance_gains_real_unit_full_state_feedback(K=10,B=1)
+    dev.set_impedance_gains_real_unit_full_state_feedback(K=1,B=0.2)
     chirp = Chirp(250, 200, 0.5)
 
     print("Starting full state feedback demo. Press ctrl+C to quit.")
 
     loop = SoftRealtimeLoop(dt = 0.001, report=True, fade=0)
-    amp = 1.0
+    amp = 00
 
     for t in loop:
         dev.update()

demos/mit_can/demo_idle_mit_can.py

@@ -3,7 +3,7 @@
 from TMotorCANControl.mit_can import TMotorManager_mit_can
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK80-9'
+Type = 'AK60-6'
 ID = 1
 
 def read_only(dev):

demos/mit_can/demo_position_step_mit_can.py

@@ -4,7 +4,7 @@
 from TMotorCANControl.mit_can import TMotorManager_mit_can
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK80-9'
+Type = 'AK60-6'
 ID = 1
 
 def position_step(dev):
@@ -26,4 +26,4 @@ def position_step(dev):
 
 if __name__ == '__main__':
     with TMotorManager_mit_can(motor_type=Type, motor_ID=ID) as dev:
-        position_step(dev)
+        position_step(dev)

demos/mit_can/demo_position_tracking_mit_can.py

@@ -2,16 +2,16 @@
 import numpy as np
 import time
 from TMotorCANControl.mit_can import TMotorManager_mit_can
+from math import pi
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK80-9'
+Type = 'AK60-6'
 ID = 1
 
 def position_tracking(dev):
     dev.set_zero_position() # has a delay!
     time.sleep(1.5)
-    dev.set_impedance_gains_real_unit(K=10,B=0.5)
-
+    dev.set_impedance_gains_real_unit(K=2,B=0.5)
     print("Starting position tracking demo. Press ctrl+C to quit.")
 
     loop = SoftRealtimeLoop(dt = 0.01, report=True, fade=0)
@@ -20,7 +20,8 @@ def position_tracking(dev):
         if t < 1.0:
             dev.position = 0.0
         else:
-            dev.position = 0.5*np.sin(np.pi*t)
+            dev.position = 0.3*np.pi*np.sin(np.pi*t)
+        print(f"\r {dev}", end='')
 
     del loop
 

demos/mit_can/demo_speed_mit_can.py

@@ -3,13 +3,13 @@
 from TMotorCANControl.mit_can import TMotorManager_mit_can
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK80-9'
+Type = 'AK60-6'
 ID = 1
 
 def speed_step(dev):
     dev.set_zero_position()
     time.sleep(1.5) # wait for the motor to zero (~1 second)
-    dev.set_speed_gains(kd=3.0)
+    dev.set_speed_gains(kd=1.0)
 
     print("Starting speed step demo. Press ctrl+C to quit.")
     loop = SoftRealtimeLoop(dt = 0.01, report=True, fade=0)

demos/mit_can/demo_torque_mit_can.py

@@ -3,7 +3,7 @@
 from TMotorCANControl.mit_can import TMotorManager_mit_can
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK80-9'
+Type = 'AK60-6'
 ID = 1
 
 
@@ -19,11 +19,11 @@ def torque_step(dev):
         if t < 1.0:
             dev.torque = 0.0
         else:
-            dev.torque = 1.0
+            dev.torque = 0.2
 
     del loop
 
 
 if __name__ == '__main__':
     with TMotorManager_mit_can(motor_type=Type, motor_ID=ID) as dev:
-        torque_step(dev)
+        torque_step(dev)

demos/servo_can/check_motor_connection_servo_can.py

@@ -1,8 +1,8 @@
 from TMotorCANControl.servo_can import TMotorManager_servo_can
 
 # CHANGE THESE TO MATCH YOUR DEVICE!
-Type = 'AK10-9'
-ID = 0
+Type = 'AK60-6'
+ID = 1
 
 with TMotorManager_servo_can(motor_type=Type, motor_ID=ID) as dev:
     if dev.check_can_connection():

demos/servo_can/demo_PD_current_can.py

@@ -10,7 +10,7 @@
 P = 2
 D = 0.3
 
-with TMotorManager_servo_can(motor_type='AK80-9', motor_ID=0) as dev:
+with TMotorManager_servo_can(motor_type='AK60-6', motor_ID=0) as dev:
     loop = SoftRealtimeLoop(dt=0.002, report=True, fade=0.0)
     dev.set_zero_position()
 

demos/servo_can/demo_idle_servo_can.py

@@ -7,11 +7,13 @@
 from TMotorCANControl.servo_can import TMotorManager_servo_can
 import time
 
-with TMotorManager_servo_can(motor_type='AK80-9', motor_ID=0) as dev:
+with TMotorManager_servo_can(motor_type='AK60-6', motor_ID=1) as dev:
 
     loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
     dev.enter_idle_mode()
-    dev.set_zero_position()
+    # dev.set_zero_position()
+    # dev.enter_position_control()
     for t in loop:
+        # dev.set_output_angle_radians(0.1,0.1,0.5)
         dev.update()
         print("\r" + str(dev),end='')

demos/servo_can/demo_position_step_servo_can.py

@@ -9,12 +9,13 @@
 import numpy as np
 
 
-with TMotorManager_servo_can(motor_type='AK80-9', motor_ID=0) as dev:
+with TMotorManager_servo_can(motor_type='AK60-6', motor_ID=1) as dev:
 
     loop = SoftRealtimeLoop(dt=0.01, report=True, fade=0.0)
     dev.set_zero_position()
     dev.enter_position_control()
     for t in loop:
-        dev.position = 1
+        # dev.position = 1
+        dev.set_output_angle_radians(0.1,0.1,0.5)
         dev.update()
         print("\r" + str(dev),end='')

demos/servo_can/demo_position_tracking_servo_can.py

@@ -9,12 +9,12 @@
 import numpy as np
 
 
-with TMotorManager_servo_can(motor_type='AK80-9', motor_ID=0) as dev:
+with TMotorManager_servo_can(motor_type='AK60-6', motor_ID=1) as dev:
 
     loop = SoftRealtimeLoop(dt=0.01, report=True, fade=0.0)
     dev.set_zero_position()
     dev.enter_position_control()
     for t in loop:
-        dev.position = np.sin(t) # rad/s
+        # dev.position = np.sin(t) # rad/s
         dev.update()
         print("\r" + str(dev),end='')

demos/servo_serial/check_motor_connection_servo_serial.py

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

demos/servo_serial/demo_PD_current_servo_serial.py

@@ -11,19 +11,20 @@
 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', baud=961200, motor_params=Servo_Params_Serial['AK60-6']) as dev:
         loop = SoftRealtimeLoop(dt=0.02, report=True, fade=0.0)
         dev.set_zero_position()
         dev.update()
 
         dev.enter_current_control()
-
+        t_min1 = 0
         for t in loop:
-            Pdes = 3*np.sin(t)
-            cmd = P*(Pdes - dev.θ ) + D*(Vdes - dev.θd)
-            dev.current_qaxis = cmd
+            Pdes = 1*np.sin(t)
+            cmd = P*(Pdes - dev.position ) + D*(Vdes - dev.velocity)
+            dev.current_qaxis = -cmd
             dev.update()
-            print(f"\r {dev}", end='')
+            print(f"\r {dev}","T:",str(t-t_min1), end='')
+            t_min1 = t
 
 
 

demos/servo_serial/demo_idle_servo_serial.py

@@ -5,7 +5,7 @@
 
 baud=921600
 port='/dev/ttyUSB0'
-motor_params = Servo_Params_Serial['AK80-9']
+motor_params = Servo_Params_Serial['AK60-6']
 
 with TMotorManager_servo_serial(port=port, baud=baud, motor_params=motor_params) as dev:
         loop = SoftRealtimeLoop(dt=0.01, report=True, fade=0.0)

demos/servo_serial/demo_position_step_servo_serial.py

@@ -5,15 +5,15 @@
 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', baud=961200, motor_params=Servo_Params_Serial['AK60-6']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
 
         dev.set_zero_position()
         dev.update()
 
         dev.enter_position_control()
         for t in loop:
-            pos = 2*np.sin(t)
+            pos = 0.5*np.sin(t)
             dev.set_output_angle_radians(pos)
             dev.update()
             print(f"\r {dev}", end='')

demos/servo_serial/demo_position_tracking_servo_serial.py

@@ -3,17 +3,18 @@
 from TMotorCANControl.servo_serial import *
 from NeuroLocoMiddleware.SoftRealtimeLoop import SoftRealtimeLoop
 import numpy as np
+from math import pi 
 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', baud=961200, motor_params=Servo_Params_Serial['AK60-6']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
 
         dev.set_zero_position()
         dev.update()
 
         dev.enter_position_control()
         for t in loop:
-            pos = 2*np.sin(t)
+            pos = 0.5*np.sin(t)
             dev.set_output_angle_radians(pos)
             dev.update()
             print(f"\r {dev}", end='')

demos/servo_serial/demo_velocity_servo_serial.py

@@ -5,7 +5,7 @@
 
 vel = 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', baud=961200, motor_params=Servo_Params_Serial['AK60-6']) as dev:
         loop = SoftRealtimeLoop(dt=0.005, report=True, fade=0.0)
         dev.enter_velocity_control()
 

src/TMotorCANControl/mit_can.py

@@ -33,6 +33,8 @@
             'V_max' : 50.0,
             'T_min' : -18.0,
             'T_max' : 18.0,
+            'Temp_min' : 0,
+            'Temp_max' : 100,
             'Kp_min': 0.0,
             'Kp_max': 500.0,
             'Kd_min': 0.0,
@@ -52,6 +54,8 @@
             'V_max' : 50.0,
             'T_min' : -65.0,
             'T_max' : 65.0,
+            'Temp_min' : 0,
+            'Temp_max' : 100,
             'Kp_min': 0.0,
             'Kp_max': 500.0,
             'Kd_min': 0.0,
@@ -69,6 +73,8 @@
             'V_max' : 50.0,
             'T_min' : -15.0,
             'T_max' : 15.0,
+            'Temp_min' : 0,
+            'Temp_max' : 100,
             'Kp_min': 0.0,
             'Kp_max': 500.0,
             'Kd_min': 0.0,
@@ -86,6 +92,8 @@
             'V_max' : 50.0,
             'T_min' : -25.0,
             'T_max' : 25.0,
+            'Temp_min' : 0,
+            'Temp_max' : 100,
             'Kp_min': 0.0,
             'Kp_max': 500.0,
             'Kd_min': 0.0,
@@ -103,6 +111,8 @@
             'V_max' : 76.0,
             'T_min' : -12.0,
             'T_max' : 12.0,
+            'Temp_min' : 0,
+            'Temp_max' : 100,
             'Kp_min': 0.0,
             'Kp_max': 500.0,
             'Kd_min': 0.0,
@@ -120,6 +130,8 @@
             'V_max' : 8.0,
             'T_min' : -144.0,
             'T_max' : 144.0,
+            'Temp_min' : 0,
+            'Temp_max' : 100,
             'Kp_min': 0.0,
             'Kp_max': 500.0,
             'Kd_min': 0.0,
@@ -501,6 +513,8 @@ def parse_MIT_message(self, data, motor_type):
                                             MIT_Params[motor_type]['V_max'], 12)
         current = CAN_Manager.uint_to_float(current_uint, MIT_Params[motor_type]['T_min'], 
                                             MIT_Params[motor_type]['T_max'], 12)
+        temp = int(CAN_Manager.uint_to_float(temp, MIT_Params[motor_type]['Temp_min'], 
+                                            MIT_Params[motor_type]['Temp_max'], 8))
 
         if self.debug:
             print('  Position: ' + str(position))
@@ -1076,7 +1090,7 @@ def get_motor_torque_newton_meters(self):
     # Pretty stuff
     def __str__(self):
         """Prints the motor's device info and current"""
-        return self.device_info_string() + " | Position: " + '{: 1f}'.format(round(self.θ,3)) + " rad | Velocity: " + '{: 1f}'.format(round(self.θd,3)) + " rad/s | current: " + '{: 1f}'.format(round(self.i,3)) + " A | torque: " + '{: 1f}'.format(round(self.τ,3)) + " Nm"
+        return self.device_info_string() + " | Position: " + '{: 1f}'.format(round(self.position,3)) + " rad | Velocity: " + '{: 1f}'.format(round(self.velocity,3)) + " rad/s | current: " + '{: 1f}'.format(round(self.current_qaxis,3)) + " A | torque: " + '{: 1f}'.format(round(self.torque,3)) + " Nm | temperature: " + '{: 1f}'.format(round(self.temperature,3)) + "Degrees C"
 
     def device_info_string(self):
         """Prints the motor's ID and device type."""