# 导入必要的库和模块
from smartcar import ticker, encoder
from seekfree import KEY_HANDLER
from machine import *
import gc
import time
from display import *
from seekfree import TSL1401, MOTOR_CONTROLLER, WIRELESS_UART, IMU963RX
import math
import ustruct
# 初始化硬件设备
imu = IMU963RX() # IMU传感器
ccd = TSL1401(10) # CCD传感器，采集周期10次
key = KEY_HANDLER(5) # 按键处理
motor_l = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C25_DIR_C27, 13000, duty=0, invert=True) # 左电机
motor_r = MOTOR_CONTROLLER(MOTOR_CONTROLLER.PWM_C24_DIR_C26, 13000, duty=0, invert=True) # 右电机
encoder_l = encoder("D0", "D1", True) # 左编码器
encoder_r = encoder("D2", "D3") # 右编码器
wireless = WIRELESS_UART(115200) # 无线通信
uart2 = UART(2) # 串口2
uart2.init(115200)
# 初始化显示设备
cs = Pin('C5', Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 片选引脚
# 拉高拉低一次 CS 片选确保屏幕通信时序正常
cs.high()
cs.low()
rst = Pin('B9', Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 复位引脚
dc = Pin('B8', Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 数据/命令选择
blk = Pin('C4', Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 背光控制
drv = LCD_Drv(SPI_INDEX=1, BAUDRATE=60000000, DC_PIN=dc, RST_PIN=rst, LCD_TYPE=LCD_Drv.LCD200_TYPE) # LCD驱动
lcd = LCD(drv) # LCD实例
lcd.color(0x0000, 0xFFFF) # 设置颜色
lcd.mode(2) # 设置显示模式
lcd.clear(0xDEF7) # 清屏使用淡灰色背景
# 初始化引脚
led = Pin('C4', Pin.OUT, pull=Pin.PULL_UP_47K, value=True) # LED指示灯
beep = Pin('C9', Pin.OUT, pull=Pin.PULL_UP_47K, value=False) # 蜂鸣器
switch1 = Pin('D8', Pin.IN, pull=Pin.PULL_UP_47K, value=True) # 拨码开关1
switch2 = Pin('D9', Pin.IN, pull=Pin.PULL_UP_47K, value=True) # 拨码开关2
end_switch = Pin('C19', Pin.IN, pull=Pin.PULL_UP_47K, value=True) # 终点开关
# 全局变量定义
# PID控制相关变量
err_1 = err_sum_1 = err_x_1 = err_last_1 = 0 # PID1误差项
err_2 = err_sum_2 = err_x_2 = err_last_2 = 0 # PID2误差项
err_3 = err_sum_3 = err_x_3 = err_last_3 = 0 # PID3误差项
errt = errt_sum = errt_x = errt_last = 0 # 转向PID误差项
# 系统状态标志
ticker_flag = ticker_flag2 = False # 定时器标志
ticker_count = ticker_count0 = ticker_count3 = 0 # 定时器计数器
motor_dir = 1 # 电机方向
motor_duty = 0 # 电机占空比
motor_duty_max = 1000 # 最大占空比
turn = 0 # 转向量
state1 = switch1.value() # 开关1状态
state2 = switch2.value() # 开关2状态
end_state = end_switch.value() # 终点开关状态
# CCD相关变量
ccd1_zhongzhi = ccd2_zhongzhi = 64 # CCD中心值
left1 = right1 = left2 = right2 = 0 # CCD边界
flag_stute1 = flag_stute2 = 0 # 状态标志
flag_shizi1 = flag_shizi2 = 0 # 十字标志
shreshlod1 = shreshlod2 = 0 # 阈值
# 路径跟踪相关变量
lline = rline = lline2 = rline2 = 0 # 左右边界线
zThreshold = zThreshold2 = 15 # 阈值
zhong = zhong2 = 64 # 中心位置
banma_flag = stop = ysbzw = 0 # 标志位
track_stop = huang_tag = 0 # 跟踪停止标志
huang_l_flag = huang_r_flag = 0 # 黄线标志
huang_l_zt = huang_r_zt = 0 # 黄线状态
bizhan_l = bizhan_r = txzb = 0 # 避障相关
bizhan_flag = cha2 = 0 # 避障标志和差值
zuocha = zuocha2 = 0 # 左差值
zebra = road2 = jifen = 0 # 斑马线和道路标志
art1 = art2 = art3 = puodao_flag = 0 # 其他标志
road = fix = circle = zebra2 = banma_slow = stop_flag = 0
# 控制参数
delta_T = 0.001 # 采样周期为5ms
I_ex, I_ey, I_ez = 0.0, 0.0, 0.0 # 积分误差
"""""""""""""""主要调节以下参数"""""""""""""""
"""根据屏幕显示Pitch确定平衡角度"""
med_roll_angle = 34 # 平衡角度 前倾+ 后仰-
med_speed1 = -185#-1.9 # 速度（速度为零即静止）
med_speed = med_speed1 #0 当前速度
# 角速度PID参数##高频震动：增大kd或减小kp
angle_kp = -650#2280.0 调节响应速度
angle_ki = -2.25#-2.25 控制车辆在平衡点附近移动
angle_kd = -320 #-122
# 角度PID参数+-
roll_angle_Kp = 0.055 #0.06585 控制车辆平衡角度
roll_angle_Ki = 0.00001 #0.00001
roll_angle_Kd = 0.38 #0.2 低频震动
# 速度PID参数
speed_Kp = -0.070#0.0792 加速/减速过慢，调节绝对值
speed_Ki = 0.00000001 #0.00000001
speed_Kd = 0.014 #0.01055 速度波动过大则增大kd或减小kp
"""""""""""""""主要调节以上参数"""""""""""""""
CCD_dir = [64,64]
# 转向PID参数
a = 0.35#0.152
Kpc = 0.45#0.2
turn_ki = 20#25 #中线位置调节
turn_kd = 2.0 #防止左右摆动幅度过大
turn_kd2 = -18 #转向速度
# 按键控制变量
param_index = 0 # 当前选择的参数索引
param_editing = True # 是否处于参数编辑模式
# 参数菜单定义
PARAM_MENU = [
"1.AngVel Kp", "2.AngVel Ki", "3.AngVel Kd",
"4.Angle Kp", "5.Angle Ki", "6.Angle Kd",
"7.Speed Kp", "8.Speed Ki","9.Speed Kd", "10.med_roll_angle", "11.Launch!"
]
pid_params = [ # 可调参数列表
angle_kp, angle_ki, angle_kd,
roll_angle_Kp, roll_angle_Ki, roll_angle_Kd,
speed_Kp, speed_Ki,speed_Kd,med_roll_angle
]
PARAM_STEP = [ # 各参数调节步长
10.0, 0.01, 1.0, # 角速度环
0.001, 0.00001, 0.001,# 角度环
0.0000001, 0.00001, 0.00001, # 速度环
0.5#角度
]
# 编码器卡尔曼滤波参数
KAL_P = KAL_P2 = 0.02 # 估算协方差
KAL_G = KAL_G2 = 0.0 # 卡尔曼增益
KAL_Q = KAL_Q2 = 0.70 # 过程噪声协方差
KAL_R = KAL_R2 = 200 # 测量噪声协方差
KAL_Output = KAL_Output2 = 0.0 # 卡尔曼滤波器输出
# 中间变量
angle_1 = speed_1 = counts = motor1 = motor2 = 0
angle_2 = speed_2 =0
distance = id_number = 0 # 距离和ID号
Filter_data = [0, 0, 0] # 滤波数据
PI = 3.14159265358 # 圆周率
last_yaw = 0 # 上次偏航角
max_gyro_x = 0 # 最大角速度X
# 无线数据传输缓冲区
data_flag = wireless.data_analysis()
data_wave = [0]*8
# 类定义
class anjian:
"""按键处理类"""
def __init__(self, KEY_1, KEY_2, KEY_3, KEY_4):
self.One = KEY_1
self.Two = KEY_2
self.Three = KEY_3
self.Four = KEY_4
class bianmaqi:
"""编码器类"""
def __init__(self, KAL_templ_pluse, KAL_tempr_pluse):
self.KAL_templ_pluse = KAL_templ_pluse
self.KAL_tempr_pluse = KAL_tempr_pluse
class Imu_element:
"""IMU数据类"""
def __init__(self, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z,
Pitch, Roll, Yaw, X, Y, Z, Total_Yaw):
self.acc_x = acc_x
self.acc_y = acc_y
self.acc_z = acc_z
self.gyro_x = gyro_x
self.gyro_y = gyro_y
self.gyro_z = gyro_z
self.Pitch = Pitch
self.Roll = Roll
self.Yaw = Yaw
self.X = X
self.Y = Y
self.Z = Z
self.Total_Yaw = Total_Yaw
class param:
"""PID参数类"""
def __init__(self, param_Kp, param_Ki):
self.param_Kp = param_Kp
self.param_Ki = param_Ki
class QInfo:
"""四元数信息类"""
def __init__(self):
self.q0 = 1.0
self.q1 = 0.0
self.q2 = 0.0
self.q3 = 0.0
# 实例化类
KEY = anjian(0, 0, 0, 0)
Encoders = bianmaqi(0, 0)
Imu = Imu_element(0.00, 0.00, 0.00, 0.00, 0.00, 0.00,
0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00)
Param = param(15.5, 0.006)
Q_info = QInfo()
# 函数定义
def limit(value, min_value, max_value):
"""限制数值范围"""
return min(max(value, min_value), max_value)
def calculate_pid(err, err_sum, err_last, med, value, kp, ki, kd):
"""计算PID控制值"""
err = med - value
err_sum += err
err_x = err - err_last
pwm = kp * err + ki * err_sum + kd * err_x
err_last = err
return pwm
def pid_position_1(med, value, kp, ki, kd):
"""PID控制器1"""
global err_1, err_sum_1, err_last_1
pwm_1 = calculate_pid(err_1, err_sum_1, err_last_1, med, value, kp, ki, kd)
err_last_1 = err_1
return pwm_1
def pid_position_2(med, value, kp, ki, kd):
"""PID控制器2"""
global err_2, err_sum_2, err_last_2
pwm_2 = calculate_pid(err_2, err_sum_2, err_last_2, med, value, kp, ki, kd)
err_last_2 = err_2
return pwm_2
def pid_position_3(med, value, kp, ki, kd):
"""PID控制器3"""
global err_3, err_sum_3, err_last_3
pwm_3 = calculate_pid(err_3, err_sum_3, err_last_3, med, value, kp, ki, kd)
err_last_3 = err_3
return pwm_3
def pid_turn(med, value, kp, ki, kd):
"""转向PID控制器"""
global errt, errt_sum, errt_last
pwmt = calculate_pid(errt, errt_sum, errt_last, med, value, kp, ki, kd)
errt_last = errt
return pwmt
def Key():
global param_index, param_editing, pid_params, angle_kp, angle_ki, angle_kd
global roll_angle_Kp, roll_angle_Ki, roll_angle_Kd, speed_Kp, speed_Ki
key_data = key.get()
beep_triggered = False # 标记是否已经触发蜂鸣器
if param_editing:
if key_data[0]: # 参数增加
pid_params[param_index] += PARAM_STEP[param_index]
print("Key1")
beep_triggered = True
if key_data[1]: # 参数减少
pid_params[param_index] -= PARAM_STEP[param_index]
print("Key2")
beep_triggered = True
if key_data[2]: # 切换参数
param_index = (param_index + 1) % len(PARAM_MENU)
print("Key3")
beep_triggered = True
if param_index == 10: # 到达"启动!"选项
param_index = 0
if key_data[3]: # 确认参数并控制电机启动的停止
print("Key4:",param_editing)
# 更新实际PID参数
angle_kp, angle_ki, angle_kd = pid_params[0], pid_params[1], pid_params[2]
roll_angle_Kp = pid_params[3]
roll_angle_Ki = pid_params[4]
roll_angle_Kd = pid_params[5]
speed_Kp, speed_Ki,speed_Kd= pid_params[6], pid_params[7],pid_params[8]
med_roll_angle = pid_params[9]
param_editing = not param_editing
beep_triggered = True
# 如果有按键触发，则蜂鸣器提示
if beep_triggered:
beep.on() # 蜂鸣器提示
time.sleep_ms(200)
beep.off()
def invSqrt(x):
"""快速平方根倒数"""
return 1.0 # 简化实现，实际使用时需要优化
def Imu963():
"""IMU数据处理函数"""
global imu_data, max_gyro_x, last_yaw
alpha = 0.3 # 滤波系数
# 数据有效性检查
for i in range(3, 6):
if abs(imu_data[i]) < 30 or abs(imu_data[i]) > 30000:
imu_data[i] = 0
# 原始数据处理
Imu.X = int(imu_data[3] / 16.4)
Imu.Y = int(imu_data[4] / 16.4)
Imu.Z = int(imu_data[5] / 16.4)
# 角速度处理
Imu.gyro_x = round((float(imu_data[3]) - Filter_data[0]), 3) * PI / 180 / 16.4
Imu.gyro_y = round((float(imu_data[4]) - Filter_data[1]), 3) * PI / 180 / 16.4
Imu.gyro_z = round((float(imu_data[5]) - Filter_data[2]), 3) * PI / 180 / 14.4
# 加速度滤波处理
Imu.acc_x = round(((float(imu_data[0]) * alpha) / 4096 + Imu.acc_x * (1 - alpha)), 3)
Imu.acc_y = round(((float(imu_data[1]) * alpha) / 4096 + Imu.acc_y * (1 - alpha)), 3)
Imu.acc_z = round(((float(imu_data[2]) * alpha) / 4096 + Imu.acc_z * (1 - alpha)), 3)
# 更新AHRS姿态
IMU_AHRSupdate(Imu.gyro_x, Imu.gyro_y, Imu.gyro_z, Imu.acc_x, Imu.acc_y, Imu.acc_z)
# 记录最大角速度
if abs(max_gyro_x) < abs(Imu.Pitch):
max_gyro_x = Imu.Pitch
def Imu963ra_Init():
"""IMU初始化校准"""
global Filter_data, imu_data
Filter_data = [0, 0, 0]
# 采集1000次数据进行平均校准
for i in range(1000):
imu_data = imu.get()
Filter_data[0] += imu_data[3]
Filter_data[1] += imu_data[4]
Filter_data[2] += imu_data[5]
time.sleep_ms(1)
# 计算平均值
Filter_data[0] = float(Filter_data[0] / 1000)
Filter_data[1] = float(Filter_data[1] / 1000)
Filter_data[2] = float(Filter_data[2] / 1000)
def IMU_AHRSupdate(gx, gy, gz, ax, ay, az):
"""AHRS姿态更新算法"""
global I_ex, I_ey, I_ez, last_yaw
halfT = 0.5 * delta_T
# 计算当前重力单位向量
q0q0 = Q_info.q0 * Q_info.q0
q0q1 = Q_info.q0 * Q_info.q1
q0q2 = Q_info.q0 * Q_info.q2
q1q1 = Q_info.q1 * Q_info.q1
q1q3 = Q_info.q1 * Q_info.q3
q2q2 = Q_info.q2 * Q_info.q2
q2q3 = Q_info.q2 * Q_info.q3
q3q3 = Q_info.q3 * Q_info.q3
# 加速度归一化
norm = invSqrt(ax*ax + ay*ay + az*az)
ax *= norm
ay *= norm
az *= norm
# 计算当前重力单位向量
vx = 2 * (q1q3 - q0q2)
vy = 2 * (q0q1 + q2q3)
vz = q0q0 - q1q1 - q2q2 + q3q3
# 计算误差
ex = ay * vz - az * vy
ey = az * vx - ax * vz
ez = ax * vy - ay * vx
# PI修正
I_ex += delta_T * ex
I_ey += delta_T * ey
I_ez += delta_T * ez
gx += Param.param_Kp * ex + Param.param_Ki * I_ex
gy += Param.param_Kp * ey + Param.param_Ki * I_ey
gz += Param.param_Kp * ez + Param.param_Ki * I_ez
# 四元数微分方程
q0 = Q_info.q0
q1 = Q_info.q1
q2 = Q_info.q2
q3 = Q_info.q3
Q_info.q0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT
Q_info.q1 = q1 + (q0*gx + q2*gz - q3*gy) * halfT
Q_info.q2 = q2 + (q0*gy - q1*gz + q3*gx) * halfT
Q_info.q3 = q3 + (q0*gz + q1*gy - q2*gx) * halfT
# 四元数归一化
norm = invSqrt(Q_info.q0**2 + Q_info.q1**2 + Q_info.q2**2 + Q_info.q3**2)
Q_info.q0 *= norm
Q_info.q1 *= norm
Q_info.q2 *= norm
Q_info.q3 *= norm
# 计算欧拉角
value1 = limit(-2 * Q_info.q1 * Q_info.q3 + 2 * Q_info.q0 * Q_info.q2, -1, 1)
Imu.Roll = round(math.asin(value1) * 180 / math.pi, 3) # 横滚角
Imu.Pitch = round(math.atan2(2 * Q_info.q2 * Q_info.q3 + 2 * Q_info.q0 * Q_info.q1,
-2 * Q_info.q1**2 - 2 * Q_info.q2**2 + 1) * 180 / math.pi, 3) # 俯仰角
Imu.Yaw = round(math.atan2(2 * Q_info.q1 * Q_info.q2 + 2 * Q_info.q0 * Q_info.q3,
-2 * Q_info.q2**2 - 2 * Q_info.q3**2 + 1) * 180 / math.pi, 3) # 偏航角
# 计算总偏航角
error_yaw = Imu.Yaw - last_yaw
if error_yaw < -360:
error_yaw += 360
if error_yaw > 360:
error_yaw -= 360
Imu.Total_Yaw += error_yaw
last_yaw = Imu.Yaw
# 保证总偏航角在0-360度范围内
if Imu.Total_Yaw > 360:
Imu.Total_Yaw -= 360
if Imu.Total_Yaw < 0:
Imu.Total_Yaw += 360
def Wireless():
"""无线数据传输处理"""
global data_flag, data_wave
data_flag = wireless.data_analysis() # 数据解析
# 更新虚拟示波器数据
data_wave[0] = angle_1
data_wave[1] = speed_1
data_wave[2] = Imu.acc_z
data_wave[3] = Imu.gyro_x
data_wave[4] = Imu.gyro_y
data_wave[5] = Imu.Roll
data_wave[6] = Imu.Pitch
# 发送数据到上位机
wireless.send_oscilloscope(*data_wave)
def KalmanFilter(input):
"""卡尔曼滤波器"""
global KAL_P, KAL_G, KAL_Output
KAL_P = KAL_P + KAL_Q # 估算协方差更新
KAL_G = KAL_P / (KAL_P + KAL_R) # 计算卡尔曼增益
KAL_Output = KAL_Output + KAL_G * (input - KAL_Output) # 更新最优估计值
KAL_P = (1 - KAL_G) * KAL_P # 更新协方差
return KAL_Output
def KalmanFilter2(input):
"""第二个卡尔曼滤波器"""
global KAL_P2, KAL_G2, KAL_Output2
KAL_P2 = KAL_P2 + KAL_Q2
KAL_G2 = KAL_P2 / (KAL_P2 + KAL_R2)
KAL_Output2 = KAL_Output2 + KAL_G2 * (input - KAL_Output2)
KAL_P2 = (1 - KAL_G2) * KAL_P2
return KAL_Output2
def draw_rect(x, y, width, height, color, thick=1, fill=False):
"""用line方法实现矩形绘制"""
if fill:
# 填充矩形 - 用垂直线条填充
for i in range(height):
lcd.line(x, y+i, x+width-1, y+i, color=color, thick=thick)
else:
# 只画边框
lcd.line(x, y, x+width-1, y, color=color, thick=thick) # 上边
lcd.line(x, y+height-1, x+width-1, y+height-1, color=color, thick=thick) # 下边
lcd.line(x, y, x, y+height-1, color=color, thick=thick) # 左边
lcd.line(x+width-1, y, x+width-1, y+height-1, color=color, thick=thick) # 右边
last_title = None
last_param_value = None
last_param_index = None
def ips200_display():
"""美化后的LCD显示函数"""
global last_title,last_motors
global last_param_value, last_param_index
#1. ---- 顶部状态栏 ----
if last_title is None:
draw_rect(0, 0, 320, 20, 0x0000, fill=True) # 黑色背景
title = " RT1021 Dashboard "
colors = [0xF800, 0xFC00, 0xFFE0, 0x07E0, 0x07FF, 0x001F, 0x801F, 0xF81F] # 彩虹色
# 阴影效果（偏移1像素）
for i, char in enumerate(title):
x_pos = 40 + i * 8
lcd.str16(x_pos + 1, 2 + 1, char, 0x3186) # 灰色阴影
for i, char in enumerate(title):
x_pos = 40 + i * 8
color = colors[i % len(colors)]
lcd.str16(x_pos, 2, char, color)
last_title = True
# 2. IMU数据区域 (带边框)
draw_rect(0, 20, 105, 65, 0x001F, thick=1) # 蓝色边框
lcd.str16(15, 20, "Attitude", 0x001F) # 蓝色标题
lcd.str16(5, 40, f"Pitch:{Imu.Pitch:6.2f}", 0x0000) # 黑色数据
lcd.str16(5, 60, f"Yaw:{Imu.Yaw:6.2f}", 0x0000)
# 3. 运动控制区域 (带边框)
draw_rect(105, 20, 135, 65, 0xF800, thick=1) # 红色边框
lcd.str16(127, 20, "Motion Ctrl", 0xF800)
lcd.str16(117, 40, f"Speed: {med_speed:4.1f}", 0x0000)
lcd.str16(117, 60, f"Turn: {turn:6.3f}", 0x0000)
# 4. 电机信息区域 (带边框)
draw_rect(0, 85, 310, 75, 0x0000, thick=1) # 黑色边框
lcd.str16(5, 110, "Motor", 0x0000)
lcd.str16(5, 126, "Output", 0x0000)
# 5. 陀螺仪数据区域 (带边框)
draw_rect(0, 160, 250, 50, 0xFC80, thick=1) # 橙色边框
lcd.str16(5, 165, "Gyroscope", 0xFC80)
lcd.str16(5, 185, f"X:{Imu.gyro_x:7.3f} Y:{Imu.gyro_y:7.3f} Z:{Imu.gyro_z:7.3f}", 0x0000)
# 6. 编码器数据区域 (带边框)
draw_rect(0, 210, 250, 40, 0x79FF, thick=1) # 天蓝色边框
lcd.str16(5, 220, f"Encoder L:{Encoders.KAL_templ_pluse:6.1f} R:{Encoders.KAL_tempr_pluse:6.1f}", 0x0000)
# 7. CCD数据区域 (带边框)
draw_rect(0, 250, 250, 70, 0xAA55, thick=1) # 紫色边框
lcd.str16(5, 255, "CCD Line Position", 0xAA55)
lcd.str16(5, 275, f"L1:{lline:3d} R1:{rline:3d} Mid:{zhong:3d}", 0x0000)
lcd.str16(5, 295, f"L2:{lline2:3d} R2:{rline2:3d} Mid2:{zhong2:3d}", 0x0000)
# 8. 道路识别信息
lcd.str16(5, 325, f"Road Type: {road:.2f}", 0x0000)
# 9. 参数编辑模式特殊显示
lcd.str16(60, 90, PARAM_MENU[param_index], 0xFFFF) # 白色标题
# 操作提示可以只绘制一次
if last_param_index is None:
draw_rect(60, 135, 190, 20, 0xFFFF, thick=1, fill=True)
lcd.str16(65, 137, "1:+ 2:- 3:Next 4:OK", 0x0000)
# 参数值显示 (带背景色)
if pid_params[param_index] != last_param_value or param_index != last_param_index:
color = 0x07E0 if pid_params[param_index] >=0 else 0xF800 #绿/红区分正负
draw_rect(60, 110, 190, 20, color, thick=1, fill=True)
lcd.str16(65, 112, f"Value: {pid_params[param_index]:.4f}", 0x0000)
last_param_value = pid_params[param_index]
last_param_index = param_index
class ALL_CCD:
def __init__(self):
self.num=0
self.dir = 0
self.dir_0 = 0
self.left = 38
self.right = 82
self.left_0 = 43
self.right_0 = 78
self.lode_wid = 44
self.lode_wid_0 = 35
self.mid = 60
self.mid_line = 60
self.mid_line_0 = 60
self.last_angle = 62
self.last_angle_1 = 62
def CCD1(self,ccd_data):
global av_dir0,a8
#计算左右平均像素值
value_av = (max(ccd_data)+min(ccd_data))//2
#二次处理
ccd_data = [250 if num > value_av else 0 for num in ccd_data]
#计算左右平均像素值
av_l = int(sum(ccd_data[15: 64])/49)
av_r = int(sum(ccd_data[64:113])/49)
if av_l>200 and av_r>200:
a8=8
else:
a8=0
print("ccd1",av_l,av_r)
for i in range(127):
if ccd_data[i]==0 and ccd_data[i] != ccd_data[i-1] and ccd_data[i] != ccd_data[i+1]:
ccd_data[i] = 250
elif ccd_data[i]==250 and ccd_data[i] != ccd_data[i-1] and ccd_data[i] != ccd_data[i+1]:
ccd_data[i] =0
ccd_dataA = ccd_data
#差比和
new_data = [int(abs(ccd_dataA[i]-ccd_dataA[i+1]))for i in range(len(ccd_dataA)-1)]#127
result_l = [self.mid_line - i for i,x in enumerate(new_data[self.mid_line:15+a8:-1]) if x > 0]
result_r = [i + self.mid_line for i,x in enumerate(new_data[self.mid_line:len(new_data)-15-a8]) if x >0]
#print("1bianjie",result_l,result_r)
#判断边界
if len(result_l)>0 and len(result_r)>0:
self.dir = 0
av_dir0 = 0
self.left = result_l[0]
self.right = result_r[0]
if abs(self.left - self.right) > 92:
self.mid_line = 62
self.dir = 1
av_dir0 = 404
#边界二次处理
if abs(self.left - self.right) - self.lode_wid > 15:
if abs(self.left - self.mid) > abs(self.right- self.mid):
self.left = self.right - self.lode_wid
elif abs(self.left - self.mid) < abs(self.right- self.mid):
self.right = self.left + self.lode_wid
elif len(result_l)== 0 and len(result_r)>0 and result_r[0] < 108:
if av_r < av_l and av_l>=240 and av_r<180:
av_dir0 = 11
else:
av_dir0 = 1
if av_r < av_l:
self.right = result_r[0]
self.left = self.right - self.lode_wid
else:
self.left = result_r[0]
self.right = self.left + self.lode_wid
elif len(result_l)>0 and len(result_r)==0 and result_l[0] > 20:
if av_r > av_l and av_r>=240 and av_l<180:
av_dir0 = 22
else:
av_dir0 = 2
if av_r < av_l:
self.right = result_l[0]
self.left = self.right - self.lode_wid
else:
self.left = result_l[0]
self.right = self.left + self.lode_wid
else:
self.mid_line = 62
self.dir = 1
av_dir0 = 404
track_info = (self.left + self.right)//2# 计算赛道中心线位置
if abs(track_info - self.mid_line) > 30:
track_info = self.mid_line
self.dir = 1
self.last_angle = track_info
self.mid_line = track_info
if self.dir == 1:
if abs(track_info-self.last_angle)<4:
self.dir =0
else:
return self.last_angle
else:
return track_info
def CCD2(self,ccd_data):
global av_dir1,a8,speed_l,speed_r,mortor_dir
self.num+=1
count=0
value_av = (max(ccd_data)+min(ccd_data))//2
#二次处理
ccd_data = [250 if num > value_av else 0 for num in ccd_data]
zebra_crossing = [i for i in range(127) if (ccd_data[i] == 0 and ccd_data[i + 1] == 250) or (ccd_data[i] == 250 and ccd_data[i + 1] == 0)]
for i in range(len(zebra_crossing) - 1):
if abs(zebra_crossing[i] - zebra_crossing[i + 1]) < 5:
count += 1
if count >= 10 and self.num>250 and -25<int(imu_z)<25:
speed_l=0
speed_r=0
mortor_dir=2
self.num=0
#计算左右平均像素值
av_l = int(sum(ccd_data[15:64])/49)
av_r = int(sum(ccd_data[64:113])/49)
if av_l>200 and av_r>200:
a8=8
else:
a8=0
#print("ccd2",av_l,av_r)
for i in range(127):
if ccd_data[i]==0 and ccd_data[i] != ccd_data[i-1] and ccd_data[i] != ccd_data[i+1]:
ccd_data[i] = 250
elif ccd_data[i]==250 and ccd_data[i] != ccd_data[i-1] and ccd_data[i] != ccd_data[i+1]:
ccd_data[i] =0
ccd_dataA = ccd_data
#差比和
new_data = [int(abs(ccd_dataA[i]-ccd_dataA[i+1]))for i in range(len(ccd_dataA)-1)]#127
result_l = [self.mid_line_0 - i for i,x in enumerate(new_data[self.mid_line_0:15+a8:-1]) if x > 0]
result_r = [i + self.mid_line_0 for i,x in enumerate(new_data[self.mid_line_0:len(new_data)-15-a8]) if x >0]
#print("2bianjie",result_l,result_r)
#判断边界
if len(result_l)>0 and len(result_r)>0:
av_dir1 = 0
self.dir_0 = 0
self.left_0 = result_l[0]
self.right_0 = result_r[0]
if abs(self.left_0 - self.right_0) > 92:
self.mid_line_0 = 62
av_dir1 = 404
self.dir_0 = 1
#边界二次处理
if abs(self.left_0 - self.right_0) - self.lode_wid_0 > 15:
if abs(self.left_0 - self.mid) > abs(self.right_0- self.mid):
self.left_0 = self.right_0 - self.lode_wid_0
elif abs(self.left_0 - self.mid) < abs(self.right_0- self.mid):
self.right_0 = self.left_0 + self.lode_wid_0
elif len(result_l)== 0 and len(result_r)>0 and result_r[0] < 108:
if av_r < av_l and av_l>=240 and av_r<180:
av_dir1 = 11
else:
av_dir1 = 1
if av_r < av_l:
self.right_0 = result_r[0]
self.left_0 = self.right_0 - self.lode_wid_0
else:
self.left_0 = result_r[0]
self.right_0 = self.left_0 + self.lode_wid_0
elif len(result_l)>0 and len(result_r)==0 and result_l[0] > 20:
if av_r > av_l and av_r>=240 and av_l<180:
av_dir1 = 22
else:
av_dir1 = 2
if av_r < av_l:
self.right_0 = result_l[0]
self.left_0 = self.right_0 - self.lode_wid_0
else:
self.left_0 = result_l[0]
self.right_0 = self.left_0 + self.lode_wid_0
else:
self.mid_line_0 = 62
av_dir1 = 404
self.dir_0 = 1
track_info = (self.left_0 + self.right_0)//2# 计算赛道中心线位置
if abs(track_info - self.mid_line_0) > 30 :
track_info = self.mid_line_0
self.dir_0 = 1
self.last_angle_1 = track_info
self.mid_line_0 = track_info
if self.dir_0 == 1:
if abs(track_info-self.last_angle_1)<4:
self.dir_0 =0
else:
return self.last_angle_1
else:
return track_info
def control_turn(zhong1_2):
"""转向控制函数"""
if zhong1_2 is None: # 如果输入无效，使用默认值（居中）
zhong1_2 = 64
errt = (zhong1_2 - 63)
turn_kp = a * abs(errt) + Kpc
turn = pid_turn(63, zhong1_2, turn_kp, turn_ki, turn_kd) + Imu.gyro_z * turn_kd2
return turn
def angle_speed1(med_gyro, cur_gyro):
"""角速度控制函数"""
motor = pid_position_1(med_gyro, cur_gyro, angle_kp, angle_ki, angle_kd)
return limit(motor, -4000, 4000)
def angle(med_roll_angle, cur_roll_angle):
"""角度控制函数"""
global angle_1
angle_1 = pid_position_2(med_roll_angle, cur_roll_angle, roll_angle_Kp, roll_angle_Ki, roll_angle_Kd)
return angle_1
def speed(med_speed, cur_speed):
"""速度控制函数"""
global speed_1
speed_1 = pid_position_3(med_speed, cur_speed, speed_Kp, speed_Ki, speed_Kd)
return speed_1
def parse_data():
"""解析串口数据"""
if uart2.any():
line = uart2.readline() # 读取一行数据
if line is None:
return None, None
try:
line_str = line.decode('utf-8').strip() # 转换为字符串
parts = line_str.split(',') # 按逗号分割
if len(parts) >= 2:
var1 = float(parts[0]) # 第一个参数为浮点数
var2 = int(parts[1]) # 第二个参数为整数
return var1, var2
except (UnicodeDecodeError, ValueError):
pass
return None, None
# 定时器中断处理函数
def time_pit_handler(time):
"""定时器1中断处理"""
global ticker_flag, ticker_count, speed_1, angle_1, motor1, motor2
ticker_flag = True
ticker_count = (ticker_count + 1) if (ticker_count < 10) else 1
# 1ms周期任务
if ticker_count % 1 == 0:
Imu963() # 更新IMU数据
# 电机控制
motor1 = angle_speed1(angle_1, Imu.gyro_x)
motor2 = angle_speed1(angle_1, Imu.gyro_x)
motor1 = limit(motor1 + control_turn(CCD_dir[0]), -4000, 4000)
motor2 = limit(motor2 - control_turn(CCD_dir[0]), -4000, 4000)
if not param_editing:
motor_l.duty(-motor1)
motor_r.duty(-motor2)
else:
motor_l.duty(0)
motor_r.duty(0)
# 5ms周期任务
if ticker_count % 5 == 0:
angle_1 = angle(med_roll_angle - speed_1, Imu.Pitch)
# 10ms周期任务
if ticker_count % 10 == 0:
speed_1 = speed(med_speed, Encoders.KAL_templ_pluse)
speed_2 = speed(med_speed, Encoders.KAL_tempr_pluse)
#print("左误差 ={:>6f}, 右误差 ={:>6f}\r\n".format(speed_1,speed_2))
def time_pit2_handler(time):
"""定时器2中断处理"""
global ticker_flag2
ticker_flag2 = True
Encoders.KAL_templ_pluse = KalmanFilter(encoder_l.get()) # 左编码器滤波
Encoders.KAL_tempr_pluse = KalmanFilter2(encoder_r.get()) # 右编码器滤波
CCD = ALL_CCD()#实例化
# 初始化定时器
pit1 = ticker(1)
pit2 = ticker(2)
pit3 = ticker(3)
# 关联数据采集
pit1.capture_list(imu)
pit2.capture_list(ccd)
pit3.capture_list(encoder_l, encoder_r,key)
# 设置定时器回调函数
pit1.callback(time_pit_handler)
pit2.callback(time_pit2_handler)
# 初始化IMU
Imu963ra_Init()
# 启动定时器
pit1.start(1) # 1ms周期
pit2.start(6) # 6ms周期
pit3.start(10) # 10ms周期
# 主循环
while True:
Key() # 处理按键输入
if ticker_flag:
ccd_data1 = ccd.get(0)
ccd_data2 = ccd.get(1)
CCD_dir[0] =CCD.CCD1(ccd_data1)
CCD_dir[1] =CCD.CCD2(ccd_data2)
# print(ccd_data1)
# print(ccd_data2)
if CCD_dir[0] is None:
zhong2 = 404
else :
zhong2=CCD_dir[0]
# print(ccd_data1)
# print("CCD1:",CCD_dir[0])
#
ticker_flag = False
#电机启动屏幕关闭刷新
if not param_editing and last_title:
lcd.str16(165, 90, "RUN>>>", 0x0000)
last_title = None
#与上相反
if param_editing:
ips200_display()
# 检查拨码开关状态
if switch2.value() != state2:
print("Test program stop.")
break
# 垃圾回收
gc.collect()
小车运行代码，不根据轨道走，蛇形走，我应该如何调参
最新发布
最低0.47元/天 解锁文章
扫一扫
2375
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
