add "readmotion" "writeMotionPosition" command

.gitignore

@@ -88,3 +88,6 @@ ENV/
 
 # Rope project settings
 .ropeproject
+
+# PyCharm IDE
+.idea/

examples/usb-teaching-control.py

@@ -8,6 +8,7 @@
 import signal
 import sys
 import os
+import math
 import pathlib
 from time import sleep
 
@@ -19,6 +20,8 @@
 from pykeigan import utils
 
 
+REC_NUMBER=1
+
 """
 ----------------------
 モーター接続し、各種情報の取得
@@ -68,18 +71,53 @@ def play_teaching(index):
     dev.set_led(1, 100, 100, 100)
     return True
 
+def read_comp_cb(index,motion_value):
+    print("read motion data "+str(index)+" >>>>>>>>>>>>>>")
+    print(motion_value)
+    print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>")
+
+def read_motion_exec(index):
+    dev.read_motion(index,read_comp_cb)
+
+def write_motion_position_exec(index):
+    print("Write Test motion>>>>" )
+    dev.enable_action()
+    dev.erase_motion(index)
+    sleep(2)
+    dev.prepare_teaching_motion(index,0)
+    sleep(1)
+    amp = math.pi/2 #deg
+    len=500
+    for i in range(len):
+        pos=amp*math.cos(2*math.pi*i/500)
+        dev.write_motion_position(pos)
+        sleep(0.02)
+        print('\r Write Test motion>>>>{0}  {1}/{2}'.format(pos,i,len), end='')
+    pass
+    sleep(0.5)
+    dev.stop_teaching_motion()
+    sleep(1)
+    print("")
+    print("Write Test motion>>>>comp")
+    play_teaching(REC_NUMBER)
+
 """
 Exit with key input
 """
 
+
 sleep(0.5)
 while True:
     print("---------------------------------------")
-    inp = input('Command input > Rec:[r] Replay:[p] Exit:[Other key] >>')
+    inp = input('Command input > Rec:[r] Replay:[p]  Write Test motion:[w] Read motion:[m] Exit:[Other key] >>')
     if inp == 'r':
-        rec_and_play_teaching(0)
+        rec_and_play_teaching(REC_NUMBER)
     elif inp == 'p':
-        play_teaching(0)
+        play_teaching(REC_NUMBER)
+    elif inp == 'w':
+        write_motion_position_exec(REC_NUMBER)
+    elif inp == 'm':
+        read_motion_exec(REC_NUMBER)
     elif inp !=None:
         dev.set_led(1, 100, 100, 100)
         dev.disable_action()

pykeigan/controller.py

@@ -28,7 +28,8 @@ def error_codes(self):
         return {0x00:"KM_SUCCESS", 0x03:"KM_ERROR_INTERNAL", 0x04:"KM_ERROR_NO_MEM", 0x05:"KM_ERROR_NOT_FOUND", 0x06:"KM_ERROR_NOT_SUPPORTED", 0x07:"KM_ERROR_INVALID_PARAM", 0x08:"KM_ERROR_INVALID_STATE", 0x09:"KM_ERROR_INVALID_LENGTH", 0x0B:"KM_ERROR_INVALID_DATA", 0x0D:"KM_ERROR_TIMEOUT", 0x0E:"KM_ERROR_NULL", 0x0F:"KM_ERROR_FORBIDDEN", 0x10:"KM_ERROR_INVALID_ADDR",0x11:"KM_ERROR_BUSY",0x14:"KM_ERROR_MOTOR_DISABLED",0x15:"KM_ERROR_MOTOR_OVER_TEMPERATURE"}
     @property
     def command_names(self):
-        return {0x02:"set_max_speed",0x03:"set_min_speed",0x05:"set_curve_type",0x07:"set_acc",0x08:"set_dec",0x0e:"set_max_torque",0x16:"set_teaching_interval",0x17:"set_playback_interval",0x18:"set_qcurrent_p",0x19:"set_qcurrent_i",0x1A:"set_qcurrent_d",0x1B:"set_speed_p",0x1C:"set_speed_i",0x1D:"set_speed_d",0x1E:"set_position_p",0x1F:"set_position_i",0x20:"set_position_d",0x21:"set_pos_control_threshold",0x22:"reset_all_pid",0x2C:"set_motor_measurement_interval",0x2D:"set_motor_measurement_settings",0x2E:"set_interface",0x3A:"set_own_color",0x3C:"set_imu_measurement_interval",0x3D:"set_imu_measurement_settings",0x40:"read_register",0x41:"save_all_registers",0x46:"read_device_name",0x47:"read_device_info",0x4E:"reset_register",0x4F:"reset_all_registers",0x50:"disable_action",0x51:"enable_action",0x58:"set_speed",0x5A:"preset_position",0x5B:"get_position_offset",0x60:"run_forward",0x61:"run_reverse",0x62:"run_at_velocity",0x65:"move_to_pos",0x66:"move_to_pos",0x67:"move_by_dist",0x68:"move_by_dist",0x6C:"free_motor",0x6D:"stop_motor",0x72:"hold_torque",0x75:"move_to_pos_until_arrival",0x76:"move_to_pos_until_arrival",0x77:"move_by_pos_until_arrival",0x78:"move_by_pos_until_arrival",0x81:"start_doing_taskset",0x82:"stop_doing_taskset",0x85:"start_playback_motion",0x86:"prepare_playback_motion",0x87:"start_playback_motion_from_prep",0x88:"stop_playback_motion",0x90:"pause_queue",0x91:"resume_queue",0x92:"wait_queue",0x94:"erase_task",0x95:"clear_queue",0x9A:"get_status",0xA0:"start_recording_taskset",0xA2:"stop_recording_taskset",0xA3:"erase_taskset",0xA4:"erase_all_tasksets",0xA5:"set_taskset_name",0xA6:"read_taskset_info",0xA7:"read_taskset_usage",0xA9:"start_teaching_motion",0xAA:"prepare_teaching_motion",0xAB:"start_teaching_motion_from_prep",0xAC:"stop_teaching_motion",0xAD:"erase_motion",0xAE:"erase_all_motions",0xAF:"set_motion_name",0xB0:"read_motion_info",0xB1:"read_motion_usage",0xB4:"get_motor_measuremet",0xB5:"get_imu_measurement",0xE0:"set_led",0xE6:"enable_motor_measurement",0xE7:"disable_motor_measurement",0xEA:"enable_imu_measurement",0xEB:"disable_imu_measurement",0xF0:"reboot"}
+        return {0x00:"unknown",0x02:"set_max_speed",0x03:"set_min_speed",0x05:"set_curve_type",0x07:"set_acc",0x08:"set_dec",0x0e:"set_max_torque",0x16:"set_teaching_interval",0x17:"set_playback_interval",0x18:"set_qcurrent_p",0x19:"set_qcurrent_i",0x1A:"set_qcurrent_d",0x1B:"set_speed_p",0x1C:"set_speed_i",0x1D:"set_speed_d",0x1E:"set_position_p",0x1F:"set_position_i",0x20:"set_position_d",0x21:"set_pos_control_threshold",0x22:"reset_all_pid",0x2C:"set_motor_measurement_interval",0x2D:"set_motor_measurement_settings",0x2E:"set_interface",0x3A:"set_own_color",0x3C:"set_imu_measurement_interval",0x3D:"set_imu_measurement_settings",0x40:"read_register",0x41:"save_all_registers",0x46:"read_device_name",0x47:"read_device_info",0x4E:"reset_register",0x4F:"reset_all_registers",0x50:"disable_action",0x51:"enable_action",0x58:"set_speed",0x5A:"preset_position",0x5B:"get_position_offset",0x60:"run_forward",0x61:"run_reverse",0x62:"run_at_velocity",0x65:"move_to_pos",0x66:"move_to_pos",0x67:"move_by_dist",0x68:"move_by_dist",0x6C:"free_motor",0x6D:"stop_motor",0x72:"hold_torque",0x75:"move_to_pos_until_arrival",0x76:"move_to_pos_until_arrival",0x77:"move_by_pos_until_arrival",0x78:"move_by_pos_until_arrival",0x81:"start_doing_taskset",0x82:"stop_doing_taskset",0x85:"start_playback_motion",0x86:"prepare_playback_motion",0x87:"start_playback_motion_from_prep",0x88:"stop_playback_motion",0x90:"pause_queue",0x91:"resume_queue",0x92:"wait_queue",0x94:"erase_task",0x95:"clear_queue",0x9A:"get_status",0xA0:"start_recording_taskset",0xA2:"stop_recording_taskset",0xA3:"erase_taskset",0xA4:"erase_all_tasksets",0xA5:"set_taskset_name",0xA6:"read_taskset_info",0xA7:"read_taskset_usage",0xA9:"start_teaching_motion",0xAA:"prepare_teaching_motion",0xAB:"start_teaching_motion_from_prep",0xAC:"stop_teaching_motion",0xAD:"erase_motion",0xAE:"erase_all_motions",0xAF:"set_motion_name",0xB0:"read_motion_info",0xB1:"read_motion_usage",0xB4:"get_motor_measuremet",0xB5:"get_imu_measurement"
+            ,0xB7:"read_motion" ,0xB8:"write_motion_position",0xE0:"set_led",0xE6:"enable_motor_measurement",0xE7:"disable_motor_measurement",0xEA:"enable_imu_measurement",0xEB:"disable_imu_measurement",0xF0:"reboot"}
     # Settings
     def set_max_speed(self,max_speed,identifier=b'\x00\x00',crc16=b'\x00\x00'):
         """
@@ -607,6 +608,31 @@ def erase_all_motions(self,identifier=b'\x00\x00',crc16=b'\x00\x00'):
         command=b'\xAE'
         self._run_command(command+identifier+crc16,'motor_tx')
 
+    def read_motion(self,index,read_comp_cb,identifier=b'\x00\x00',crc16=b'\x00\x00'):
+        """
+        Externally output the motion stored by direct teaching. (Motor farm ver>=2.0)
+        It is valid only for a wired connection(USB).
+          callback signature
+            read_comp_cb(motion_index,motion_value[])
+        """
+        command=b'\xB7'
+        values = uint16_t2bytes(index)
+        self._run_command(command + identifier + values + crc16, 'motor_tx')
+        self.on_read_motion_read_comp_cb=read_comp_cb
+        return True
+
+    def write_motion_position(self,position,identifier=b'\x00\x00',crc16=b'\x00\x00'):
+        """
+        Record one coordinate of movement from the outside. (Motor farm ver>=2.0)
+        It is valid only for a wired connection(USB).
+            ※ This command can not be accepted unless it is in prepareTeaching state.What to do after running "prepareTeachingMotion"
+            ※ This command records coordinates one by one. If one record is made, it will be waiting to record the next coordinate.
+            ※ After recording coordinates with this command, recording can not be completed normally unless stopTeaching is executed.
+        """
+        command=b'\xB8'
+        values = float2bytes(position)
+        self._run_command(command + identifier + values + crc16, 'motor_tx')
+
     # LED
     def set_led(self,ledState,red,green,blue,identifier=b'\x00\x00',crc16=b'\x00\x00'):
         """
@@ -795,3 +821,6 @@ def read_status(self):
 
     def _read_setting_value(self, comm):#dummy
         pass
+    def _read_motion_value(self):#dummy
+        pass
+

pykeigan/usbcontroller.py

@@ -19,11 +19,13 @@ def __init__(self, port='/dev/ttyUSB0',debug_mode=False):
         self.__motor_measurement_value = None
         self.__imu_measurement_value = None
         self.__motor_log_value = None
+        self.__read_motion_value = []
         self.port = port
         self.serial = serial.Serial(port, 115200, 8, 'N', 1, None, False, True)
         self.on_motor_measurement_value_cb = False
         self.on_motor_imu_measurement_cb = False
         self.on_motor_connection_error_cb = False
+        self.on_read_motion_read_comp_cb = False
         self.on_motor_log_cb = False
         self.set_interface(self.interface_type['USB'] + self.interface_type['BTN'])
         self.start_auto_serial_reading()
@@ -189,7 +191,7 @@ def __serialdataParse(self, byte_array):
             if (callable(self.on_motor_imu_measurement_cb)):
                 self.on_motor_imu_measurement_cb(self.__imu_measurement_value)
             return True
-        elif datatype == 0x40:
+        elif datatype == 0x40: #Register infomations
             float_value_comms = [0x02, 0x03, 0x07, 0x08, 0x0E, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D, 0x1E, 0x1F, 0x20,
                                  0x21, 0x5B]
             comm = payload[2]
@@ -221,16 +223,37 @@ def __serialdataParse(self, byte_array):
             else:
                 return False
         elif datatype == 0xBE:  # command log
-            self.__motor_log_value={'command_names':self.command_names[payload[2]],'error_codes':self.error_codes[bytes2uint16_t(payload[6:8])]}
+            self.__motor_log_value={'command_names':self.command_names[payload[2]],'error_codes':self.error_codes[bytes2uint32_t(payload[3:7])]}
             if (callable(self.on_motor_log_cb)):
                 self.on_motor_log_cb(self.__motor_log_value)
             if self.DebugMode:
                 print(self.__motor_log_value['command_names'],self.__motor_log_value['error_codes'])
             return True
+        elif datatype == 0xB7: #Position coordinates of "readMotion" (Motor farm ver>=2.0)
+            motion_index=bytes2uint16_t(payload[0:2])
+            total_pos_len = bytes2uint32_t(payload[2:6])
+            payload_fast_pos_idx=bytes2uint32_t(payload[6:10])
+            payload_pos_len=bytes2uint32_t(payload[10:14])
+            ar=[]
+            for i in range(payload_pos_len):
+                try:
+                    ar.append(bytes2float(payload[14+(i*4):18+(i*4)]))
+                except:
+                    break
+            try:
+                del self.__read_motion_value[payload_fast_pos_idx:]
+            except:
+                pass
+            self.__read_motion_value.extend(ar)
+
+            if(len(self.__read_motion_value) >= total_pos_len): #Completed receiving all data
+                if (callable(self.on_read_motion_read_comp_cb)):
+                    self.on_read_motion_read_comp_cb(motion_index, self._read_motion_value())
+            return True
         else:  # Unknown data
             return False
 
-    def _read_setting_value(self, comm, validation_threshold=1.0):
+    def _read_setting_value(self, comm, validation_threshold=1.0):#Register infomation
         float_value_comms = [0x02, 0x03, 0x07, 0x08, 0x0E, 0x18, 0x19, 0x1A, 0x1B, 0x1C, 0x1D, 0x1E, 0x1F, 0x20, 0x21,
                              0x5B]
         valid_comms = [0x05, 0x3A, 0x46, 0x47, 0x9A]
@@ -273,9 +296,12 @@ def __read_measurement_value(self, comm, validation_threshold=1.0):
             if comm == 0xB5:
                 raise ValueError("No data within ", validation_threshold, " sec.")
 
+    def _read_motion_value(self):
+        return self.__read_motion_value
 
     def read_motor_measurement(self):
         return self.__read_measurement_value(0xB4)
 
     def read_imu_measurement(self):
         return self.__read_measurement_value(0xB5)
+

pykeigan/utils.py

@@ -40,10 +40,10 @@ def uint32_t2bytes(uint32_value):
     return struct.pack("BBBB",val1,val2,val3,val4)
 
 def bytes2uint32_t(ba):
-    return struct.unpack("BBBB",ba)[0]
+    return struct.unpack(">I",ba)[0]
 
 def bytes2uint16_t(ba):
-    return struct.unpack("BB",ba)[0]
+    return struct.unpack(">H", ba)[0]
 
 def bytes2uint8_t(ba):
     return struct.unpack("B",ba)[0]