モーター制御基板 PC側ソフト 参考

~
import serial
import time

flg_uart_err=0

ser = serial.Serial('/dev/ttyAMA0', '19200', timeout=0.1)

def send_command(send_data=None):
    ser.reset_output_buffer()
    send_data = send_data + '\n'
    send_data_b = send_data.encode('utf-8')
    ser.write(send_data_b)
    return True

def receive_command(chk_command=None, chk_data=None, retry= 10):
    for n in range(retry):
        ser.reset_input_buffer()
        receive_data = ser.readline().decode('utf-8')
        if len(receive_data) > 0:
            if receive_data == "uart,error":
                flg_uart_err = 1
                return None
            else:
                receive_data = receive_data.replace('\n','')
                receive_data_ls = receive_data.split(',')
                if chk_command != None and receive_data_ls[0] != chk_command:
                    return None
                if chk_data != None and receive_data_ls[1] != chk_data:
                    return None
                return receive_data_ls
    return None

def get_dist():
    command = "get_dist,0"
    send_command(command)
    receive_data_ls = receive_command(chk_command='get_dist')
    if receive_data_ls is None:
        return None
    return int(receive_data_ls[1])

def set_dist_angle(angle=0):
    command = "set_dist_angle," + str(angle)
    send_command(command)
    receive_data_ls = receive_command(chk_command='set_dist_angle',chk_data=str(angle))
    if receive_data_ls is None:
        return None
    return True

def serch_dist(split_angle=30):
    dist_ls = []
    mes_angle = -90
    delay = (split_angle / 60) * 0.15
    while mes_angle <= 90:
        set_dist_angle(angle=mes_angle)
        if mes_angle <= -90:
            time.sleep(0.3)
            dist = get_dist()
        else:
            time.sleep(delay)
            dist = get_dist()
        if dist is None:
            return None
        else:
            dist_ls.append(dist)
            mes_angle += split_angle
    set_dist_angle(angle=0)
    time.sleep(0.2)
    set_dist_angle(angle=999)
    max_p = dist_ls.index(max(dist_ls))
    max_angle = int(-90 + max_p * split_angle)
    dist_ls.append(max_p)
    dist_ls.append(max_angle)
    return dist_ls

def drive_motor(rps=0, stop_rot=0, enable_break=False):
    if enable_break == True:
        if set_drive_break(1) == True:
            return True
        else:
            return None
    if enable_break != True:
        if set_drive_break(0) == None:
            return None
    if clr_drive_rot_count() == None:
        return None
    if set_drive_stop_rot(rot=stop_rot) == None:
        return None
    if clr_drive_auto_break() == None:
        return None
    if set_drive_rps(rps=rps) == None:
        return None
    return True

def set_drive_pwm_freq(freq=100):
    if freq <= 10:
        freq = 10
    if freq >= 2500:
        freq = 2500
    set_clkdiv = 2500 / freq
    if set_drive_pwm_warp(warp=49999) == None:
        return None
    if set_drive_pwm_clkdiv(clkdiv=set_clkdiv) == None:
        return None
    return True

def set_drive_break(enable_break=0):
    command = 'enable_break,' + str(enable_break)
    send_command(command)
    receve_data_ls = receive_command(chk_command='enable_break',chk_data=str(enable_break))
    if receve_data_ls != None:
        return True
    return None

def set_drive_rps(rps=0):
    command = 'set_rps,' + str(rps)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_rps',chk_data=str(rps))
    if receve_data_ls != None:
        return True
    return None

def set_drive_stop_rot(rot=0):
    command = 'set_stop_rot,' + str(rot)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_stop_rot',chk_data=str(rot))
    if receve_data_ls != None:
        return True
    return None

def set_drive_pwm_clkdiv(clkdiv=25):
    command = 'set_drive_pwm_clkdiv,' + str(clkdiv)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_drive_pwm_clkdiv',chk_data=str(clkdiv))
    if receve_data_ls != None:
        return True
    return None

def set_drive_pwm_warp(warp=49999):
    command = 'set_drive_pwm_warp,' + str(warp)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_drive_pwm_warp',chk_data=str(warp))
    if receve_data_ls != None:
        return True
    return None

def chk_auto_stop_drive():
    send_command('get_auto_break_flg,0')
    receve_data_ls = receive_command(chk_command='get_auto_break_flg',chk_data='1')
    if receve_data_ls != None:
        return True
    return None

def clr_drive_rot_count():
    send_command('clr_count_rot,0')
    receve_data_ls = receive_command(chk_command='clr_count_rot',chk_data='0')
    if receve_data_ls != None:
        return True
    return None

def get_drive_rps():
    send_command('get_rps,0')
    receve_data_ls = receive_command(chk_command='get_rps')
    if receve_data_ls != None:
        return receve_data_ls[1]
    return None

def get_drive_rps_ave():
    send_command('get_rps_ave,0')
    receve_data_ls = receive_command(chk_command='get_rps_ave')
    if receve_data_ls != None:
        return receve_data_ls[1]
    return None

def get_drive_duty():
    send_command('get_drive_duty,0')
    receve_data_ls = receive_command(chk_command='get_drive_duty')
    if receve_data_ls != None:
        return receve_data_ls[1]
    return None

def get_rot_count():
    send_command('get_count_rot,0')
    receve_data_ls = receive_command(chk_command='get_count_rot')
    if receve_data_ls != None:
        return receve_data_ls[1]
    return None

def clr_drive_auto_break():
    send_command('clr_auto_break_flg,0')
    receve_data_ls = receive_command(chk_command='clr_auto_break_flg',chk_data='0')
    if receve_data_ls != None:
        return True
    return None

def chk_trq_err():
    send_command('get_trq_err,0')
    receve_data_ls = receive_command(chk_command='get_trq_err',chk_data='1')
    if receve_data_ls != None:
        return True
    return None

def clr_trq_err():
    send_command('clr_trq_err,0')
    receve_data_ls = receive_command(chk_command='clr_trq_err',chk_data='0')
    if receve_data_ls != None:
        return True
    return None

def set_steering(angle=0):
    command = 'set_steering,' + str(angle)
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_steering',chk_data=str(angle))
    if receve_data_ls != None:
        return True
    return None

def chk_ready():
    send_command('get_total_fault,0')
    receve_data_ls = receive_command(chk_command='get_total_fault',chk_data=str(0))
    if receve_data_ls != None:
        return True
    else:
        return None

def get_temp():
    send_command('get_temp,0')
    receve_data_ls = receive_command(chk_command='get_temp')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_temp_max():
    send_command('get_temp_max,0')
    receve_data_ls = receive_command(chk_command='get_temp_max')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_temp_min():
    send_command('get_temp_min,0')
    receve_data_ls = receive_command(chk_command='get_temp_min')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vsys():
    send_command('get_Vsys,0')
    receve_data_ls = receive_command(chk_command='get_Vsys')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vsys_max():
    send_command('get_Vsys_max,0')
    receve_data_ls = receive_command(chk_command='get_Vsys_max')
    if receve_data_ls != None:
        return float(receve_data_ls[1])


def get_Vsys_min():
    send_command('get_Vsys_min,0')
    receve_data_ls = receive_command(chk_command='get_Vsys_min')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vbat():
    send_command('get_Vbat,0')
    receve_data_ls = receive_command(chk_command='get_Vbat')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vbat_max():
    send_command('get_Vbat_max,0')
    receve_data_ls = receive_command(chk_command='get_Vbat_max')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_Vbat_min():
    send_command('get_Vbat_min,0')
    receve_data_ls = receive_command(chk_command='get_Vbat_min')
    if receve_data_ls != None:
        return float(receve_data_ls[1])

def get_pid_kp():
    send_command('get_pid_kp,0')
    receve_data_ls = receive_command(chk_command='get_pid_kp')
    if receve_data_ls != None:
        return receve_data_ls[1]

def get_pid_ki():
    send_command('get_pid_ki,0')
    receve_data_ls = receive_command(chk_command='get_pid_ki')
    if receve_data_ls != None:
        return receve_data_ls[1]

def get_pid_kd():
    send_command('get_pid_kd,0')
    receve_data_ls = receive_command(chk_command='get_pid_kd')
    if receve_data_ls != None:
        return receve_data_ls[1]

def set_pid_kp(kp=2):
    kp = '{:.6f}'.format(kp)
    command = 'set_pid_kp,' + kp
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_pid_kp',chk_data=kp)
    if receve_data_ls != None:
        return True

def set_pid_ki(ki=0.1):
    ki = '{:.6f}'.format(ki)
    command = 'set_pid_ki,' + ki
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_pid_ki',chk_data=ki)
    if receve_data_ls != None:
        return True

def set_pid_kd(kd=0.1):
    kd = '{:.6f}'.format(kd)
    command = 'set_pid_kd,' + kd
    send_command(command)
    receve_data_ls = receive_command(chk_command='set_pid_kd',chk_data=kd)
    if receve_data_ls != None:
        return True

def chk_condition():
    print(chk_ready())
    print(get_temp())
    print(get_temp_max())
    print(get_temp_min())
    print(get_Vbat())
    print(get_Vbat_max())
    print(get_Vbat_min())
    print(get_Vsys())
    print(get_Vsys_max())
    print(get_Vsys_min())
    return True




ser.close()

~