[DIY]基於openMV的追球小車
注：本文僅用於學習交流分享，[若有不妥之處，請指正，感謝]
關鍵詞：【OpenMV】【顏色識別】【PID】【STM32】
最後面有程序與原理圖PCB分享
用到的工具有：
- openMV IDE
- Keil 5 編譯器
- Altium Designer
實現的小功能有：
①設別顏色小球，並自動追尋小球
②簡單測試與顏色小球的粗略距離，並且在小球10cm處停車
③按鍵調節PID參數以及調節識別的顏色
總體設計
1.基礎硬件DIY設計
2.openMV簡單識別程序設計 與 單片機控制程序設計
3.效果展示
1.基礎硬件DIY設計
電路硬件：
[MCU] STM32F103C8T6最小系統板
[穩壓電源]【L7805】 7.2V穩壓5.0V 【AMS1117-3.3】5.0V穩壓3.3V
[外圍電路] 按鍵、蜂鳴器、OLED、幹簧管
1）整體原理圖
2）PCB電路
3）3D_PCB
-
TOP層
![在這裏插入圖片描述]()
-
BOTTOM層
![在這裏插入圖片描述]()
2.openMV簡單識別程序設計 與 STM32控制程序設計
1）openMV簡單識別程序設計【microPython】
識別小球顏色並通過串口定時發送小球座標與距離的數據包
#2018.8.2 【microPython】
import sensor, image, time , pyb
from pyb import UART
from pyb import Timer
from pyb import LED
import json
led = pyb.LED(3) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
thresholds = [(27, 67, 19, 91, 45, 76), # 紅色
#(21, 75, 3, -38, 34, 68), # 綠色
(27, 90, -3, -28, 31, 125),
(0, 30, 0, 64, -128, 0)] # generic_blue_thresholds
threshold_index = 1 # 0 for red, 1 for gre9en, 2 for blue
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA) #320*240
sensor.skip_frames(time = 100)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
clock = time.clock()
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1, timeout_char=1000) # 使用給定參數初始化
def tick(timer): # we will receive the timer object when being called
global data
if blobs:
print("Find")
print('you send:',output_str)
uart.write(data)
tim = Timer(4, freq=10) # create a timer object using timer 4 - trigger at 1Hz
tim.callback(tick) # set the callback to our tick function
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
def Uart_Receive(): #UART接收 改變框小球的顏色閾值
global threshold_index
if uart.any():
temp_data = uart.readchar()
if temp_data==0: #紅色
threshold_index=0
print(temp_data,threshold_index)
elif temp_data==1:
threshold_index=1
print(temp_data,threshold_index)
while(True):
clock.tick()
img = sensor.snapshot()
Uart_Receive()
blobs = img.find_blobs([thresholds[threshold_index]])
if blobs:
max_blob = find_max(blobs)
b = max_blob[0] #方框元組
L = (max_blob[2]+max_blob[3])/2
l=int(1000/L)
#x_error = max_blob[5]-img.width()/2 #求橫向偏差
x_error = max_blob[5]-img.width()/2
img.draw_rectangle(max_blob[0:4]) # 畫矩形
img.draw_cross(max_blob[5], max_blob[6]) # 畫十字
#發送 小球的(x,y,l,n)
#x爲橫座標，y爲縱座標，l爲粗略的距離，n爲小球顏色(0：紅 1：綠)
output_str="%d,%d,%d,%d" % (max_blob.cx(),max_blob.cy(),l,threshold_index) #10進制字符包
checkout=0xAA+0x55+0x07+int(max_blob.cx()/2)+max_blob.cy()+l+threshold_index
data = bytearray([0xAA,0x55,0x07,int(max_blob.cx()/2),max_blob.cy(),l,threshold_index,0x00,0x00,checkout])#轉成16進制
#uart.write(data)
time.sleep(1)
led.on()
else:
print("NO FIND")
data = bytearray([0xAA,0x55,0x07,0x00,0x00,0x00,0x00,0x00,0x00,0x06])
uart.write(data)
led.off()
 圖3 抓取到綠色小球 |
 圖4 抓取到紅色小球 |
2）STM32控制程序設計 【C語言】
①對OpenMV發送的數據包進行解析
void USART1_IRQHandler(void) //串口1中斷服務程序
{
uint8 i=0,j=0;
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
{//接收中斷
control_data[data_number]=USART_ReceiveData(USART1);
data_number++;
if(data_number<(MAX_DATA_LENS+4))
{ //定義數據長度未包括包頭和包長3個字節,+4
if(control_data[0]==0xaa)//數據包包頭字節
{
if(data_number>3)
{
if(control_data[1]==0x55)
{
if(data_number>=(control_data[2]+3))
{//接收完數據包（第三個字節爲數據長度，數據長度不包含開頭和校驗字）
for(i=0;i<=(data_number-2);i++)
{
j +=control_data[i];
}
if(j==control_data[data_number-1]) //判斷校驗是否成功
{
j=0;
recv1_data_ok=1; //接收到正確完整數據包標誌位置
}
else
{
recv1_data_ok=0;
}
j=0;
data_number=0;
}
}
else
{
recv1_data_ok=0;
data_number=0;
}
}
}
else
{
recv1_data_ok=0;
data_number=0;
}
}
else
{
recv1_data_ok=0;
data_number=0;
}
}
}
②小車的簡單PID控制
#include "my_include.h"
extern CAR_STATUS_e car_mode;
extern moty_duty run_duty;
extern u8 control_data[MAX_DATA_LENS];
extern float C_P; //cameraP
extern float C_D; //cameraD
extern int16 ser_duty;
extern int16 x_error;
extern int16 last_x_error;
extern uint8 ball_colcor;
extern uint8 BEEP_ON_OFF;
extern uint8 out_edge;//出界
void Car_mode_control()//小車簡單控制邏輯單元
{
if(control_data[3]<50 && control_data[3]!=0) //左出界
{
out_edge=Left;
}
else if(control_data[3]>110 && control_data[3]!=0) //右出界
{
out_edge=Right;
}
//--------------------車位狀態判斷-----------------//
if(out_edge==Left && control_data[4]==0 && control_data[5]==0)
{
car_mode=finding_L;
}
else if(out_edge==Right && control_data[4]==0 && control_data[5]==0)
{
car_mode=finding_R;
}
else if (control_data[3]>0 && control_data[4]>0 && control_data[5]>0)
{
car_mode=run;
}
if(control_data[5]<=12 && control_data[5]>=3)
{
LED1=0;
car_mode=stop;
}
else {LED1=1;}
if(Boma4==0)//強制菜單
{
car_mode=stop;
}
//尋找小球的 色號（0爲紅，1爲綠）
if(Boma3==0)
{
BEEP_ON_OFF=OFF;
}
else {BEEP_ON_OFF=ON;}
}
void PWM_updata()//速度控制中心
{
if(car_mode == run)
{
TIM_SetCompare1(TIM1,run_duty.Speed_Duty_R); //右爲 TIM1 CH1
TIM_SetCompare4(TIM1,run_duty.Speed_Duty_L); //左爲 TIM1 CH4
}
else if(car_mode == finding_R)
{
TIM_SetCompare1(TIM1,1400); //右爲 TIM1 CH1
TIM_SetCompare4(TIM1,1400); //左爲 TIM1 CH4
}
else if(car_mode == finding_L)
{
TIM_SetCompare1(TIM1,1500); //右爲 TIM1 CH1
TIM_SetCompare4(TIM1,1500); //左爲 TIM1 CH4
}
else if(car_mode == stop)
{
TIM_SetCompare1(TIM1,0); //右爲 TIM1 CH1
TIM_SetCompare4(TIM1,0); //左爲 TIM1 CH4
}
}
void PD_control()
{
last_x_error=x_error;
x_error=control_data[3]-80;
ser_duty = C_P*x_error-C_D*(last_x_error-x_error);
run_duty.Speed_Duty_R=1550-ser_duty;//正爲正轉
run_duty.Speed_Duty_L=1350-ser_duty;
//左邊FTM波//限幅
run_duty.Speed_Duty_L=run_duty.Speed_Duty_L<1300?1300:run_duty.Speed_Duty_L;
run_duty.Speed_Duty_L=run_duty.Speed_Duty_L>1600?1600:run_duty.Speed_Duty_L;
//右邊FTM波//限幅
run_duty.Speed_Duty_R=run_duty.Speed_Duty_L<1300?1300:run_duty.Speed_Duty_R;
run_duty.Speed_Duty_R=run_duty.Speed_Duty_L>1600?1600:run_duty.Speed_Duty_R;
}
③系統狀態設定
typedef struct D //速度結構體
{
int16 Speed_Duty_L;
int16 Speed_Duty_R;
}moty_duty;
typedef enum //枚舉小車簡單狀態
{
finding_R=4,
finding_L=3,
run=2,
stop=1,
error=0,
}
CAR_STATUS_e; //車子狀態
typedef enum
{
mode_ON_OFF=0,
car_run=1,
flash=2,
picture=3,
}
MENU_LIST_e; //OLED菜單
3.效果展示
DESIGN
1）.整體效果圖
OLED頁面設計
OELD可通過撥碼開關切換頁面
①在OLED上顯示小球的實時座標【x,y】以及距離 l
② 顯示小球在攝像頭中的座標並在屏幕上用“x”表示出來
調試界面
預留五個調試按鍵
①可調節PD參數
②切換追蹤小球顏色閾值
 圖1 小車整體效果圖 |
 圖2 小車整體效果圖 |
附【Download】：
程序+硬件（原理圖+PCB）：
注：本文僅供學習分享，如有侵權請聯繫
Dwfish 淹死的魚 [集美大學學生電子技術協會[19th]] 2018.11.26