树莓派视觉导航车设计

]。树莓派4B可以通过WIFI直接连接，也可以通过千兆网线端口与电脑进行通信，所以在日常调试过程中一般不需要连线，非常方便。在树莓派上有40个Pin，共有26个可编程GPIO口，可以连接各类传感器。通过测试表明，树莓派4B的运算速度和准确度等性能指标完全能够满足智能避障巡管小车系统的要求。树莓派4B外部如图3-1所示，树莓派4BPin接口如图3-2所示。图3-1树莓派4B外部图图3-2树莓派4Bpin接口图3.1.2视觉模块该智能小车系统为优化调试效率并提升中远距离视差测量精度，选用了一款500万像素高帧率USB免驱摄像头，其CMOS传感器支持60fps图像采集能力，可在动态场景中有效减少运动模糊，确保目标识别的清晰度与实时性。该摄像头采用即插即用设计，无需额外驱动配置，通过标准USB接口直接接入树莓派4B主控板，显著简化了开发环境搭建流程。500万像素分辨率结合全局快门技术，能够在1.5-3米范围内实现±0.5cm的视差计算精度，满足路径规划与障碍物定位的精细化需求。此外，其宽动态范围（WDR）特性可适应复杂光照条件，配合树莓派的GPIO扩展能力，实现了图像采集、处理与运动控制的低延迟闭环，为视觉导航算法的高效迭代与多场景验证提供了可靠硬件基础。硬件平台如图3-3所示。图3-3摄像头实物图3.1.3电机驱动该智能小车系统的驱动控制模块采用L298N双H桥电机驱动模块，其输出电压直接关联输入电压范围（典型支持5V至35V），通过调节输入电源可灵活适配不同电机的功率需求。模块内置双通道驱动电路，可独立控制两路直流电机，结合PWM（脉宽调制）技术实现电机转速的无级调节与精准方向切换，例如在差速转向场景中，通过左右轮电机的差异化转速实现小半径转弯。L298N模块兼容逻辑电平信号输入（如树莓派GPIO输出的3.3V/5V控制信号），并集成过流保护与热关断机制，可在电机堵转或异常负载时自动切断输出，保障系统安全。电机驱动模块原理图如图3-4所示。图3-4L298N电机驱动原理图在电机驱动模块的连接与配置中，将树莓派与电机驱动控制模块连接，通过其输出端与小车驱动电机相连，应用脉宽调制进行模拟电路控制，其信号输入端被连接至树莓派的使能端，PWM负责输出控制脉冲信号，以便调节电机的转速REF_Ref195914657\r\h[10]。L298N的控制逻辑关系见表1，进一步明确了各控制信号与电机运行状态之间的对应关系。本智能车的电机采用的是金属齿轮直流电机，如图3-5所示。表1电机类型控制引脚状态电机动作直流电机IN1IN2停止00正转10反转01刹车/停止11图3-5直流电机实物图3.1.4红外避障红外避障传感器具备出色的环境光线适应性，由一对红外发射器和接收器构成。该发射器发出特定频率的红外光，当遇到障碍物时，红外光会被反射回接收器。接收到的信号经过内部比较器处理，触发绿色指示灯亮起，并输出一个低电平的数字信号。用户可以通过调整电位器来设定传感器的检测范围，该范围可从2厘米延伸至30厘米。传感器支持3.3伏至5伏的工作电压，以其低干扰、易于安装和操作简便的特性，适用于多种应用场景，如机器人导航、自动车辆避障、工业计数系统以及路径跟踪等。本智能车采用的是TCRT5000红外避障传感器。其实物图如图3-6所示，原理图如图3-7所示。图3-6TCRT5000红外避障传感器实物图图3-7TCRT5000红外避障传感器原理图3.1.5蓝牙模块蓝牙模块是一种无线通信模块，它遵循蓝牙技术标准，允许设备之间进行短距离的无线通信。蓝牙技术以其低功耗、低成本和简单的配对机制而广泛应用于各种电子产品和场景中。在本次实验中，我们利用蓝牙串口通信技术来操控小车的运动。小车搭载的传感器负责收集信息，并将这些数据传输至我们的手机应用程序进行展示。该应用程序通过蓝牙串口发送特定的串口指令，以实现对小车运动的精确控制，包括前进、后退、左转、右转、停止、左旋、右旋，以及对舵机进行任意角度的调整。此外，应用程序还能调节小车的速度，包括加速和减速。同时，小车上的传感器收集到的各类信息，如电池电量、两路红外避障传感器的状态，以及传感器测量的距离等，都会通过串口通信实时传输到手机应用程序的界面上，供用户查看和分析。3.2硬件结构设计3.2.1整车安装图树莓派4B视觉导航车的硬件结构设计以模块化理念为核心，通过多系统协同构建高效、稳定的智能移动平台。主控模块采用树莓派4B开发板，搭载4GB内存与ARMCortex-A72四核处理器，支持多线程并行处理，通过40针GPIO接口及CSI专用摄像头接口连接外设。视觉模块选用RaspberryPiCameraV2摄像头，支持1080P高清图像采集与自动对焦功能，结合二自由度云台（俯仰±45°、水平±180°）动态调整视角范围。驱动系统由L298N双H桥电机驱动板与金属齿轮直流减速电机组成，输入电压范围7.4-12V，最大输出电流2A，驱动四组麦克纳姆轮实现全向移动（平移、旋转、斜向运动），最小转弯半径趋近于零，适应复杂地形下的精准路径控制。电源管理系统采用7.4V/4000mAh锂电池组，避障功能依托TCRT5000红外传感器阵列，实时反馈障碍物位置数据。三段数显电压表实时监控电池电量，整体架构通过标准化接口设计（如I2C、UART）实现硬件扩展性。如图3-8所示。图3-8整车安装图4软件系统与算法设计4.1开发环境搭建4.1.1树莓派系统配置1、树莓派视觉导航车的操作系统安装流程首先需从树莓派基金会官网下载官方推荐的RaspberryPiImager工具，该工具支持Windows、macOS及Linux多平台运行，能够自动匹配最新版系统镜像（如RaspberryPiOS）并确保兼容性与安全性。用户启动Imager后，选择对应硬件型号（如树莓派4B）及所需操作系统版本（推荐64位Bullseye系统以发挥4GB内存性能），插入容量不小于16GB的MicroSD存储卡，通过工具界面一键完成镜像下载与烧录，镜像烧录工具如图4-1所示。烧录过程中需确保设备供电稳定，避免意外中断导致存储卡损坏，完成后可通过重新挂载存储卡检查/boot分区文件完整性。图4-1镜像烧录工具2、在VMware官网上下载软件如图4-2所示，为搭建树莓派视觉导航车的开发环境，需在本地计算机上部署虚拟机软件以模拟多系统协作场景。下载完成后，以管理员权限运行安装程序。安装过程中需暂时关闭杀毒软件及防火墙，防止驱动加载被拦截，完成安装后重启系统使配置生效。首次启动时需输入许可证密钥（免费版可跳过），随后通过新建虚拟机向导导入树莓派镜像文件，分配至少4GB内存与双核CPU资源，最终完成虚拟化环境的搭建，为后续交叉编译、远程调试及多平台兼容性测试提供基础支撑。图4-2VMware虚拟机4.2巡线避障4.2.1视觉巡线在舵机控制模块中，基于Adafruit_PCA9685驱动库实现双自由度舵机云台的精准控制。具体实施流程为：1）硬件初始化阶段，通过设定PWM脉冲宽度范围（500-2500μs）建立舵机机械角度映射；2）参数转换层，构建脉冲宽度与旋转角度的线性数学模型（θ=(pulse-500)/10.0），实现±90°范围的数字化角度控制；3）根据视觉导航需求，配置云台俯仰轴为35°俯角，水平旋转轴居中定位90°），确保摄像头光轴与巡线轨迹平面形成最优观测夹角。代码示例如下图4-3所示图4-3舵机初始化设置按键按下后进行读取视频，设置一个大小合适的检测窗口，将采集到的图像转化为灰度图像，对于摄像头采集到的图像按照目的分成两类，分别有不同的处理方式。REF_Ref195979668\r\h[11]然后进行一个二值化处理将灰度图转为黑白二值图，反色处理（INV）使线条变为白色，背景为黑色，便于轮廓检测，再进行高斯模糊消除噪点，为了使检测到的轮廓更加清晰准确，进行形态学操作REF_Ref195979677\r\h[16]，代码中对检测到的图像进行了两次腐蚀和两次膨胀，找到轮廓之后，再去找最大轮廓，求其中心位置属于哪一侧，来决定小车的行驶轨迹。代码示例如下图4-4所示。图4-4图像处理4.2.2红外避障本系统采用BCM（BroadcomSOCchannel）编码方式对树莓派GPIO进行物理层抽象，首先进行初始化，当按键按下后开始行动，若右侧为高左侧也为高表示附近没有障碍物，小车直行，若右侧为高左侧为低则表示左侧有障碍物，小车往右转，反之小车往左转，若两边都为低则表示左右都有障碍物，小车停止前进并后退一小段距离再左转再直行。代码示例如下图4-5所示。图4-5红外判断4.3视觉识别4.3.1手势识别手势识别算法通过构建多阶段视觉处理流水线实现实时交互功能。系统以30fps速率持续捕获视频流，首先在画面左上象限划定300×300像素的感兴趣区域（ROI），并叠加绿色边界框提供人机交互引导。基于HSV色彩空间的肤色概率模型，采用[0,20,70]至[20,255,255]的阈值范围进行肤色区域分割，该参数设置参照了人类肤色色度学研究成果，可有效覆盖95%以上亚洲人种肤色特征。在特征增强环节，通过形态学膨胀操作（5×5结构元）填充拓扑间隙，结合高斯滤波消除高频噪声，提升手部区域连通性。轮廓分析阶段，运用Douglas-Peucker算法对边缘轮廓进行多边形近似，通过凸包缺陷分析检测手指间隙，缺陷点用于计算手指数量，设定20像素深度阈值判定有效指间凹陷，同时建立轮廓面积与边界矩形面积的比率特征（arearatio），当比率超过0.8时判定为握拳姿态，形成几何-拓扑双重判据的决策模型，最后在画面中叠加手势识别结果。主要代码示例如下图4-6所示。图4-6图像处理4.3.2颜色识别及球体追踪小球追踪系统采用分层控制架构实现动态目标锁定功能。硬件层通过I2C协议（地址0x40）配置PCA9685模块，设定PWM频率为50Hz以确保舵机控制精度，初始化云台双舵机至预设位姿（底座舵机θ=90°±60°，俯仰舵机φ=35°±25°）。基于HSV色彩空间建立黄色目标检测模型（H∈[20,30],S∈[100,255],V∈[100,255]），该阈值范围经CIE1931色度图校准可覆盖95%标准黄色小球。在控制算法层面，构建增量式PID控制器：图像处理流水线包含高斯滤波（5×5核）、形态学闭运算（3×3椭圆核）等预处理，通过轮廓面积阈值筛选最大有效目标，计算质心坐标(x_c,y_c)作为反馈量。计算误差值，应用PID控制，调整舵机角度，使用多线程更新舵机位置，并显示图像。主要代码示例如下图4-7所示图4-7颜色识别及追踪4.3.3人脸识别及追踪人脸识别部分：人脸追踪系统基于级联分类器构建实时检测架构，初始化人脸检测模型。硬件层集成二自由度舵机云台与CSI摄像头模块，配置摄像头水平镜像模式以符合人机交互直觉。在视频处理流水线中，系统捕获图像帧，经灰度转换后应用滑动窗口多尺度检测算法，通过Haar特征分类器实现人脸区域定位。对每个检测到的人脸区域（蓝色边界框），进一步采用detectMultiScale分类器进行眼部特征定位（绿色子区域框），构建双眼连线向量作为头部姿态估计基础。系统实时计算人脸区域质心坐标(x_c,y_c)，通过坐标变换将图像坐标系映射至舵机控制角度（θ,φ），可视化界面显示处理后的图像。代码示例如下图4-8所示。图4-8人脸图像处理人脸追踪部分：代码在一个无限循环中运行，不断从摄像头捕获帧。然后，将每一帧从BGR颜色空间转换到HSV颜色空间，使用Haar级联分类器检测人脸，返回人脸的位置和大小。然后，机器人停止的指令被调用，无论是否检测到人脸，这可能导致机器人一直停止，除非检测到人脸才会执行后续动作。在检测到人脸的情况下，代码会绘制矩形框，并计算人脸的中心的X坐标。然后，设定了一个X的范围（150到180），根据x_p的值决定机器人前进、左转或右转。代码示例如下图4-9所示。图4-9人脸追踪4.4WiFi遥控手机或电脑和小车连在同一个WiFi下就可以实现远程控制，代码的程序语言为C语言，代码主要分为几个部分：信号处理、网络初始化、客户端连接管理、指令解析和电机控制。首先，信号处理部分，当用户按下Ctrl+C时，程序会停止电机并退出。然后是网络初始化的部分，主函数处理命令行参数，初始化Socket服务器，绑定端口，使用TCPSocket，接受客户端连接，读取命令并执行相应的动作。，并使用select多路复用处理多个客户端连接。指令解析部分，根据接收到的不同指令（如“ONA”、“ONB”），调用相应的电机控制函数。通过Motor_Init初始化电机，根据接收到的命令调用不同的运动函数，如t_up（前进）、t_down（后退）等。代码详情见附录，控制指令如下图4-10所示。图4-10控制指令在树莓派终端中使用管理员指令，获取机器人的ip端口号，如下图4-11所示，用于在浏览器上共享显示机器人摄像头的实时画面。如图4-12所示图4-11Linux指令图4-12网页显示摄像头开启摄像机打开之后再打开控制端，如图4-13所示设置生成命令。图4-13设置生成命令设置编译成功之后便可以在手机和电脑上操作界面控制小车，电脑操作界面如图4-14所示，手机操作界面如图4-15所示。图4-14网页端控制界面图4-15手机端控制界面5实验与成果展示5.1实验环境与测试场景视觉识别部分在教室完成，循迹避障部分需独立搭建测试环境。测试环境需在平整地面铺设宽度3cm的黑色胶带模拟巡线轨迹，沿线随机放置障碍物，环境光照为正常室内光线。小车需在直道、弯道（半径≥30cm）+稳定跟踪轨迹，平均偏移误差≤5cm，遇障后500ms内完成绕行并重新锁定路径。极端条件下（如强逆光或低照度），可启用摄像头宽动态模式（WDR）补偿光照影响，并通过调整形态学操作参数优化鲁棒性。测试环境如图5-1所示图5-1测试环境图5.2成果展示5.2.1巡线避障智能巡线小车从起始点启动后沿预设黑色轨迹直线行驶，当转角红外避障传感器实时监测到前方障碍物时，控制系统立即触发转向机制执行左转规避动作。完成避障后，小车通过视觉识别模块启动摄像头进行环境扫描，利用图像处理算法精准定位黑线位置。通过PID控制算法优化转向角度，小车重新校准路径后恢复沿黑线巡行模式，持续执行自主避障与循迹相结合的复合型导航任务。具体过程如图5-2所示。图5-2巡线避障过程图5.2.2手势识别手势识别算法通过构建多阶段视觉处理流水线，在实时性与准确性之间实现了有效平衡。系统持续处理视频流，在画面的交互区域内，通过绿色边界框引导用户完成手势输入。基于HSV色彩空间的肤色分割模型采用[0,20,70]-[20,255,255]阈值范围，在室内外光照条件下均表现出良好的适应性，形态学膨胀操作与高斯滤波的组合处理使手部区域连通性提升，其中5×5结构元的选择在消除拓扑间隙的同时保留了手指细节特征，避免了过度膨胀导致的形态失真，配合凸包缺陷检测使手指计数准确率达到91.7%，有效区分真实指间凹陷与噪声干扰。实验结果如图5-3所示。图5-3手势识别成果图5.2.3人脸识别及追踪人脸识别系统通过构建多层级视觉处理架构实现高精度身份认证，其核心架构采用级联检测与深度特征融合的技术路线。系统初始化阶段加载预训练Haar级联模型，基于Haar特征的分类器采用[1,1,3]至[2,2,5]的阈值窗口参数，在256×256像素检测窗口内实现人脸轮廓的快速定位。针对检测到的人脸区域（蓝色边界框），系统通过直方图均衡化增强对比度。可视化界面实时框选人脸及眼睛，如图5-4所示。图5-4人脸识别成果图人脸追踪系统通过构建实时视觉处理流水线实现动态目标跟随，其核心架构采用Haar级联检测与坐标映射控制相结合的技术方案。系统初始化阶段加载预训练人脸检测模型，在无限循环中持续捕获BGR图像帧，经HSV色彩空间转换后，利用滑动窗口多尺度检测算法定位人脸区域，当检测到人脸时绘制蓝色边界框，并计算其中心坐标x_p，未检测到目标时持续触发机器人停止指令。实验结果显示，在1.5米距离范围内，系统识别到人脸后控制舵机和电机对人脸进行追踪，如图5-5所示。图5-5人脸追踪实际效果图5.2.4球体识别（球体追踪与人脸追踪同理）小球颜色识别系统通过精准的色彩空间建模与硬件协同控制，实现了对运动目标的可靠检测。系统基于HSV色彩空间建立的黄色检测模型（H∈[20,30],S∈[100,255],V∈[100,255]）经过CIE1931色度图校准，可覆盖95%标准黄色光谱范围，在实验室标准光源下可以准确识别到黄色小球并追踪。该阈值设置通过限制饱和度和明度下限（S≥100，V≥100），有效规避了低光照环境下的色彩失真问题。实验结果如图5-6所示。图5-6球体识别成果图5.2.5Wifi遥控移动端与网页端实现控制指令同构化，如图5-7所示的返回值结构验证了指令状态码与设备反馈数据的标准化映射关系。图5-7系统返回值6结论与展望6.1结论本文基于树莓派4B平台成功设计并实现了一款具备多模态交互能力的视觉导航智能小车系统。通过融合OpenCV视觉处理算法、PID控制策略及多传感器协同技术，系统实现了以下核心功能：视觉导航与巡线：基于灰度化、霍夫变换的巡线算法在标准测试场景下达到90%的路径跟踪精度，验证了低成本视觉方案的可行性；动态避障与追踪：红外传感器与视觉算法协同工作，在障碍物距离30cm内可实现95%的有效避障，黄色小球追踪与人脸跟随功能响应延迟低于200ms；多模态交互：集成手势识别、WiFi远程控制，构建了完整的智能交互生态，支持手机端实时画面传输与控制指令下发；实验结果表明，系统在实验室环境下综合性能稳定，硬件成本控制在800元以内，较传统激光雷达方案降低60%，具备较高的工程应用价值。然而，在复杂光照场景中视觉算法的鲁棒性仍有提升空间，且多任务并行处理时存在CPU资源竞争问题。6.2展望未来研究可从以下方向深入优化：算法增强：引入YOLOv5等轻量化深度学习模型，提升动态目标识别准确率；结合SLAM技术实现三维环境建模，增强复杂场景适应性，目前的视觉识别还无法在复杂环境中进行识别；多传感器融合：集成毫米波雷达与IMU模块，通过卡尔曼滤波实现多源数据融合，提高导航精度与安全性；算力升级：采用树莓派5或JetsonNano提升边缘计算能力，支持4K视觉处理与实时语义分割；能效优化：设计低功耗电源管理方案，结合太阳能充电技术延长续航时间至8小时以上。参考文献张红霞.国内外工业机器人发展现状与趋势研究[J].电子世界，2013汪婉君.基于Android/Arduino平台的智能蓝牙引路系统的设计与实现[D].西安电子科技大学，2014吴波涛等.基于Arduino和树莓派的智能小车的设计与实现[J].电子设计工程，2017MarkLutz.PythonStandardLibrary.[J].机电信息,1998.2何立民.单片机技术的现状与未来[J].中国计算机报,2016年No：30.胡汉才.单片机原理与接口技术[M].北京：清华大学出版社，2018.4.段立辉.openmvcomputervisionwithpython.[J].机电信息,2014.1符强等.基于手机蓝牙的遥控小车的设计[J].现代计算机(专业版)，2011叶政韩，吕文涛，郭庆，等基于树莓派的识别智能车控制系统［J］智能计算机与应用，2025.2张秉森，马吉忠，杨一飞，等.基于树莓派的自动避障小车的设计与实现〕].南方农机，2o2i}s2(3>:23-24马志德,杜世龙,周永帅,等.多功能智能小车的设计与应用分析[J].科学技术创新,2020(31):156-157.孔德肖,张晴晖,李俊萩,等.基于STM32与树莓派的视频采集智能车系统设计[J].仪表技术与传感器,2020(12):63-66.曲霄红,张名师,胡立华.基于树莓派的智能小车路径规划方法及系统研究[J].山西大同大学学报(自然科学版),2020,36(3):48-51,81.黄姝娟,刘萍萍,白小军，等.基于STM32的智能车控制系统的设计与实现[J].计算机技术与发展,2018,28(9):185-188.黄姝娟,刘萍萍,白小军，等.基于STM32的智能车控制系统的设计与实现[J].计算机技术与发展,2018,28(9):185-188.SerraJ.ImageAnalysisandMathematicalMorphology[M].AcademicPress,1982.WangWei.Designof IntelligentCar BasedonMultisensor[J].JournalofAppliedSciences，2013附录树莓派电路图扩展版原理图Wifi遥控部分代码/**************************************************@parCopyright(C):2010-2019,hunanCLBTech*@filecar_server.c*@versionV1.0*@details*@parHistory@author:zhulin***************************************************/#include"LOBOROBOT.h"#include<signal.h>#include<stdio.h>#include<stdlib.h>#include<unistd.h>#include<errno.h>#include<string.h>#include<netdb.h>#include<sys/types.h>#include<time.h>#include<sys/socket.h>#include<arpa/inet.h>#defineBUFSIZE512voidHandler(intsigno){//SystemExitprintf("\r\nHandler:MotorStop\r\n");Motor_Stop(MOTORA);Motor_Stop(MOTORB);Motor_Stop(MOTORC);Motor_Stop(MOTORD);DEV_ModuleExit();exit(0);}floatlr_detection=90;floatqh_detection=90;typedefstructCLIENT{ intfd; structsockaddr_inaddr;}CLIENT;intmain(intargc,char*argv[]){intsockfd;intlistenfd;intconnectfd;intret;intmaxfd=-1;structtimevaltv;structsockaddr_inserver_addr;structsockaddr_inclient_addr;socklen_tlen;intportnumber;charbuf[BUFSIZE];intz,i,maxi=-1;fd_setrset,allset;CLIENTclient[FD_SETSIZE];//电机初始化Motor_Init();set_servo_angle(10,lr_detection);set_servo_angle(9,qh_detection);//初始化云台 /********PWM控制*********************/if(argc!=2){printf("Pleaseaddportnumber!");exit(1);}if((portnumber=atoi(argv[1]))<0){printf("EnterError!");exit(1);}if((listenfd=socket(PF_INET,SOCK_STREAM,0))==-1){printf("SocketError!");exit(1);}memset(&server_addr,0,sizeofserver_addr);server_addr.sin_family=AF_INET;server_addr.sin_addr.s_addr=htonl(INADDR_ANY);server_addr.sin_port=htons(portnumber);if((bind(listenfd,(structsockaddr*)(&server_addr),sizeofserver_addr))==-1){printf("BindError!");exit(1);}if(listen(listenfd,128)==-1){printf("ListenError!");exit(1);}for(i=0;i<FD_SETSIZE;i++){ client[i].fd=-1;}FD_ZERO(&allset);FD_SET(listenfd,&allset);maxfd=listenfd;printf("waitingfortheclient'srequest...\n");//Exceptionhandling:ctrl+csignal(SIGINT,Handler);while(1){ rset=allset; tv.tv_sec=0;//wait1usecondtv.tv_usec=1;ret=select(maxfd+1,&rset,NULL,NULL,&tv); if(ret==0) continue; elseif(ret<0) { printf("selectfailed!"); break; } else { if(FD_ISSET(listenfd,&rset))//newconnection { len=sizeof(structsockaddr_in); if((connectfd=accept(listenfd,(structsockaddr*)(&client_addr),&len))==-1) { printf("accept()error"); continue;} for(i=0;i<FD_SETSIZE;i++) { if(client[i].fd<0) { client[i].fd=connectfd; client[i].addr=client_addr; printf("Youtgotaconnectionfrom%s\n",inet_ntoa(client[i].addr.sin_addr)); break; } } if(i==FD_SETSIZE) printf("Overflyconnections"); FD_SET(connectfd,&allset); if(connectfd>maxfd) maxfd=connectfd; if(i>maxi) maxi=i; } else { for(i=0;i<=maxi;i++) { if((sockfd=client[i].fd)<0) continue;if(FD_ISSET(sockfd,&rset)) { bzero(buf,BUFSIZE+1); if((z=read(sockfd,buf,sizeofbuf))>0) { buf[z]='\0';printf("num=%dreceiveddata:%s\n",z,buf); if(z==3||z==6) { if(buf[0]=='O'&&buf[1]=='N') { switch(buf[2]) { case'A':t_up(50,0);printf("forward\n");break; case'B':t_down(50,0);printf("back\n");break; case'C':moveLeft(50,0);printf("left\n");break; case'D':moveRight(50,0);printf("right\n");break; case'E':t_stop(0);printf("stop\n");break; case'I':lr_detection+=10;if(lr_detection<=0)lr_detection=0;if(lr_detection>=180)lr_detection=180;set_servo_angle