E. 实验8_10_蛇形矩阵

该博客介绍如何使用C语言创建蛇形矩阵。通过输入一个整数n,程序生成一个n阶的蛇形矩阵,按照特定顺序填充1到n²的数字。博客展示了5阶和10阶矩阵的示例,并提供了作者的实现代码。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

题目描述
问题描述:
蛇形矩阵是一个nn的矩阵,将整数1到nn按照蛇形的顺序装入一个 n*n 的蛇形矩阵中,如样例所示分别为5阶和10阶蛇形矩阵:
输入与输出要求:
输入一个整数n,代表蛇形矩阵的阶数,n的范围是1—100。输出蛇形矩阵。每行的每个元素用空格分隔,注意最后一个数的后面为换行符。
程序运行效果:
Sample 1:
5
1 3 4 10 11
2 5 9 12 19
6 8 13 18 20
7 14 17 21 24
15 16 22 23 25
Sample 2:
10
1 3 4 10 11 21 22 36 37 55
2 5 9 12 20 23 35 38 54 56
6 8 13 19 24 34 39 53 57 72
7 14 18 25 33 40 52 58 71 73
15 17 26 32 41 51 59 70 74 85
16 27 31 42 50 60 69 75 84 86
28 30 43 49 61 68 76 83 87 94
29 44 48 62 67 77 82 88 93 95
45 47 63 66 78 81 89 92 96 99
46 64 65 79 80 90 91 97 98 100

我的写法:

#include<stdio.h>
main()
{
 int n,i,j,k=1,x,y;
 scanf("%d",&n);
 int a[n+10][n+10];
  for(j=1;j<=n;j++)                                            //“蛇”爬前n次时的规律
  {
   if(j%2==1)
    for(x=j-1,y=0;x>=0&&y>=0;y++,x--,k++)
        a[y][x]=k;
      else
          for(x=0,y=j-1;x>=0&&y>
# 导入必要的库和模块 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(&#39;C5&#39;, Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 片选引脚 # 拉高拉低一次 CS 片选确保屏幕通信时序正常 cs.high() cs.low() rst = Pin(&#39;B9&#39;, Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 复位引脚 dc = Pin(&#39;B8&#39;, Pin.OUT, pull=Pin.PULL_UP_47K, value=1) # 据/命令选择 blk = Pin(&#39;C4&#39;, 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(&#39;C4&#39;, Pin.OUT, pull=Pin.PULL_UP_47K, value=True) # LED指示灯 beep = Pin(&#39;C9&#39;, Pin.OUT, pull=Pin.PULL_UP_47K, value=False) # 蜂鸣器 switch1 = Pin(&#39;D8&#39;, Pin.IN, pull=Pin.PULL_UP_47K, value=True) # 拨码开关1 switch2 = Pin(&#39;D9&#39;, Pin.IN, pull=Pin.PULL_UP_47K, value=True) # 拨码开关2 end_switch = Pin(&#39;C19&#39;, 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(&#39;utf-8&#39;).strip() # 转换为字符串 parts = line_str.split(&#39;,&#39;) # 按逗号分割 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() 小车运行代码,不根据轨道走,蛇形走,我应该如何调参
最新发布
06-11
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值