网球底线区域智能机器人系统设计

[9]内部特意设置了过热保护与低压检测电路，PWM控制的频率可高达100kHz121。如图3.5所示。图3.5电机H桥驱动部分电路3.7蓝牙电路设计本设计通过蓝牙将数据发送至手机，同时手机也可以发送指令到单片机进行控制。HC-05是一款经典的蓝牙2.0模块，该模块具有性价比高、体积小巧、功耗低、收发数据稳定的特点，其内部的芯片支持SPP蓝牙协议。HC-05蓝牙的实物图如图3.6所示，其具体的参数如下：(1)供电电压：3.6V-6V，工作电流：10-50mA。(2)通信频段：2.4GHz。(3)通信接口：TTL异步串口，默认波特率为9600。(4)配置方式：AT通信指令。图3.6HC-05蓝牙模块实物图HC-05蓝牙模块具有6个对外接口，这是设计蓝牙通信电路的基础，下面对模块的通信接口进行介绍：(1)VCC/GND:模块工作供电引脚，本设计使用的5V电源供电，供电的电流为500mA。(2)通过连接RX/TX模块的串口输入/输出接口与单片机的串口，实现了数据通信的无缝衔接。(3)STATE：用于输出系统工作状态的模块状态指示引脚，以指示系统的运行状态(4)ENB:模块使能引脚，使能通过AT指令进行控制，当ENB接3.3V后，模块进入AT配置模式。图3.7是HC-05电路原理图，图中U30是HC-05蓝牙模块，模块的VCC和GND与板子的5V连接。HC-05模块的RX连接单片机的PA9口，HC-05的TX连接单片机的PA10口。图3.7HC-05电路原理图3.8收集装置我设计的捡球机器人主要是由舵机与小框组成。当摄像头识别到网球时，机械臂会自动将网球拾取进小框内。舵机电路如图3.8所示。图3.8舵机电路3.9处理器系统捡球机器人不光要看见识别网球更主要的是要让他具有一定的自我选择的权力，必须具有清晰的头脑，所以我们给他安装了较为强大的处理系统，能够更加准确的识别网球从而进一步捡球。一般的处理系统由微控制器如DSP、单片机、ARM等组成。在捡球机器人的处理过程中，机器人的处理器系统包含了上机位和下位机。上机位可以更加清楚的收集数据和信息，从而更进一步的将控制指令发送到我们的运动系统上。软件采用了对网球识别的图像显示。通过对捡球机器人的进一步处理（如上下移动、左右转弯、机械臂捡球等）和一些传感器的进一步处理和计算。通过串行连接等方式从而进行通信。3.10传感器系统捡球机器人在对场上进行获取到所有情况，故而我安装了传感器来获取信息。对机器人的内部结构来说，传感器是必不可上的一个重要组成部分。传感器系统分别由内部传感器和外部传感器组成。内部传感器我们通过编程来解决，外部传感器是有视觉传感器的重要组成部分。在机器人不清楚场上的情况时，我们通过视觉传感器来获取相应的信息。我设计的捡球机器人带有一个摄像头，用于进行的信息收集；两个增量型光电编码器，主要用于驱动轮进行速度检测。2.11执行机构对于我设计的这个捡球机器人来说，要完成我交给他的任务就必须有自我的执行能力，捡球机器人的执行方式主要由运动结构和捡球结构组成，目前的带有轮毂的机器人有很多，他们一般都装有一个电机，另一种就是有两个轮毂的，它们是通过驱动电机来控制的一个点机用来转动，另一个电机用来控制方向，而我设计的这个是通过履带来控制的。第4章系统软件设计4.1系统软件总体设计本机器人由全金属外壳机械臂，纯铝合金底盘，金属齿马达，履带轮，金属电机，大手爪，电流驱动，移动电源等部位组成，我用于设计的这一款捡球机器人是通过摄像头来进行分辨网球，在第一时间发现网球的时候会把网球的位置通过控制软件来反馈，控制软件会通过履带来推动机器人的运动从而进一步的靠近目标，当机器人靠近网球时会通过机械臂来控制并拾取网球，从而通过机械臂放进事先准备好的框里。当完成第一次拾取后，机器人会继续进行的同样的运动从而实现捡球工作。如图4.1所示。图4.1系统总体结构框图4.2测距避障功能为了实现对网球的测距，需要通过单片机向传感测距模块控制端发出10μs的高电平信号，随后从接收端检测信号是否返回。当在设定好的时间段内没有接收到信号时，单片机就会产生一个低电平信号。若有，则向接收端发出高电平信号并激活定时器计算高电平持续时间，再通过公式(距离=(时间*声速)/2,声速340m/s)进一步计算捡网球机器人与障碍物的距离。对于网球进球机器人避障功能，提前由程序设计规则将安全距离定为10cm。如果前方障碍物的距离在10cm以内，则立刻停止行进，这时舵机驱动传感测距模块旋转以测量左右两个障碍物之间的距离，以选择朝向距离大的方向旋转；如左、右障碍物间距均在20cm以内，作后退处理。如图4.2所示为测距避障流程。图4.2测距避障流程4.3网球识别功能实现OpenMV4摄像头识别图像并转为灰度的图像，规定的阈值并切割图像。通过降噪等预处理加强网球图像的重要特征，然后提取网球的轮廓、长度、宽度、面积、颜色等各种特征值。采取图像梯度的方法来获取网球轮廓，分别产生二值图的边缘来提取图提取网球面积和周长。采集网球图片的素材制作数据集并训练模型，使用网络ResNet来作为基石，模型的训练要用生成器来将数据集循环进而写入内存，同时图像增强以泛化模型REF_Ref19312\r\h[12]。通过训练能够得到一种网球的识别规则来进行网球识别，模块内部的处理器经过分类来进行决策后就能够进一步识别到网球，从而得出网球的具体位置，采用视差测距法来计算网球的距离REF_Ref19241\r\h[13]，然后进一步传递给单片机。如图4.3所示为网球识别流程。图4.3网球识别流程4.4控制电路总体结构设计我设计的网球机器人的控制电路整体结构设计包括以下几个部分：1.控制器：控制器是控制电路的核心部分，它的功能是用于接收用户的指令，从而控制各个电器元件的工作。控制器也可以选择单片机或者嵌入式系统，可以通过对机器人的复杂度和功能要求选择不同的控制器型号和配置。2.电源管理模块：通过为整个电路提供电源，他们中也包括自主供电和外部供电两种模式，进而满足机器人运行所需的电源需求。在电源模块中，还需要添加过压保护、电流保护、电源开关、电池充电等一系列功能。3.传感器：传感器是通过机器人的重要的硬件。通过对传感器，机器人可以来感受运动过程中他们的位置、速度、方向等信息。在网球机器人中，需要加入重量传感器、距离传感器、方向传感器等。4.驱动模块：驱动模块是机器人运动过程中所必不可少的一部分，包括电机驱动、机械臂驱动和轮子驱动等功能。要实现机器人运动的平稳性和精确性，需要使用高精度的电机，可以选择步进电机，直流电机等。保护模块：保护模块是机器人安全保障的最后一道保护措施，可以设置紧急停止按钮和电路保护器等。4.5蓝牙模块蓝牙通信软件子程序是通过串口进行透传发送接收数据的，通过调用串口发送函数printf()发送数据到HC-05模块，如果HC-05模块接收到数据后，单片机通过串口中断函数进行接收，接收的数据放到串口结束数据缓存数组USART_RX_BUF中，主程序通过对数组进行校验，如果校验正确择根据数组解析的数据，进行相应的程序执行，之后子程序返回。

第5章系统调试5.1软件调试软件调试是以硬件调试为根本进行的，需要保证硬件电路没有问题，是调试过程中相对复杂的过程，很多问题不容易被仪器测试出来。软件调试的思路与硬件调试思路完全不同。既需要语言语法基础又需要一定的编程经验。本设计采用的是C语言，调试程序第一步就是将C语言程序中的语法错误部解决好。本设计使用的Keil软件进行程序编写，在编写编译过程中会进行语法检测，可以实现大部分语法问题的调试。但是程序的驱动和程序的执行逻辑是无法通过软件进行识别的，要结合测试现象、调试结果和设计者经验进行一步一步的修改尝试，直到使程序正常运行。其中常见的问题比有数组越界、内存混乱、子程序之间相互干扰等问题。这些问题需结合硬软件调试工具进行逐步的探索和解决问题。在对IIC子程序进行调试时，通过软件编译以后，显没有错误可以正常进行下到单片机中，单片机运行程序以后出现了系统失灵，显示错误，系统死机，无法执行后边的程序。经过断开电源重新启动之后仍然现前面的现象。Keil软件提供了程序调试器和单步执行功能。在出问题的子程序上，通过设置断点，判断每一步执行以后每个变量的数值是多少，通过watch窗口可以看到出现问题的变量是哪个，再结合变量的实际位置，进定位出现问题的程序语句，实现程序的寄存器、变量等数据的监测。在程序运行一个for循环语句的时候，指针变量访问到了数组规定范围以外的位置，发生了越界，数组定义的偏离位置最高位8位，但指针在访问过程却访问到了第9位，将其他变量的数据进行了异常修改。导致执行到这条语句以后，使程序崩溃，无法进行运行。程序整个内存中数据发乱，系统无法正常执行，导致程序指令混乱死机，图5.1为程序调试过程的截图，通过调试解决了程序中存在的数组越界问题，使程序正常运行，达到了的预期的效果。图5.1子程序仿真调试图通过使用软件内置的调试器发现了这个问题。这个问题的解决方案是对读取数组过程中的偏移变量进行排查，防止越界读写数据。数组越界是很常见的软件编写错误，造成这种问题的原因可能是程序编写人员在输入的时候输入了错误的计数值，也有可能是没有确定好数组的具体范围。本次设计出现了数组越界以后对程序其他代码也进行了检查，尤其是对使用循环语句访问数组，进行重点关注，确保程序中其他位置不会出现类似的问题。5.2实物调试图5.2是捡球机器人的实物图。机器人上方通过机械臂的表面来安装摄像头与蓝牙模块，尾部由机械臂的底部来支撑所有的机械设备，从而保证各个模块直接不会发生故障，进而可以更好的完成测试工作。通过测试，我设计捡球机器人可以通过摄像头来精准的抓取网球，同时，在路线上有一定的障碍时，我们的机器人可以正确的判断出正确的路线，从而实现自主避障。此外，我们可以通过蓝牙模块来连接手机，从而进行远程操作。图5.2是捡球机器人的实物图图5.3时捡球机器人自主避障的测试结果。通过表上的数据我们可以直观的发现机器人的避障功能有了更好的提高，图5.4为捡球率的测试。通过测试表明，我们的机器人在通过对光照的强度、网球的密度、网球的位置等环境方面的影响，进而我们进行了更加便利的改变。图5.3自主避障的测试结果图5.4为网球捡拾率测试结果随机在场馆放40个球进行多次实验，来记录机器人开始工作及工作后的时间、拾球个数，测试结果如图8所示．场馆网球数越多，机器人移动速度就会越慢，随着地上网球数量的逐步减少，网球机器人的移动速度会进一步增加．实验结果来表明拾球机器人抓取精度能达到97%．如图5.5所示。图5.5测试结果第6章总结与展望由于目前市面上的智能网球收集机器人还是寥寥无几，已经有的设备大多是简单的辅助捡球器，而不能实现高效自主收集网球，对于正在训练的运动员和业余爱好者来说，拾取散落在球场上的网球是繁重的体力劳动，雇佣球童费用又较高。针对拾球难的问题，进而对网球收集机器人的关键技术研究，试图研制一款基于视觉的智能网球收集机器人，代替人工实现自主收集网球。网球底线区域智能捡球机器人系统设计虽然已有一定的研究成果，但仍然存在一些问题和挑战。以下是一些主要的问题和发展趋势：1.精度问题：当前技术虽然可以精确识别网球的位置，但在捡球过程中机器人的机械臂和抓取器的精度还存在一定的问题，捡球不够灵活和准确。2.移动速度问题：机器人在捡球过程中需要快速移动，否则会影响比赛的进程，但目前机器人的移动速度还不能达到专业选手和人类的速度。目前智能捡球机器人的研发和制造成本较高，需要更多的技术创新和成本优化。未来，智能捡球机器人的发展趋势将有以下几个方面：1.技术创新：机器人技术将不断创新和完善，更加精准的识别技术、更高效的移动速度或更灵活的机器臂等新技术将不断涌现。2.应用拓展：初步的研究表明，智能捡球机器人不仅可以在网球比赛中使用，还可以在其他球类和运动比赛中使用，例如，足球、篮球和棒球等。3.集成化应用：随着人工智能、云计算和大数据技术的发展，智能捡球机器人将与其他解决方案集成，为运动员和球迷提供更多智能服务，如实时数据分析、球员排名和比赛回放等。综上所述，虽然智能捡球机器人的研发还面临许多问题和挑战，但其发展前景广阔，具有无限的潜力和探索空间。网球底线区域智能捡球机器人系统设计有以下几个发展趋势：1.多功能化：未来的智能捡球机器人将不仅仅是处理网球的捡球任务，而是要具备更多的功能，例如，可以帮助球员训练、提供数据分析和反馈等，这将为球员提供更好的训练和比赛体验。2.自主导航技术的提高：自主导航技术是智能捡球机器人实现自主行动的核心技术，未来的智能捡球机器人将具备更加先进的自主导航技术，可以在不同的场地和环境下，更加灵活地完成任务。3.环保化和可持续性：环保化和可持续性是未来机器人发展的趋势，未来的智能捡球机器人将越来越注重环保，采用更加环保和节能的技术，同时具备更长的使用寿命，减少对环境的影响。4.互联网+：智能捡球机器人将与互联网技术紧密结合，未来可以通过云计算和大数据技术，实现数据的共享和分析，打造更加智能化、集成化的服务系统，为用户提供更好的服务和体验。5.个性化和定制化：未来的智能捡球机器人将越来越注重个性化和定制化，可以根据不同用户的需求和场地环境进行个性化定制，提供更加适合的服务和解决方案。综上所述，未来网球底线区域智能捡球机器人系统设计将不断发展和进步，不仅具备更高的精度、速度和自主性，还将具备更多的功能。参考文献段元奕,刘家博,沈怡平.基于嵌入式系统的捡球机器人设计[J件,2022,43(05):162-165.覃静,孙霞.基于STM32单片机的捡球机器人设计[J].计算机与网络,2021,47(22):60-62.马飞.一种智能捡球机器人的多传感器数据采集系统设计[J].信息技术与信息化,2021(09):243-245.章明干.AI捡球机器人[J].中国科技教育,2021(07):64-65.任云青.智能乒乓球自动捡球机器人的设计与实现[D].南京邮电大学,2020.陈青华,左太虎,徐一红.基于OpenMV的智能捡球机器人的设计与实现[J].信息技术与信息化,2020(10):210-213.陈章宝,侯勇.网球捡球机器人视觉导航系统设计[J].廊坊师范学院学报(自然科学版),2020,20(02):42-45.刘永兵.基于SoC的羽毛球捡拾机器人设计与实现[D].重庆大学,2020.普邑,杨哲,王新勇.基于OpenMV的捡球机器人系统设计[J].山西电子技术,2020(01):70-73.陈卫,邓志良.捡球机器人路径规划算法研究及仿真[J].计算机仿真,2020,37(10):291-296.彭磊.视觉导航的自主捡球机器人设计与开发[D].东南大学,2019.DOI:10.27014/ki.gdnau.2019.003512.郑植,席先鹏,王楠,蒋欣燎,夏山淋.机器视觉在网球捡球机器人中的应用研究[J].科技创新与应用,2019(16):34-35.马天月.基于ESP8266WiFi芯片的物联网模块的设计与实现[D].上海：上海交通大学，2017.盖荣丽，蔡建荣，王诗宇，等.卷积神经网络在图像识别中的应用研究综述[J].小型微型计算机系统，2021，42（9）：1980-1984.姜映舟.基于计算机视觉的测距技术研究[D].桂林：桂林电子科技大学，2019.黄玉金，杨越，薛伟，等.无线模块的AT指令UDP透传设计[J].电子产品世界，2018，25（1）：34-36.LiJiaxin.DesignofIntelligentDaylilyPickingRobot[J].HanJinghai.MultimediaMotionofPickingRobotBasedonCooperativeRelationshipofMatchingGradientAlgorithm[J].AdvancesinMultimedia,2021,2021.TangZuoliang,XuLijia,WangYuchao,KangZhiliang,XieHong.Collision-FreeMotionPlanningofaSix-LinkManipulatorUsedinaCitrusPickingRobot[J].AppliedSciences,2021,11(23).Zhang,Huan,Yu,Li.Cantilevermulti-axiscontrolmethodforanewarmpickingrobot[J].InternationalJournalofSystemAssuranceEngineeringandManagement,2021(prepublish).LiuPeng,QiaoXinzhou,ZhangXuhui.Stabilitysensitivityforacable-basedcoal–ganguepickingrobotbasedongreyrelationalanalysis[J].InternationalJournalofAdvancedRoboticSystems,2021,18(6).Miki,Kohei,Nagata,Fusaomi,Ikeda,Takeshi,Watanabe,Keigo,Habib,MakiK..Moldedarticlepickingrobotusingimageprocessingtechniqueandpixel-basedvisualfeedbackcontrol[J].ArtificialLifeandRobotics,2021,26(4).TheveninSimon,MebarkiNasser,ChatellierPauline.Dynamicschedulingofapickingrobotwithlimitedbufferandrejection:anindustrialcasestudy[J].InternationalTransactionsinOperationalResearch,2021,29(3).附录程序#Untitled-By:mengfei-周一2月132023importsensor,time,image,lcd,mathfrompybimportServo,UART,Pin,TimerfrompidimportPID#自动搬运小车数据协议：0x00停止;0x01前进（机械臂向上）;0x02后退（机械臂向下）；0x03左转（机械臂向左）；0x04右转（机械臂向右）；#0x05循迹（自动搬运）；0x06跟随（自动捡球）；0x07机械臂控制（上下左右按钮用来控制机械臂）；0x08释放；0x09抓取#小车状态全局变量#定义小车状态数据结构classCARSTATE(object):enSTOP=0enRUN=1enBACK=2enLEFT=3enRIGHT=4enRELEASE=5enCATCH=6enMANUAL=7enTRACING=8enTRAKING=9enAVOIDING=10#globalg_carstate,g_cardir,power,ball_sg_carstate=CARSTATE.enTRAKINGg_cardir=CARSTATE.enSTOPpower=800ball_s=0#串口uart1=UART(1,9600)#PA9,PA10#globalbluetooth_stutas,key_state,key_event,G_FLAGbluetooth_stutas=0G_FLAG=0key_state=0key_event=0key=Pin('D8',Pin.IN,Pin.PULL_UP)THRESHOLD=[(40,96,-9,-38,20,57)]#黄绿色网球#THRESHOLD=[(53,99,-13,46,29,57)]#黄色乒乓球##生成50HZ方波，使用TIM3，channels1,2#控制舵机需要输出2.5%-12.5%的PWM#0度0.5/20*100=2.5;(要看实际的舵机是否支持0度，一般运动不到0度，需要把这个值调大一点)#45度1.0/20*100=5;#90度1.5/20*100=7.5;#135度2.0/20*100=10;#180度2.5/20*100=12.5;#生成50HZ方波#控制舵机需要输出2.5%-12.5%的PWMtim=Timer(2,freq=50)#Frequencyin50Hz(20ms)claw=tim.channel(3,Timer.PWM,pin=Pin("A2"),pulse_width_percent=7.5)#初始角度90度globalclaw_offset#定义机械爪中心和摄像头图像中心偏移位置claw_offset=(-20)claw_release=160claw_catch=10count=0#控制舵机需要输出2.5%-12.5%的PWMpan_servo=Servo(1)#左右控制tilt_servo=Servo(2)#上下控制#PID云台PID,小车跟随PID，小车循迹PIDpan_pid=PID(p=0.2,i=0,imax=90)#脱机运行或者禁用图像传输，使用这个PIDtilt_pid=PID(p=0.15,i=0,imax=90)#脱机运行或者禁用图像传输，使用这个PIDdis_pid=PID(p=0.9,i=0.01)len_pid=PID(p=0.6,i=0.01)#Untitled-By:mengfan_zheng-周日5月82022############################电机驱动部分#############################如果上电时出现电机异常运转，是因为PB4引脚默认为JTAG功能，更新到最新日期的固件可解决这个问题pwmA1=Pin('B0')#TIM3_CH3pwmA2=Pin('B1')#TIM3_CH4pwmB1=Pin('B4')#TIM3_CH1pwmB2=Pin('B5')#TIM3_CH2#定义定时器3，频率为1000Hz，即1ms一次定时器中断tim=Timer(3,freq=1000)#初始化PWM，通过调节PWM控制左右轮速度，初试占空比为0（车静止）Left1=tim.channel(4,Timer.PWM,pin=pwmA1,pulse_width_percent=0)Left2=tim.channel(3,Timer.PWM,pin=pwmA2,pulse_width_percent=0)Ritht1=tim.channel(1,Timer.PWM,pin=pwmB1,pulse_width_percent=0)Ritht2=tim.channel(2,Timer.PWM,pin=pwmB2,pulse_width_percent=0)#定义机械爪舵机运动角度#输入：舵机运动到目标角度#输出：无defclaw_angle(servo_angle):ifservo_angle<=0:servo_angle=0ifservo_angle>=180:servo_angle=180percent=(servo_angle+45)/18claw.pulse_width_percent(percent)#定义舵机运动#输入：舵机运动的起止角度#输出：无defservo_move(servo_n,start_angle,end_angle):ifstart_angle>=end_angle:while(start_angle>=end_angle):servo_n.angle(start_angle-2)time.sleep(50)start_angle=start_angle-2return0ifstart_angle<=end_angle:while(start_angle<=end_angle):servo_n.angle(start_angle+2)time.sleep(50)start_angle=start_angle+2return0#功能：小车控制#输入：左，右速度#输出：无defrun(left_speed,right_speed):#print("left_speed",left_speed)#print("right_speed",right_speed)ifleft_speed<-100:left_speed=-100elifleft_speed>100:left_speed=100ifright_speed<-100:right_speed=-100elifright_speed>100:right_speed=100ifleft_speed>0:Left1.pulse_width_percent(abs(left_speed))Left2.pulse_width_percent(abs(0))else:Left1.pulse_width_percent(abs(0))Left2.pulse_width_percent(abs(left_speed))ifright_speed>0:Ritht1.pulse_width_percent(abs(right_speed))Ritht2.pulse_width_percent(abs(0))else:Ritht1.pulse_width_percent(abs(0))Ritht2.pulse_width_percent(abs(right_speed))#功能：寻找最大的目标，计算方式为像素面积排序#输入：链表objects，坐标X,坐标y,像素宽,像素高#输出：最大目标元组deffind_max_object(objects):max_size=0forobjectinobjects:ifobject[2]*object[3]>max_size:max_object=objectmax_size=object[2]*object[3]returnmax_objectdefclaw_auto_catch(img,ball_s,max_blob):globalcountpan_error=0tilt_output=0title_angle=0pan_error=img.width()/2-max_blob.cx()tilt_error=(img.height()/2-claw_offset)-max_blob.cy()print("pan_error:",pan_error)#pan_output=pan_pid.get_pid(pan_error,1)/2tilt_output=tilt_pid.get_pid(tilt_error,1)#关闭左右追踪title_angle=tilt_servo.angle()-tilt_output#pan_servo.angle(pan_servo.angle()+pan_output)tilt_servo.angle(title_angle)############################抓取#############################抓取得策略是摄像头与夹子呈一定角度安装，大概45度，让一半得夹子在图像中，目的是方便判断目标是否一直在夹子上#然后修订目标在夹子中得位置，当修订好之后再执行夹取动作。if(ball_s>=60andball_s<=100):run(0,0)count=count+1ifcount>=3:count=0run(0,0)claw_angle(claw_catch)#夹取time.sleep(1000)servo_move(tilt_servo,tilt_servo.angle(),45)#抬起time.sleep(1000)run(60,60)#前进time.sleep(3000)run(0,0)servo_move(tilt_servo,tilt_servo.angle(),60)#向下time.sleep(1000)claw_angle(claw_release)#释放time.sleep(1000)run(-60,-60)#后退time.sleep(5000)run(0,0)if(ball_s>100)and(ball_s<800):############################云台追踪############################count=0claw_angle(claw_release)#############################小车追踪############################power=0power_s=0power_l=0#前进与后退控制，#没有做后退，而是采用先大致接近目标，再修订距离得方式#前进与后退控制，这里采用的是像素面积计算距离，而抓取采用的是距离参数，#两者可以统一一下，具体可以根据实际物体大小测定后调整ifball_s>300andball_s<=800:dis_error=ball_s-300power_s=int(dis_pid.get_pid(dis_error,1)/2)if(power_s>100):power_s=100if(power_s<60):power_s=60elifball_s>100andball_s<=300:dis_error=ball_s-100power_s=int(dis_pid.get_pid(dis_error,1)/2)#电机PWM限幅，防止最后一点距离走不到if(power_s<60):power_s=60else:power_s=0power_l=int(len_pid.get_pid(pan_error,1))print("power_s:",power_s)print("power_l:",power_l)run(power_s-power_l,power_s+power_l)defcolor_detect(img):globallast_objects,objectsball_s=0iflast_objects:#如果上一张图像中找到了色块，就在色块周边扩展，然后在扩展后的ROI区域进行寻找色块，这样有利于追踪forbinobjects:x1=b[0]-7y1=b[1]-7w1=b[2]+12h1=b[3]+12#print(b.cx(),b.cy())roi2=(x1,y1,w1,h1)#print(roi2)objects=img.find_blobs(THRESHOLD,roi=roi2,area_threshold=200)last_objects=objectselse:#如果没有找到色块，就重新计算全图的色块，并将全图色块信息保存到上一张图像上，这样下一次就可以再进行ROI区域寻找色块objects=img.find_blobs(THRESHOLD,area_threshold=200)last_objects=objectsifobjects:#如果找到了色块，就计算最大的色块位置，并做舵机追踪max_blob=find_max_object(objects)#色块的位置和大小需要满足图像本身大小的要求，以免越界ball_s=20000/(max_blob[2]+max_blob[3])#计算距离img.draw_rectangle(max_blob.rect())#rect目标区域画矩形img.draw_cross(max_blob.cx(),max_blob.cy())#cx,cy画中心点img.draw_string(max_blob.cx(),max_blob.cy(),"%.2fmm"%(ball_s))#显示目标的距离参数claw_auto_catch(img,ball_s,max_blob)else:run(0,0)if(tilt_servo.angle()<=45):tilt_servo.angle(45)else:tilt_servo.angle(tilt_servo.angle())claw_angle(claw_release)defbluetooth_deal(img):globalbluetooth_stutas,G_FLAG,g_cardir,g_carstateifuart1.any():rec=uart1.read(1)ifrec!=None:rec=bytes(rec)print(rec)ifrec==b'\x00':#停车g_cardir=CARSTATE.enSTOPelifrec==b'\x01':#前进g_cardir=CARSTATE.enRUNelifrec==b'\x02':#后退g_cardir=CARSTATE.enBACKelifrec==b'\x03':#左转g_cardir=CARSTATE.enLEFTelifrec==b'\x04':#右转g_cardir=CARSTATE.enRIGHTelifrec==b'\x05':#循迹g_carstate=CARSTATE.enTRACINGelifrec==b'\x06':#跟随g_carstate=CARSTATE.enTRAKINGelifrec==b'\x07':#避障#按两次就回到手动模式bluetooth_stutas=bluetooth_stutas+1ifbluetooth_stutas==1:g_carstate=CARSTATE.enAVOIDINGG_FLAG=1ifbluetooth_stutas==2:g_carstate=CARSTATE.enMANUALG_FLAG=0bluetooth_stutas=0elifrec==b'\x08':#app上是关灯按钮，自定义释放g_cardir=CARSTATE.enRELEASEelifrec==b'\x09':#app上是开灯，自定义抓取g_cardir=CARSTATE.enCATCHelifrec==b'\xF0':#一键切换到手动模式g_carstate=CARSTATE.enMANUALBST_fLeftMotorOut,BST_fRightMotorOut,g_power=0,0,800if(g_carstate==CARSTATE.enMANUAL):#手动控制小车if(g_cardir==CARSTATE.enSTOP):BST_fLeftMot