import serial
import os
import time
import RPi.GPIO as GPIO
from serial.tools import list_ports
 
# gpio
# 1
IN1 = 26
IN2 = 21
EN1 = 19

# 4
IN7 = 27
IN8 = 17
EN4 = 13

# 2
IN3 = 24
IN4 = 23
EN2 = 18

# 3
IN5 = 20
IN6 = 16
EN3 = 12

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)

# 1
GPIO.setup(IN1,GPIO.OUT)
GPIO.setup(IN2,GPIO.OUT)
GPIO.setup(EN1,GPIO.OUT)

# 2
GPIO.setup(IN3,GPIO.OUT)
GPIO.setup(IN4,GPIO.OUT)
GPIO.setup(EN2,GPIO.OUT)

# 3
GPIO.setup(IN5,GPIO.OUT)
GPIO.setup(IN6,GPIO.OUT)
GPIO.setup(EN3,GPIO.OUT)

# 4
GPIO.setup(IN7,GPIO.OUT)
GPIO.setup(IN8,GPIO.OUT)
GPIO.setup(EN4,GPIO.OUT)

p1=GPIO.PWM(EN1,1024)
p1.start(0)

p2=GPIO.PWM(EN2,1024)
p2.start(0)

p3=GPIO.PWM(EN3,1024)
p3.start(0)

p4=GPIO.PWM(EN4,1024)
p4.start(0)

#获取端口列表
plist = list(serial.tools.list_ports.comports())
for port in plist:
    print('端口号：' + port[0] + '   端口名：' + port[1])
#打开端口
port = serial.Serial(port = '/dev/ttyAMA0', baudrate = 420000, bytesize = 8
                         , parity = 'N', stopbits = 1 , timeout= 1.0)
#校验初始化
lut = []
poly = 0xD5
idx = 0
for CNT in range(256):
    lut.append(0)
while idx<256:
    crc = idx
    shift = 0
    while shift<8:
        shift += 1
        if crc & 0x80:
            crc_temp = poly
        else:
            crc_temp = 0
        crc = (crc << 1) ^ crc_temp
    lut[idx] = crc & 0xff
    idx += 1
#主循环
while(True):
    time.sleep(0.01)

    #读端口
    data = port.read_all()

    if(len(data)>7):
        #校验
        len_data = data[1]
        data_idx = 2
        try:

            inCrc = data[2+len_data-1]

        except Exception as e:  
            # 捕获其他类型的异常  
            # print(f"An unexpected error occurred: {e}")
            continue
        crc = 0
        while len_data-1:
            try:
                crc = lut[crc ^ data[data_idx]]
                data_idx += 1
                len_data -= 1
            except Exception as e:  
                # 捕获其他类型的异常  
                # print(f"An unexpected error occurred: {e}")
                break  
        if (data[2] == 22) & (len(data) >= (len_data+2)) & (inCrc == crc):
            #解析,从地四个字节开始，每个字节占用11位
            ch1 = ((data[3]>>0) | (data[4]<<8)) & 0x07FF
            ch2 = ((data[4]>>3) | (data[5]<<5)) & 0x07FF
            ch3 = ((data[5]>>6) | (data[6]<<2) | (data[7]<<10)) & 0x07FF
            ch4 = ((data[7]>>1) | (data[8]<<7)) & 0x07FF
            ch5 = ((data[8]>>4) | (data[9]<<4)) & 0x07FF
    
            #结果显示(可不看)
            #清屏
            os.system('clear')
            #通道数值
            print("ch3->"+ str(ch3))

            if (ch3>910 and ch3<1080) :
                print("ch3中间")

                GPIO.output(IN1,GPIO.LOW)
                GPIO.output(IN2,GPIO.LOW)
                p1.ChangeDutyCycle(0) 

                GPIO.output(IN7,GPIO.LOW)
                GPIO.output(IN8,GPIO.LOW)
                p4.ChangeDutyCycle(0)
            elif(ch3 >1080 ):
                print("ch3前进")
                GPIO.output(IN1,GPIO.HIGH)
                GPIO.output(IN2,GPIO.LOW)
                p1.ChangeDutyCycle((ch3-1079)/10) 

                GPIO.output(IN7,GPIO.HIGH)
                GPIO.output(IN8,GPIO.LOW)
                p4.ChangeDutyCycle((ch3-1079)/10)

            elif(ch3 < 910):
                print("ch3后退")
                GPIO.output(IN2,GPIO.HIGH)
                GPIO.output(IN1,GPIO.LOW)
                p1.ChangeDutyCycle((1000-ch3)/10) 

                GPIO.output(IN8,GPIO.HIGH)
                GPIO.output(IN7,GPIO.LOW)
                p4.ChangeDutyCycle((1000-ch3)/10)

            print("ch2->"+ str(ch2))
            if (ch2>950 and ch2<1050) :
                print("ch2中间")
                GPIO.output(IN4,GPIO.LOW)
                GPIO.output(IN3,GPIO.LOW)
                p2.ChangeDutyCycle(0)

                GPIO.output(IN6,GPIO.LOW)
                GPIO.output(IN5,GPIO.LOW)
                p3.ChangeDutyCycle(0)
            elif(ch2 >1050 ):
                print("ch2前进")

                GPIO.output(IN3,GPIO.HIGH)
                GPIO.output(IN4,GPIO.LOW)
                p2.ChangeDutyCycle((ch2-1050)/10)

                GPIO.output(IN5,GPIO.HIGH)
                GPIO.output(IN6,GPIO.LOW)
                p3.ChangeDutyCycle((ch2-1050)/10)

            elif(ch2<950):
                print("ch2后退")
                GPIO.output(IN4,GPIO.HIGH)
                GPIO.output(IN3,GPIO.LOW)
                p2.ChangeDutyCycle((1000-ch2)/10)

                GPIO.output(IN6,GPIO.HIGH)
                GPIO.output(IN5,GPIO.LOW)
                p3.ChangeDutyCycle((1000-ch2)/10)