基于MSP430送药小车 ----- 基础篇【2021年全国电赛(F题)】
文章目录
-
- 该篇文章并非完全按照赛题要求完成，仅当做个人休闲娱乐产品！！！！
- 一、赛题
-
- 1. 任务
- 2. 要求
- 3. 说明
- 二、构思 分析
-
- 1. 引脚利用
- 2. PID算法
- 3. 灰度循迹及标志位
- 4. 视觉模块
- 5. 直角转弯、原地转向
- 三、硬件清单
- 四、逻辑设计
-
- 1. 近端送药
- 2. 中端送药
- 3. 远端送药
- 五、程序设计
-
- 1. OpenMV
- 2. 灰度循迹
- 3. 装药卸药
- 六、视频展示
- 总结
该篇文章并非完全按照赛题要求完成，仅当做个人休闲娱乐产品！！！！
一、赛题
1. 任务
设计并制作智能送药小车，模拟完成在医院药房与病房间药品的送取作业。院区结构示意如图1所示。院区走廊两侧的墙体由黑实线表示。走廊地面上画有居中的红实线,并放置标识病房号的黑色数字可移动纸张。药房和近端病房号(1、2号）如图1所示位置固定不变，中部病房和远端病房号(3-8号）测试时随机设定。
工作过程:参赛者手动将小车摆放在药房处(车头投影在门口区域内，面向病房），手持数字标号纸张由小车识别病房号，将约200g药品一次性装载到送药小车上:小车检测到药品装载完成后自动开始运送;小车根据走廊上的标识信息自动识别、寻径将药品送到指定病房（车头投影在门口区域内)，点亮红色指示灯，等待卸载药品;病房处人工卸载药品后，小车自动熄灭红色指示灯，开始返回;小车自动返回到药房（车头投影在门口区域内，面向药房〉后，点亮绿色指示灯。
2. 要求
- 基本要求
(1） 单个小车运送药品到指定的近端病房并返回到药房。要求运送和返回时间均小于20s，超时扣分。
(2） 单个小车运送药品到指定的中部病房并返回到药房。要求运送和返回时间均小于20s，超时扣分。
(3) 单个小车运送药品到指定的远端病房并返回到药房。要求运送和返回时间均小于20，超时扣分.
- 发挥部分
(1) 两个小车协同运送药品到同一指定的中部病房。小车1识别病房号装载药品后开始运送，到达病房后等待卸载药品;然后，小车2识别病房号装载药品后启动运送，到达自选暂停点后暂停，点亮黄色指示灯，等待小车1卸载;小车1卸载药品，开始返回，同时控制小车2熄灭黄色指示灯并继续运送。要求从小车2启动运送开始，到小车1返回到药房且小车2到达病房的总时间(不包括小车2黄灯亮时的暂停时间）越短越好，超过60s 计0分。
(2) 两个小车协同到不同的远端病房送、取药品，小车1送药，小车2取药。小车1识别病房号装载药品后开始运送，小车2于药房处识别病房号等待小车1的取药开始指令;小车1到达病房后卸载药品，开始返回，同时向小车2发送启动取药指令;小车2收到取药指令后开始启动，到达病房后停止，亮红色指示灯。要求从小车1返回开始，到小车1返回到药房且小车2到达取药病房的总时间越短越好，超过60s计0分。
(3) 其他。
3. 说明
(1) 院区可由铺设白色亚光喷绘布制作。走廊上的黑线和红线由喷绘或粘贴线宽约为1.5cm~1.8cm的黑色和红色电工胶带制作。药房和病房门口区域指其标线外沿所涵盖的区域，其标线为约2cm 黑白相间虚线。图1中非黑色、非红色仅用于识图解释，在实测院区中不出现。
(2) 标识病房的黑色数字可在纸张上打印，数值为1-8，每个数字边框长宽为8cm×6cm，将“数字字模.pdf”文件按实际大小打印即可;数字标号纸张可由无痕不干胶等粘贴在走廊上，其边框距离实线约2cm;图1中标识远端病房的两个并排数字边框之间距离约2cm。
(3) 小车长×宽×高不大于25cm×20cm×25cm，使用普通车轮（不能使用履带或麦克纳姆轮等特殊结构)。两小车均由电池供电，小车间可无线通信，外界无任何附加电路与控制装置。
(4) 作品应能适应无阳光直射的自然光照明及顶置多灯照明环境，测试时不得有特殊照明条件要求。
(5) 每项测试开始时，只允许按一次复位键，装载药品后即刻启动运送时间计时，卸载药品后即刻启动返回时间计时。计时开始后，不得人工干预。每个测试项目只测试一次。
(6) 小车于药房处识别病房号的时间不超过20s。发挥部分(1)中自选暂停点处的小车2与小车1的车头投影外沿中心点的红实线距离不小于70cm。
(7) 有任何一个指示灯处于点亮状态的小车必须处于停止状态。两小车协同运送过程中不允许在同一走廊上错车或超车。
(8) 测试过程中，小车投影落在黑实线上或两小车碰撞将被扣分;小车投影连续落在黑实线上超过30cm 或整车越过黑实线，或两小车连续接触时间超过5s，该测试项计0分。
(9) 参赛者需自带⒉套数字标号纸张，无需封箱。
二、构思 分析
以下是我个人见解，如有不妥之处，欢迎批评指正。
1. 引脚利用
首先，在拿到赛题后(已该题为例)，仔细阅读赛题要求并留意记录（如车型、开发板、电机限制等），其次框架设计，在搭框架的同时请把模块清单列出来，若缺失必需模块，请及时下单购买。再是模块搭配与引脚的使用，若是四轮驱动，而开发板选用的是msp430xxx或stm32核心板引脚相对较少的板子，那么请认真构思了，充分利用到每一个引脚，把特殊功能引脚框出来，先选用普通IO口（以mps430f5529为例，例如将两个串口功能引脚P44、P45、P33、P34留出来，同理定时器的功能引脚也稍放到后面再使用），一般像近几年小车控制题，都是以两辆车的方式出现，所以我单方面认为视觉模块和小车通信是大概率的，可想常用的视觉模块有openmv和k210等，小车通信用蓝牙、WiFi等，这里俩个通信方式一般都是以串口实现，至此在不使用其他串口通信模块的基础上像msp430f5529的俩个串口已经拉满了。
2. PID算法
如果你想引入PID算法，那么最常用的俩种方案是定时器输入捕获和外部中断捕获。
①定时器输入捕获
这里我以电机双相使用为例，假如你是三轮驱动，一个电机需消耗一个定时器使用输入捕获功能进行脉冲捕获，俩电机就需两个定时器，且还需一个定时器进行定时采集脉冲数及PID计算，所以这里需3个定时器。
②外部中断
相对定时器捕获，外部中断可利用的引脚就相对较多，在P1、P2端口的各引脚都可利用，每触发一个脉冲中断计数加1，且同理需消耗一个定时器定时脉冲捕获和算法计算，将返回的值传入PWM驱动。
3. 灰度循迹及标志位
在历年的小车控制题中，国赛题，基本都是在灰度循迹的基础上进行拓展功能，但每年的赛图都不大一样，各式各样的路口需要识别判断，所以尽多的灰度探头能更精准的判断转向，其次标志位很重要，可以在每个转向口都设立一个标志位，且每个路口标志位息息相关，大多赛题在时间上也有限制要求，如送药小车 基本要求中去病房送药和回药房都需在20s内完成，去年跟随小车也是有时间限制，所以还有一个技巧，你可以利用定时器在指定的时间设立标志位，如该送药小车小车去远端药房送药回来时，是一个长直线，这时你可以在病房取药的时刻打开定时器（具体时间需自己测量），设立标志位，恰在转完最后一个弯道时根据标志位直线加速，以确保在规定时间内完成送药。
4. 视觉模块
就新手而言，视觉模块我更推荐使用OpenMV。
自我感受：官网有各类例程供参考使用，一般电赛在视觉上不会有过难的要求，因为控制组它主打控制。所以我单方面认为学会使用官网各项例程在完成赛题上够用了，且每项例程都有视频讲解，通俗易懂，“十分钟上手”实实在在。
使用的是Python编程，大家都搞嵌入式的，C语言应该都不会太差吧，会C语言Python就没啥问题了，可以简单理解为Python为简化版的C语言（不需要类型定义、逗号隔行等等），且OpenMV有它自己的IDE，功能是相对比较强大的（个人认为）。
5. 直角转弯、原地转向
直角转弯和原地转向可通过简单的延迟函数实现。
以下附上我的代码示例
//直角左转向
left();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
delay_us(350000);
//直角右转向
right();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
delay_us(350000);
//原地转向
back();
SetPwm_Init(24,1000,400);//左边
SetPwm_Init(25,1000,400);//右边
delay_us(500000);
left();
SetPwm_Init(24,1000,550);//左边
SetPwm_Init(25,1000,650);//右边
delay_us(700000);
三、硬件清单
1. 小车底盘
2. 七路灰度
3. 蓝牙 HC-05
4. Ti msp430f5529开发板
5. OLED显示屏
6. OpenMV
7. 编码器电机
8. 18650锂电池
9. LM2596S稳压模块
10. TB6612电机驱动模块
11. 万向轮
12. OpenMV云台支架
该云台支架为自己3D打印，若需可联系我，分享图纸文件
13. 红外对管
14. 杜邦线、铜柱、绑带、模拟药盒若干
四、逻辑设计
Readme：本项目中使用的视觉模块为OpenMV4 H7，非plus版本，所以该项目并没有涉及神经网络，纯控制手笔。
1. 近端送药
近端为①②号病房，这里我在实现灰度循迹的基础上介绍，在去往①②号病房的途中只有一个十字路口，可想，接收到openmv发过来的病房号无非就是①和②，即一个左拐一个右拐，可根据接收到的数字在十字路口逻辑控制，成功拐弯后 若全部探头没踩到颜色线（这里是红线），就电机停止转动，等待卸掉药品，装卸药品可用红外对管进行检测，成功卸掉药品后根据红外对管发出的信号原地180°转向返回药房，在返回途中有需途径十字路口，这时你可以在来病房途中设一个标志位，在取完药品后改变标志位，则通过改变后的标志位进行转向控制，即可成功返回药房。
void judge1(void)
{
xunji();
if(((P6IN&BIT1)==BIT1) && ((P6IN&BIT2)==BIT2) && ((P6IN&BIT3)==BIT3) )
{
if(flag1==0) left();
if(flag1==1) right();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
}
if(((P6IN&BIT5)!=BIT5) && ((P6IN&BIT0)!=BIT0) && ((P6IN&BIT1)!=BIT1)&& ((P6IN&BIT2)!=BIT2) && ((P6IN&BIT3)!=BIT3) && ((P6IN&BIT4)!=BIT4) && ((P7IN&BIT0)!=BIT0))
{
if( pill==0 && ((P7IN&BIT4)==BIT4) ) //拿走药品
{
pill=1;
TB0CTL &= ~MC__UP; // 停止计数器
TB0CTL &= ~TBIE; // 关闭中断
TB0CTL &= ~TBIFG; // 清除中断标志
TB0CTL |= TBCLR; // 清除计数器
back();
SetPwm_Init(24,1000,400);//左边
SetPwm_Init(25,1000,400);//右边
delay_us(700000);
left();
SetPwm_Init(24,1000,550);//左边
SetPwm_Init(25,1000,650);//右边
delay_us(700000);
flag1=1;
}
else
{
stop();
SetPwm_Init(24,1000,0);//左边
SetPwm_Init(25,1000,0);//右边
}
}
}
2. 中端送药
中断送药相对于近端多了俩个十字路口（来回各多一次），即根据近端送药的操作在此基础上增加一个标志位即可，若有疑问，可在评论区留言或加下方wx联系我。
void judge3(void)
{
xunji();
if(((P6IN&BIT1)==BIT1) && ((P6IN&BIT2)==BIT2) && ((P6IN&BIT3)==BIT3) )
{
if(flag3==-1)
{
flag3=-2;
right();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
}
if(flag33==0) flag3++;
if(flag3>=250)
{
flag3=0;
left();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
}
}
if(((P6IN&BIT0)!=BIT0) && ((P6IN&BIT1)!=BIT1)&& ((P6IN&BIT2)!=BIT2) && ((P6IN&BIT3)!=BIT3) && ((P6IN&BIT4)!=BIT4) && (((P7IN&BIT0)!=BIT0)||((P6IN&BIT5)!=BIT5)))
{
if( pill==0 && ((P7IN&BIT4)==BIT4) ) //拿走药品
{
pill=1;
TB0CTL &= ~MC__UP; // 停止计数器
TB0CTL &= ~TBIE; // 关闭中断
TB0CTL &= ~TBIFG; // 清除中断标志
TB0CTL |= TBCLR; // 清除计数器
back();
SetPwm_Init(24,1000,400);//左边
SetPwm_Init(25,1000,400);//右边
delay_us(700000);
left();
SetPwm_Init(24,1000,550);//左边
SetPwm_Init(25,1000,650);//右边
delay_us(700000);
flag3=-1;
flag33=1;
}
else
{
stop();
SetPwm_Init(24,1000,0);//左边
SetPwm_Init(25,1000,0);//右边
}
}
}
3. 远端送药
远端送药相对较其他药房无非也是标志位的问题，这里介绍一个判断在哪个路口转向的技巧，可以设定一个全局变量，当每经过一个十字路口时是不是会出现大于等于5个探头踩到信号线（我这里用的7路），所以可以这样定义，每大于等于5个探头踩到信号线时，表示小车正经过一个十字路口，在程序中就给该变量++操作，即根据该变量的值进行设计，不同的药房设定相对应的区域，具体变量值的大小需你自己根据实际情况测定，以下给出我在40%占空比情况下值的变化。
- 经过第一个十字路口 变量变化在200左右
- 经过第二个十字路口 变量变化在700左右
- 经过第三个十字路口 变量变化在1200左右
所以在去病房的直线上可根据值得变化控制转向，这样就省去许多标志位了。
以⑤号病房为例，它位于左上角处，给每一个路口设定标志位，比如在经过该路口后，立即改变该路口标志位以备回程时用，且同时开启下一个路口的标志位以供下个路口控制转向，总的来说就是每个路口都环环相扣。
void judge5(void)
{
xunji();
if(flag55==1)
{
if(((P6IN&BIT5)==BIT5) && ((P6IN&BIT0)==BIT0) &&((P6IN&BIT4)!=BIT4)&&((P7IN&BIT0)!=BIT0) && (((P6IN&BIT2)==BIT2)||((P6IN&BIT1)==BIT1)))
{
left();
SetPwm_Init(24,1000,900);//左边
SetPwm_Init(25,1000,900);//右边
delay_us(250000);
flag55555++;
}
if(((P6IN&BIT5)!=BIT5) && ((P6IN&BIT0)!=BIT0) &&((P6IN&BIT4)==BIT4)&&((P7IN&BIT0)==BIT0) && (((P6IN&BIT3)==BIT3)||((P6IN&BIT2)==BIT2)))
{
right();
SetPwm_Init(24,1000,900);//左边
SetPwm_Init(25,1000,900);//右边
delay_us(250000);
flag55555++;
}
}
if(((P6IN&BIT1)==BIT1) && ((P6IN&BIT2)==BIT2) && ((P6IN&BIT3)==BIT3) )
{
if(flag55555>1)
{
flag55=-7;
head();
SetPwm_Init(24,1000,400);//左边
SetPwm_Init(25,1000,400);//右边
}
if(flag55==0) flag5++;
if(flag5>=1300&&flag555==1)
{
flag55=1;
flag5=-3;
right();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
delay_us(350000);
}
if(flag5>=1050&&flag555==0)
{
flag5=1200;
flag555=1;
left();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
delay_us(350000);
}
}
//if(((P6IN&BIT0)!=BIT0) && ((P6IN&BIT1)!=BIT1)&& ((P6IN&BIT2)!=BIT2) && ((P6IN&BIT3)!=BIT3) && ((P6IN&BIT4)!=BIT4) && (((P7IN&BIT0)!=BIT0)||((P6IN&BIT5)!=BIT5)))
if(((P6IN&BIT1)!=BIT1) && ((P6IN&BIT2)!=BIT2) && ((P6IN&BIT3)!=BIT3) && (((P7IN&BIT0)!=BIT0)||((P6IN&BIT5)!=BIT5)) && (((P6IN&BIT0)!=BIT0)||((P6IN&BIT4)!=BIT4)))
{
if( pill==0 && ((P7IN&BIT4)==BIT4) ) //拿走药品
{
pill=1;
back();
SetPwm_Init(24,1000,400);//左边
SetPwm_Init(25,1000,400);//右边
delay_us(700000);
left();
SetPwm_Init(24,1000,550);//左边
SetPwm_Init(25,1000,650);//右边
delay_us(700000);
flag55555=2;
}
else
{
stop();
SetPwm_Init(24,1000,0);//左边
SetPwm_Init(25,1000,0);//右边
}
}
}
五、程序设计
1. OpenMV
因为我这不带神经网络，所以我多拍了几组图片方便识别数字，部分易识别错误的数字（1，3，5）保持默认匹配度0.7不用改变，不易识别错的数字（2，4，6，7，8）可降低匹配度，可加快识别效率。
import sensor, image, time
sensor.reset() # Reset and initialize the sensor.
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QVGA) # Set frame size to QVGA (320x240)
sensor.skip_frames(time = 2000) # Wait for settings take effect.
clock = time.clock() # Create a clock object to track the FPS.
import time, sensor, image
from image import SEARCH_EX, SEARCH_DS
from pyb import UART
# Reset sensor
sensor.reset()
# Set sensor settings
sensor.set_contrast(1)
sensor.set_gainceiling(16)
# Max resolution for template matching with SEARCH_EX is QQVGA
sensor.set_framesize(sensor.QQVGA)
# You can set windowing to reduce the search image.
#sensor.set_windowing(((640-80)//2, (480-60)//2, 80, 60))
sensor.set_pixformat(sensor.GRAYSCALE)
# Load template.
# Template should be a small (eg. 32x32 pixels) grayscale image.
template1 = image.Image("/1.pgm")
template2 = image.Image("/2.pgm")
template3 = image.Image("/3.pgm")
template4 = image.Image("/4.pgm")
template5 = image.Image("/5.pgm")
template6 = image.Image("/6.pgm")
template7 = image.Image("/7.pgm")
template8 = image.Image("/8.pgm")
template11 = image.Image("/11.pgm")
template22 = image.Image("/22.pgm")
template33 = image.Image("/33.pgm")
template44 = image.Image("/44.pgm")
template55 = image.Image("/55.pgm")
template66 = image.Image("/66.pgm")
template77 = image.Image("/77.pgm")
template88 = image.Image("/88.pgm")
template111 = image.Image("/111.pgm")
template222 = image.Image("/222.pgm")
template333 = image.Image("/333.pgm")
template444 = image.Image("/444.pgm")
template555 = image.Image("/555.pgm")
template666 = image.Image("/666.pgm")
template777 = image.Image("/777.pgm")
template888 = image.Image("/888.pgm")
template1111 = image.Image("/1111.pgm")
template3333 = image.Image("/3333.pgm")
template5555 = image.Image("/5555.pgm")
template7777 = image.Image("/7777.pgm")
clock = time.clock()
uart = UART(3, 115200)
# Run template matching
while (True):
clock.tick()
img = sensor.snapshot()
# find_template(template, threshold, [roi, step, search])
# ROI: The region of interest tuple (x, y, w, h).
# Step: The loop step used (y+=step, x+=step) use a bigger step to make it faster.
# Search is either image.SEARCH_EX for exhaustive search or image.SEARCH_DS for diamond search
#
# Note1: ROI has to be smaller than the image and bigger than the template.
# Note2: In diamond search, step and ROI are both ignored.
r1 = img.find_template(template1, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r1:
img.draw_rectangle(r1)
print("1")
while (True):
uart.write("1")
r2 = img.find_template(template2, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r2:
img.draw_rectangle(r2)
print("2")
while (True):
uart.write("2")
r3 = img.find_template(template3, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r3:
img.draw_rectangle(r3)
print("3")
while (True):
uart.write("3")
r4 = img.find_template(template4, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r4:
img.draw_rectangle(r4)
print("4")
while (True):
uart.write("4")
r5 = img.find_template(template5, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r5:
img.draw_rectangle(r5)
print("5")
while (True):
uart.write("5")
r6 = img.find_template(template6, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r6:
img.draw_rectangle(r6)
print("6")
while (True):
uart.write("6")
r7 = img.find_template(template7, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r7:
img.draw_rectangle(r7)
print("7")
while (True):
uart.write("7")
r8 = img.find_template(template8, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r8:
img.draw_rectangle(r8)
print("8")
while (True):
uart.write("8")
r11 = img.find_template(template11, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r11:
img.draw_rectangle(r11)
print("1")
while (True):
uart.write("1")
r22 = img.find_template(template22, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r22:
img.draw_rectangle(r22)
print("2")
while (True):
uart.write("2")
r33 = img.find_template(template33, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r33:
img.draw_rectangle(r33)
print("3")
while (True):
uart.write("3")
r44 = img.find_template(template44, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r44:
img.draw_rectangle(r44)
print("4")
while (True):
uart.write("4")
r55 = img.find_template(template55, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r55:
img.draw_rectangle(r55)
print("5")
while (True):
uart.write("5")
r66 = img.find_template(template66, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r66:
img.draw_rectangle(r66)
print("6")
while (True):
uart.write("6")
r77 = img.find_template(template77, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r77:
img.draw_rectangle(r77)
print("7")
while (True):
uart.write("7")
r88 = img.find_template(template88, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r88:
img.draw_rectangle(r88)
print("8")
while (True):
uart.write("8")
r111 = img.find_template(template111, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r111:
img.draw_rectangle(r111)
print("1")
while (True):
uart.write("1")
r222 = img.find_template(template222, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r222:
img.draw_rectangle(r222)
print("2")
while (True):
uart.write("2")
r333 = img.find_template(template333, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r333:
img.draw_rectangle(r333)
print("3")
while (True):
uart.write("3")
r444 = img.find_template(template444, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r444:
img.draw_rectangle(r444)
print("4")
while (True):
uart.write("4")
r555 = img.find_template(template555, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r555:
img.draw_rectangle(r555)
print("5")
while (True):
uart.write("5")
r666 = img.find_template(template666, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r666:
img.draw_rectangle(r666)
print("6")
while (True):
uart.write("6")
r777 = img.find_template(template777, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r777:
img.draw_rectangle(r777)
print("7")
while (True):
uart.write("7")
r888 = img.find_template(template888, 0.65, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r888:
img.draw_rectangle(r888)
print("8")
while (True):
uart.write("8")
r1111 = img.find_template(template1111, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r1111:
img.draw_rectangle(r1111)
print("1")
while (True):
uart.write("1")
r3333 = img.find_template(template3333, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r3333:
img.draw_rectangle(r3333)
print("3")
while (True):
uart.write("3")
r5555 = img.find_template(template5555, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r5555:
img.draw_rectangle(r5555)
print("5")
while (True):
uart.write("5")
r7777 = img.find_template(template7777, 0.70, step=4, search=SEARCH_EX) #, roi=(10, 0, 60, 60))
if r7777:
img.draw_rectangle(r7777)
print("7")
while (True):
uart.write("7")
while(True):
clock.tick() # Update the FPS clock.
img = sensor.snapshot() # Take a picture and return the image.
print(clock.fps()) # Note: OpenMV Cam runs about half as fast when connected
# to the IDE. The FPS should increase once disconnected.
2. 灰度循迹
为保持车身平稳前行可只通过中间3个和边上俩个探头进行灵活判断
void xunji()
{
if(((P6IN&BIT1)!=BIT1) && ((P6IN&BIT2)==BIT2) && ((P6IN&BIT3)!=BIT3)) //左3、右3都没踩到，中踩到
{
head();
SetPwm_Init(24,1000,400);//左边
SetPwm_Init(25,1000,400);//右边
}
else if(((P6IN&BIT1)==BIT1) && ((P6IN&BIT2)!=BIT2) && ((P6IN&BIT3)!=BIT3)) //左3踩到
{
head();
SetPwm_Init(24,1000,0);//左边
SetPwm_Init(25,1000,500);//右边
}
else if(((P6IN&BIT1)!=BIT1) && ((P6IN&BIT2)!=BIT2) && ((P6IN&BIT3)==BIT3)) //右3踩到
{
head();
SetPwm_Init(24,1000,500);//左边
SetPwm_Init(25,1000,0);//右边
}
else if(((P6IN&BIT1)==BIT1) && ((P6IN&BIT2)==BIT2) && ((P6IN&BIT3)!=BIT3)) //左3和中踩到
{
head();
SetPwm_Init(24,1000,150);//左边
SetPwm_Init(25,1000,500);//右边
}
else if(((P6IN&BIT1)!=BIT1) && ((P6IN&BIT2)==BIT2) && ((P6IN&BIT3)==BIT3)) //右3和中踩到
{
head();
SetPwm_Init(24,1000,500);//左边
SetPwm_Init(25,1000,150);//右边
}
else if(((P6IN&BIT5)!=BIT5) && ((P6IN&BIT0)!=BIT0) && ((P6IN&BIT1)!=BIT1) && ((P6IN&BIT2)!=BIT2) && ((P6IN&BIT3)!=BIT3) && ((P6IN&BIT4)!=BIT4) && ((P7IN&BIT0)==BIT0)) //右3和中踩到
{
head();
SetPwm_Init(24,1000,600);//左边
SetPwm_Init(25,1000,0);//右边
}
else if(((P6IN&BIT5)==BIT5) && ((P6IN&BIT0)!=BIT0) && ((P6IN&BIT1)!=BIT1) && ((P6IN&BIT2)!=BIT2) && ((P6IN&BIT3)!=BIT3) && ((P6IN&BIT4)!=BIT4) && ((P7IN&BIT0)!=BIT0)) //右3和中踩到
{
head();
SetPwm_Init(24,1000,0);//左边
SetPwm_Init(25,1000,600);//右边
}
if(((P6IN&BIT5)==BIT5) && ((P6IN&BIT0)==BIT0) &&((P6IN&BIT1)!=BIT1)&&((P6IN&BIT2)!=BIT2) &&((P6IN&BIT3)!=BIT3) &&((P6IN&BIT4)!=BIT4)&&((P7IN&BIT0)!=BIT0))
{
left();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
}
if(((P6IN&BIT5)!=BIT5) && ((P6IN&BIT0)!=BIT0) &&((P6IN&BIT1)!=BIT1)&&((P6IN&BIT2)!=BIT2) &&((P6IN&BIT3)!=BIT3) &&((P6IN&BIT4)==BIT4)&&((P7IN&BIT0)==BIT0))
{
right();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
}
if(((P6IN&BIT5)!=BIT5) && ((P6IN&BIT0)==BIT0) &&((P6IN&BIT1)!=BIT1)&&((P6IN&BIT2)!=BIT2) &&((P6IN&BIT3)!=BIT3) &&((P6IN&BIT4)!=BIT4)&&((P7IN&BIT0)!=BIT0))
{
left();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
}
if(((P6IN&BIT5)!=BIT5) && ((P6IN&BIT0)!=BIT0) &&((P6IN&BIT1)!=BIT1)&&((P6IN&BIT2)!=BIT2) &&((P6IN&BIT3)!=BIT3) &&((P6IN&BIT4)==BIT4)&&((P7IN&BIT0)!=BIT0))
{
right();
SetPwm_Init(24,1000,700);//左边
SetPwm_Init(25,1000,700);//右边
}
}
3. 装药卸药
Tips:这里我主打设计想法，不一定完全安照题型要求设计。
首先装药过程，在接收到识别到得数字后，将数字打印在oled上，然后由原来的红灯变为黄灯，原地等待装药，当装满药品后触发红外对管信号变化，驱动电机开启循迹功能。
while(1)
{
while(!find_num)
{
P1OUT|=BIT0;
}
P1OUT&=~BIT0;
if(oled_flag==1)
{
oled_flag=0;
OLED_ShowNum(65,24,find_num,1,16,1);
OLED_Refresh();
}
while((P7IN&BIT4)==BIT4)
{
P4OUT|=BIT7;
}
P4OUT&=~BIT7;
while(head_flag)
{
head_flag=0;
head();
SetPwm_Init(24,1000,400);//左边
SetPwm_Init(25,1000,400);//右边
delay_us(500000);
}
break;
}
六、视频展示
送药小车(基础篇)
总结
其实该赛题并不需要较复杂的控制算法，我这里就简单得PWM控制实现全部功能，其难点为逻辑设计，标志位的设立和时间速度控制等等，下篇介绍两辆车送药的详细介绍。
欢迎一键三连，以上若有不妥之处，烦请批评指正！
疑难解答或技术交流联系下方wx即可