import machine

import time

import ps2

#电机1转动方向控制引脚，设置18号、19号引脚为输出引脚

pin18 = machine.Pin(18, machine.Pin.OUT)

pin19 = machine.Pin(19, machine.Pin.OUT)

#电机1转速控制，5号引脚输出PWM到电机驱动板的ENA引脚上

pwm5=machine.PWM(machine.Pin(5), freq=1000, duty=0)

#电机2转动方向控制引脚，设置22号、23号引脚为输出引脚

pin22 = machine.Pin(22, machine.Pin.OUT)

pin23 = machine.Pin(23, machine.Pin.OUT)

#电机2转速控制，17号引脚输出PWM到电机驱动板的ENB引脚上

pwm17=machine.PWM(machine.Pin(17), freq=1000, duty=0)

#PS2遥控接收器参数设置

ps2ctl = ps2.PS2Controller(di_pin_no=26, do_pin_no=27, cs_pin_no=14, clk_pin_no=12)

ps2ctl.init()

while True:

    key_str= ps2ctl.read_once()   # 接收指令，收到的指令字符格式为 keys:UP,RIGHT: pos(lx,ly):0,-1: pos(rx,ry): 0,-1:

    key_list= key_str.split(':')  # 用冒号来将字符串进行分割，写入数组key_list中，key_list[1]值为输入的按键，key_list[3]值为左摇杆坐标，key_list[5]值为右摇杆坐标

    if len(key_list)>=6 :

        #获取左摇杆的位置

        leftJoystickX = int( key_list[3].split(',')[0] )

        leftJoystickY = int( key_list[3].split(',')[1] )

        #打印获取到的左摇杆位置

        print( "左摇杆X方向值："+str(leftJoystickX) )

        print( "左摇杆Y方向值："+str(leftJoystickY) )

        command = "" #FORWARD，BACK，LEFT，RIGHT，STOP

        speed = 1023 #速度

        if leftJoystickY<0:

            command = "FORWARD"

        elif leftJoystickY>0:

            command = "BACK"

        elif leftJoystickY==0:

            command = "STOP"

        speed = int( abs(leftJoystickY) * (1023/128) )

        if leftJoystickX<-100:

            command = "LEFT"

        elif leftJoystickX>100:

            command = "RIGHT"

        if command=="FORWARD": #“前进”

            pwm5.duty(speed) #设置电机1速度。

            pwm17.duty(speed) #设置电机2速度。

            #电机1，向前转

            pin18.value(1)

            pin19.value(0)

            #电机2，向前转

            pin22.value(1)

            pin23.value(0)

        elif command=="BACK": #“后退”

            pwm5.duty(speed) #设置电机1速度。

            pwm17.duty(speed) #设置电机2速度。

            #电机1，向后转

            pin18.value(0)

            pin19.value(1)

            #电机2，向后转

            pin22.value(0)

            pin23.value(1)

        elif command=="LEFT": #“左转”

            pwm5.duty(1023) #设置电机1速度。

            pwm17.duty(1023) #设置电机2速度。

            #电机1，向后转

            pin18.value(0)

            pin19.value(1)

            #电机2，向前转

            pin22.value(1)

            pin23.value(0)

        elif command=="RIGHT": #“右转”

            pwm5.duty(1023) #设置电机1速度。

            pwm17.duty(1023) #设置电机2速度。

            #电机1，向前转

            pin18.value(1)

            pin19.value(0)

            #电机2，向后转

            pin22.value(0)

            pin23.value(1)

        elif command=="STOP": #“右转”

            #电机1，停止

            pin18.value(0)

            pin19.value(0)

            #电机2，停止

            pin22.value(0)

            pin23.value(0)