编辑代码

import sys,os
#调用数学库和时间库
import math,time
#调用多线程库
import threading

#定义全局变量
#PID控制器相关变量，大家可以找找相关资料来调一下玩玩
KP = 2.5
KI = 0.08
KD = 0.3
delta_t = 0.01
#车速相关变量
target_speed = 0
program_running_flag = False
speed_limit = 100
accel_limit = 10

#标准getch部分，不重复说明
def getch():
    # tty = teletypewriter, termios = terminal io interface
    import tty, termios
    # stdin = standard input, fileno = filestream number 
    fd = sys.stdin.fileno()
    # tcgetattr = termios configuration get attribute
    old_settings = termios.tcgetattr(fd)
    # 尝试
    try:
        # 通过tty模块获取stdin的file stream，然后用read（1）读取1个字节
        tty.setraw(sys.stdin.fileno())
        ch = sys.stdin.read(1)
    # 尝试部分执行完成后执行的“善后”代码
    finally:
        # termios configuration set attribute
        # TCSADRAIN，在termios传输完成后生效，具体参照Unix termios模块的说明
        # 不确定通过tty模块读取输入会对配置造成什么影响，这段代码是用来消除这个影响的
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    # 返回并输出读取到的ch
    return ch

#定义输入任务
def input_task():
    #设定获取全局变量
    #全局变量可被跨线程访问
    global program_running_flag, target_speed, speed_limit, delta_t
    #用flag控制主程序循环，这样在需要的时候可以更好的控制退出的时机和行为
    while program_running_flag:
        #获取输入
        input_key = getch()
        #如果输入是ESC按键
        if ord(input_key) == 27:
            #通过改变flag状态控制所有线程退出
            program_running_flag = False
        #如果输入是w则目标速度+1
        elif ord(input_key) == ord("w"):
            target_speed+=1
        #如果输入是s则目标速度-1
        elif ord(input_key) == ord("s"):
            target_speed-=1
        #如果输入是W则目标速度+10
        elif ord(input_key) == ord('W'):
            target_speed += 10
        #如果输入是S则目标速度-10
        elif ord(input_key) == ord('S'):
            target_speed -= 10
        #如果输入是空格则目标速度为0
        elif ord(input_key) == ord(" "):
            target_speed = 0
        #通过inline if来控制目标速度的范围
        #math.copysign(x,y)返回y的符号和x的值
        target_speed = target_speed if abs(target_speed)<=speed_limit else math.copysign(speed_limit,target_speed)
        #通过控制线程睡眠时间避免跑得过快抢占系统资源
        #PID系统需要精确控制循环时间，输入线程其实不影响输入后的控制效果，大家可以自行尝试
        time.sleep(delta_t)
    

#定义输出任务
def output_task():
    #设定获取全局变量
    #全局变量可被跨线程访问
    global program_running_flag, target_speed, accel_limit, KP, KI, KD, delta_t
    #删除整行文字的控制符，具体可以参考下面链接(仅适用linux):
    # https://tldp.org/HOWTO/Bash-Prompt-HOWTO/x361.html
    erase_str = "\r\033[k"
    # 设置本地变量
    # 用于存储车速信息
    speed = 0
    accel = 0
    # 用于PID控制器存储历史控制误差
    error_integral = 0
    previous_speed_error = 0
    #用flag控制主程序循环，这样在需要的时候可以更好的控制退出的时机和行为
    while program_running_flag:
        #更新速度误差
        speed_error = target_speed-speed
        #通过误差更新P项
        PTerm = KD*speed_error
        #计算误差积分
        error_integral += speed_error * delta_t
        #计算I项
        ITerm = KI*error_integral
        #计算误差微分
        error_derivative = (speed_error-previous_speed_error)/delta_t
        #计算D项
        DTerm = KD * error_derivative
        #组合PID三项获取加速度
        accel = PTerm+ITerm+DTerm
        #通过inline if来控制加速度的范围
        accel = accel if abs(accel)<accel_limit else math.copysign(accel_limit,accel)
        #更新速度
        speed = speed+accel*delta_t
        #更新历史误差以计算下一循环微分项
        previous_speed_error = speed_error
        #组合显示信息
        display_str = "Target Speed:\t {0:.2f} m/s\tCurrent Speed:\t {1:.2f}m/s\tAccel:\t {2:.2f} m/s^2".format(target_speed,speed,accel)
        #通过write写入stdout，res为实际写入字符数量
        res = sys.stdout.write(erase_str+display_str)
        #PID系统需要精确控制循环时间来保证积分量可用
        time.sleep(delta_t)

#定义主函数
def main():
    #设定获取全局变量
    #全局变量可被跨线程访问
    global program_running_flag
    #通过改变flag状态确保所有线程能运行
    program_running_flag = True
    #设置输入线程任务目标
    input_thread = threading.Thread(target=input_task)
    #启动输入线程
    input_thread.start()
    #设置输出线程任务目标
    output_thread = threading.Thread(target=output_task)
    #启动输出线程
    output_thread.start()
    
    #等待输入输出线程正常退出
    input_thread.join()
    output_thread.join()

#启动main函数
main()