edgeboardAI 小车驱动实现

涉及的库函数

1.server_port edgeboardAI调用wobot接口库

2.cc.py 控制库

3.test.py主要 调用库 控制函数

#serial_port.py
import serial
import time

from threading import Lock

class Serial:
    def __init__(self):
        portx = "/dev/ttyUSB0"
        bps = 115200
        self.res = None
        self.serial = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)
        time.sleep(1)
    def write(self, data):
        lock = Lock()
        lock.acquire()
        try:
            self.serial.write(data)
            self.serial.flush()
            self.res = self.serial.readline()
        finally:
            lock.release()

    def read(self):
        return self.res

serial_connection = Serial()

------------------------------------------------------------------

#cc.py
from serial_port import serial_connection;

from ctypes import *
import time
import serial
import math

comma_head_01_motor = bytes.fromhex('77 68 06 00 02 0C 01 01')
comma_head_02_motor = bytes.fromhex('77 68 06 00 02 0C 01 02')
comma_head_03_motor = bytes.fromhex('77 68 06 00 02 0C 01 03')
comma_head_04_motor = bytes.fromhex('77 68 06 00 02 0C 01 04')

comma_head_21_motor = bytes.fromhex('77 68 06 00 02 0C 02 01')
comma_head_22_motor = bytes.fromhex('77 68 06 00 02 0C 02 02')
comma_head_23_motor = bytes.fromhex('77 68 06 00 02 0C 02 03')
comma_head_24_motor = bytes.fromhex('77 68 06 00 02 0C 02 04')

comma_trail = bytes.fromhex('0A')

class Cart:
    def __init__(self):
        self.velocity = 25
        self.Kx=0.85
        portx = "/dev/ttyUSB0"
        bps = 115200
        self.serial = serial.Serial(portx, int(bps), timeout=1, parity=serial.PARITY_NONE, stopbits=1)

        self.p = 0.8;
        self.full_speed = self.velocity
        self.slow_ratio = 0.97;
        self.min_speed = 20

    def stop(self):
        self.move([0, 0, 0, 0,0,0,0,0])

    def exchange(self, speed):#if and else
        if speed > 100:
            speed = 100
        elif speed < -100:
            speed = -100
        else:
            speed = speed
        return speed

    def move(self, speeds):
        left_front = -int(speeds[0]);
        right_front = int(speeds[1]);
        left_rear = -int(speeds[2]);
        right_rear = int(speeds[3]);
        
        m1 = -int(speeds[4]);
        m2 = int(speeds[5]);
        m3 = -int(speeds[6]);
        m4 = int(speeds[7]);

        self.min_speed = int(min(speeds))
        
        left_front=self.exchange(left_front)
        right_front = self.exchange(right_front)
        left_rear=self.exchange(left_rear)
        right_rear = self.exchange(right_rear)
        
        send_data_01_motor = comma_head_01_motor + left_front.to_bytes(1, byteorder='big', signed=True) + comma_trail
        send_data_02_motor = comma_head_02_motor + right_front.to_bytes(1, byteorder='big',signed=True) + comma_trail
        send_data_03_motor = comma_head_03_motor + left_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
        send_data_04_motor = comma_head_04_motor + right_rear.to_bytes(1, byteorder='big', signed=True) + comma_trail
        send_data_21_motor = comma_head_21_motor + m1.to_bytes(1, byteorder='big', signed=True) + comma_trail
        send_data_22_motor = comma_head_22_motor + m2.to_bytes(1, byteorder='big',signed=True) + comma_trail
        send_data_23_motor = comma_head_23_motor + m3.to_bytes(1, byteorder='big', signed=True) + comma_trail
        send_data_24_motor = comma_head_24_motor + m4.to_bytes(1, byteorder='big', signed=True) + comma_trail
        
        self.serial.write(send_data_01_motor)
        self.serial.write(send_data_02_motor)
        self.serial.write(send_data_03_motor)
        self.serial.write(send_data_04_motor)
        
        self.serial.write(send_data_21_motor)
        self.serial.write(send_data_22_motor)
        self.serial.write(send_data_23_motor)
        self.serial.write(send_data_24_motor)
        
    def right(self,CarSpeedControl=10):
        self.move([CarSpeedControl,0,CarSpeedControl,0,0,0,0,0])
        
    def left(self,CarSpeedControl=10):
        self.move([0,CarSpeedControl,0,CarSpeedControl,0,0,0,0])
        
    def run(self,CarSpeedControl=10):
        self.move([CarSpeedControl,CarSpeedControl,CarSpeedControl,CarSpeedControl,0,0,0,0])
        
    def back(self,CarSpeedControl=10):
        self.move([-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,-CarSpeedControl,0,0,0,0])
    
    def brake(self,CarSpeedControl=0):
        self.move([0,0,0,0,0,0,0,0])
        
cart = Cart()

-------------------------------------------------------------------------------------------

#test.py
import time
import struct
from serial_port import serial_connection
from cc import cart;
serial = serial_connection

class Button:
    def __init__(self, port, buttonstr):
        self.state = False
        self.port = port
        self.buttonstr = buttonstr
        port_str = '{:02x}'.format(port)
        self.cmd_data = bytes.fromhex('77 68 05 00 01 E1 {} 01 0A'.format(port_str))

    def clicked(self):
        serial.write(self.cmd_data)
        response = serial.read()
        buttοnclick="no"
        if len(response) == 9 and  response[5]==0xE1 and response[6]==self.port:
            button_byte=response[3:5]+bytes.fromhex('00 00')
            button_value=struct.unpack('             # print("%x"%button_value)
            if button_value>=0x1f1 and button_value<=0x1ff:
                buttοnclick="UP"
            elif button_value>=0x330 and button_value<=0x33f:
                buttonclick = "LEFT"
            elif button_value>=0x2ff and button_value<=0x30f:
                buttonclick = "DOWN"
            elif button_value>=0x2a0 and button_value<=0x2af:
                buttonclick = "RIGHT"
            else:
                buttonclick
        return self.buttonstr==buttonclick

class LimitSwitch:
    def __init__(self, port):
        self.state = False;
        self.port = port;
        self.state = True;
        port_str = '{:02x}'.format(port)
        # print (port_str)
        self.cmd_data = bytes.fromhex('77 68 04 00 01 DD {} 0A'.format(port_str))

    # print('77 68 04 00 01 DD {} 0A'.format(port_str))

    def clicked(self):
        serial.write(self.cmd_data);
        response = serial.read();  # 77 68 01 00 0D 0A
        if len(response) < 8 or response[4] != 0xDD or response[5] != self.port \
                or response[2] != 0x01:
            return False;
        state = response[3] == 0x01
        # print("state=",state)
        # print("elf.state=", self.state)
        clicked = False
        if state == True and self.state == True and clicked == False:
            clicked = True;
        if state == False and self.state == True and clicked == True:
            clicked = False
        # print('clicked=',clicked)
        return clicked

class UltrasonicSensor():
    def __init__(self, port):
        self.port = port
        port_str = '{:02x}'.format(port)
        self.cmd_data = bytes.fromhex('77 68 04 00 01 D1 {} 0A'.format(port_str))

    def read(self):
        serial.write(self.cmd_data)
        # time.sleep(0.01)
        return_data = serial.read()
        if len(return_data)<11 or return_data[7] != 0xD1 or return_data[8] != self.port:
            return None
        # print(return_data.hex())
        return_data_ultrasonic = return_data[3:7]
        ultrasonic_sensor = struct.unpack('         # print(ultrasonic_sensor)
        return int(ultrasonic_sensor)

class Servo:
    def __init__(self, ID):
        self.ID = ID
        self.ID_str = '{:02x}'.format(ID)

    def servocontrol(self, angle, speed):
        cmd_servo_data = bytes.fromhex('77 68 06 00 02 36') + bytes.fromhex(self.ID_str) + speed.to_bytes(1,
                                                                                                            byteorder='big', \
                                                                                                            signed=True) + angle.to_bytes(
            1, byteorder='big', signed=True) + bytes.fromhex('0A')

        # for i in range(0,2):
        serial.write(cmd_servo_data)
        #     time.sleep(0.3)

class Servo_pwm:
    def __init__(self, ID):
        self.ID = ID
        self.ID_str = '{:02x}'.format(ID)

    def servocontrol(self, angle, speed):
        cmd_servo_data = bytes.fromhex('77 68 06 00 02 0B') + bytes.fromhex(self.ID_str) + speed.to_bytes(1,
                                                                                                            byteorder='big', \
                                                                                                            signed=True) + angle.to_bytes(
            1, byteorder='big', signed=False) + bytes.fromhex('0A')
        # for i in range(0,2):
        serial.write(cmd_servo_data)
        # time.sleep(0.3)

class Light:
    def __init__(self, port):
        self.port = port
        self.port_str = '{:02x}'.format(port)

    def lightcontrol(self, which, Red, Green, Blue):  # 0代表全亮，其他值对应灯珠亮，1~4
        which_str = '{:02x}'.format(which)
        Red_str = '{:02x}'.format(Red)
        Green_str = '{:02x}'.format(Green)
        Blue_str = '{:02x}'.format(Blue)
        cmd_servo_data = bytes.fromhex('77 68 08 00 02 3B {} {} {} {} {} 0A'.format(self.port_str, which_str, Red_str \
                                                                                    , Green_str, Blue_str))
        serial.write(cmd_servo_data)

    def lightoff(self):
        cmd_servo_data1 = bytes.fromhex('77 68 08 00 02 3B 02 00 00 00 00 0A')
        cmd_servo_data2 = bytes.fromhex('77 68 08 00 02 3B 03 00 00 00 00 0A')
        cmd_servo_data = cmd_servo_data1 + cmd_servo_data2
        serial.write(cmd_servo_data)

class Motor_rotate:
    def __init__(self, port):
        self.port = port
        self.port_str = '{:02x}'.format(port)

    def motor_rotate(self, speed):
        cmd_servo_data = bytes.fromhex('77 68 06 00 02 0C 02') + bytes.fromhex(self.port_str) + \
                         speed.to_bytes(1, byteorder='big', signed=True) + bytes.fromhex('0A')
        serial.write(cmd_servo_data)

class Infrared_value:
    def __init__(self, port):
        port_str = '{:02x}'.format(port)
        self.cmd_data = bytes.fromhex('77 68 04 00 01 D4 {} 0A'.format(port_str))

    def read(self):
        serial.write(self.cmd_data)
        return_data = serial.read()
        if return_data[2] != 0x04:
            return None
        if return_data[3] == 0x0a:
            return None
        # print("**********************************")
        # print("return_data[3]=%x" % return_data[3])
        # print(type(return_data[3]))
        # print("return_data[4]=%x" % return_data[4])
        # print("return_data[5]=%x" % return_data[5])
        # print("return_data[6]=%x" % return_data[6])
        # print(return_data.hex())
        return_data_infrared = return_data[3:7]
        print(return_data_infrared)
        infrared_sensor = struct.unpack('         # print(ultrasonic_sensor)
        return infrared_sensor

class Buzzer:
    def __init__(self):
        self.cmd_data = bytes.fromhex('77 68 06 00 02 3D 03 02 0A')

    def rings(self):
        serial.write(self.cmd_data)
class Magneto_sensor:
    def __init__(self,port):
        self.port=port
        port_str = '{:02x}'.format(self.port)
        self.cmd_data = bytes.fromhex('77 68 04 00 01 CF {} 0A'.format(port_str))

    def read(self):
        serial.write(self.cmd_data)
        return_data = serial.read()
        # print("return_data=",return_data[8])
        if len(return_data) < 11 or return_data[7] != 0xCF or return_data[8] != self.port:
            return None
        # print(return_data.hex())
        return_data = return_data[3:7]
        mag_sensor = struct.unpack('         # print(ultrasonic_sensor)
        return int(mag_sensor)
class Test:
    pass

CarSpeedControl = 10 #init speed
c=cart

target = 20 # 微调的最佳 目标距离

def Distance_test():
    ultr_sensor=UltrasonicSensor(4)
    distance=ultr_sensor.read()
    return distance

def near_distance(distance):
    i = 0#Limit of adjustment times
    if distance > target+1:
        print("Move up to the right by a large margin and small left down retreat")
        while int(distance - target) != 0:#一直调整
            h = distance
            c.right()
            time.sleep(0.5) #偏向角15度左右
            c.run()
            time.sleep(0.5)
            c.left()
            time.sleep(0.75)
            c.back()
            time.sleep(1)
            distance = Distance_test()
            print(str(distance)+" CM")
        
            i = i + 1            
            if int(target - distance) > 0 or i>= 10 or h < distance:#防止其它因素导致 出现移动太过情况
                break

def away_distance(distance):
    i = 0#Limit of adjustment times
    if distance < target-1:
        print("Large left up advance and back down slightly to the right")
        while int(target - distance) != 0:#一直调整
            h = distance
            c.left()
            time.sleep(0.5) #偏向角15度左右
            c.run()
            time.sleep(0.5)
            c.right()
            time.sleep(0.75)
            c.back()
            time.sleep(1)
            distance = Distance_test()
            print(str(distance)+" CM")
            i = i + 1        
            if int(distance - target) > 0 or i>= 10 or h > distance:#防止其它因素导致 出现移动太过情况
                break

def control(x):
    global CarSpeedControl
    if str(x) == 'a' or str(x) == 'A':#Left
        c.left(CarSpeedControl)
        
    elif str(x) == 'd' or str(x) == 'D':#Right
        c.right(CarSpeedControl)
        
    elif str(x) == 'w' or str(x) == 'W':#Run
        c.run(CarSpeedControl)
        
    elif str(x) == 's' or str(x) == 'S':#Back
        c.back(CarSpeedControl)
        
    elif str(x) == 'j' or str(x) == 'J':#Accelerate
        CarSpeedControl = CarSpeedControl + 10
        if CarSpeedControl > 90:#防止堆栈溢出
            CarSpeedControl = 90
    elif str(x) == 'k' or str(x) == 'K':#Slow down
        CarSpeedControl = CarSpeedControl - 10
        if CarSpeedControl < 10:#防止堆栈溢出
            CarSpeedControl = 10
    elif str(x) == 'U' or str(x) == 'u':
        speed = CarSpeedControl #保存微调前速度        
        CarSpeedControl = 10#慢速微调

        print("Fine tuning starts...")#begin 微调开始 根据超声波测距 进行水平位移
        distance = Distance_test()
        print(str(distance)+" CM")
        p = 0
        while 1:
            distance = Distance_test()
            p = p + 1
            if p >= 3:#循环调3次
                break
            if distance > target+1:
                near_distance(distance)
            
                continue
            elif distance < target-1:
                away_distance(distance)
            
                continue
            break

        c.brake()

        print("Fine tuning ends.")#end
        
        CarSpeedControl = speed
    else:
        c.brake()
        

#from Serial_0305 import SerialAssistant
if __name__ == '__main__':
    while 1:
        scan = input("Key command: ")
        if str(scan) == 'p' or str(scan) == 'P':
            break
        control(scan)
    
    pass