概述

小車外形：

功能簡介

利用攝像頭識別前車尾部的AprilTag，得到前車位置，傳回stm32主控板處理，使兩車在行駛時保持恒定距離，實現(xiàn)自動跟車。

openMV4攝像頭

1.1 Apriltag識別與串口傳輸

AprilTag是一個視覺基準(zhǔn)庫，在AR，機器人，相機校準(zhǔn)領(lǐng)域廣泛使用。通過特定的標(biāo)志（與二維碼相似，但是降低了復(fù)雜度以滿足實時性要求），可以快速地檢測標(biāo)志，并計算相對位置。

Apriltag示例：

通過識別Apriltag，可以得到x,y,z三個方向的距離以及偏移角度。這里只需要三維的距離即可，通過串口傳回stm32.

import sensor, image, time, math,pybfrom pyb import UART sensor.reset()sensor.set_pixformat(sensor.RGB565)sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...sensor.skip_frames(time = 2000)sensor.set_auto_gain(False) # must turn this off to prevent image washout...sensor.set_auto_whitebal(False) # must turn this off to prevent image washout...clock = time.clock()uart = UART(3, 115200)#串口波特率 f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not setf_y = (2.8 / 2.952) * 120 # find_apriltags defaults to this if not setc_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5) def degrees(radians): return (180 * radians) / math.pi while(True): clock.tick() img = sensor.snapshot() for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11 img.draw_rectangle(tag.rect(), color = (255, 0, 0)) img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation()) #degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation())) # Translation units are unknown. Rotation units are in degrees. # print("Tx %f, Ty %f, Tz %f" % print_args) uart.write("A%.2f,B%.2f,C%.2f," % print_args+' ')#設(shè)置特定格式，以便于stm32分割取得數(shù)據(jù) #pyb.delay(500) # print(clock.fps())

STM32主控板（型號為F407）

2.1 時鐘與中斷配置

附上stm32時鐘示意圖：

定時器示意圖：

定時器分配：

所有時鐘初始化的函數(shù)：（每個函數(shù)的詳細內(nèi)容在后面）

TIM8_PWM_Init(400-1,20-1); //用于控制電機，168M/20=8.4Mhz的計數(shù)頻率,重裝載值400，所以PWM頻率為 8.4M/400=21Khz. TIM3_PWM_Init(200-1,8400-1);//用于控制舵機，50HZ TIM2_Int_Init(400-1,20-1);//定時中斷，21KHZ TIM7_Int_Init(500-1,8400-1);//用于編碼器計數(shù)，20HZ，50ms中斷一次 uart_init(115200); //初始化串口1波特率為115200 Encoder_Init_TIM4();//編碼器接口初始化

2.2 串口收發(fā)與數(shù)據(jù)處理

串口中斷：USART1，USART2
串口初始化函數(shù)（以USART1為例）：

void uart_init(u32 bound){ //GPIO端口設(shè)置 GPIO_InitTypeDef GPIO_InitStructure; USART_InitTypeDef USART_InitStructure; NVIC_InitTypeDef NVIC_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); //使能GPIOA時鐘 RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);//使能USART1時鐘 //串口1對應(yīng)引腳復(fù)用映射 GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_USART1); //GPIOA9復(fù)用為USART1 GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_USART1); //GPIOA10復(fù)用為USART1 //USART1端口配置 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; //GPIOA9與GPIOA10 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//復(fù)用功能 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度50MHz GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽復(fù)用輸出 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉 GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA9，PA10 //USART1 初始化設(shè)置 USART_InitStructure.USART_BaudRate = bound;//波特率設(shè)置 USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字長為8位數(shù)據(jù)格式 USART_InitStructure.USART_StopBits = USART_StopBits_1;//一個停止位 USART_InitStructure.USART_Parity = USART_Parity_No;//無奇偶校驗位 USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//無硬件數(shù)據(jù)流控制 USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; //收發(fā)模式 USART_Init(USART1, &USART_InitStructure); //初始化串口1 USART_Cmd(USART1, ENABLE); //使能串口1 USART_ClearFlag(USART1, USART_FLAG_TC); #if EN_USART1_RX USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//開啟相關(guān)中斷 //Usart1 NVIC 配置 NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;//串口1中斷通道 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//搶占優(yōu)先級3 NVIC_InitStructure.NVIC_IRQChannelSubPriority =3; //子優(yōu)先級3 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能 NVIC_Init(&NVIC_InitStructure); //根據(jù)指定的參數(shù)初始化VIC寄存器、 #endif }

串口中斷處理函數(shù)：

void USART1_IRQHandler(void) //串口1中斷服務(wù)程序{ u8 Res;#ifdef OS_TICKS_PER_SEC //如果時鐘節(jié)拍數(shù)定義了,說明要使用ucosII了. OSIntEnter(); #endif if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收中斷(接收到的數(shù)據(jù)必須是0x0d 0x0a結(jié)尾) { Res =USART_ReceiveData(USART1);//(USART1->DR); //讀取接收到的數(shù)據(jù) if((USART_RX_STA&0x8000)==0)//接收未完成 { if(USART_RX_STA&0x4000)//接收到了0x0d { if(Res!=0x0a)USART_RX_STA=0;//接收錯誤,重新開始 else USART_RX_STA|=0x8000; //接收完成了 } else //還沒收到0X0D { if(Res==0x0d)USART_RX_STA|=0x4000; else { USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ; USART_RX_STA++; if(USART_RX_STA>(USART_REC_LEN-1))USART_RX_STA=0;//接收數(shù)據(jù)錯誤,重新開始接收 } } } } #ifdef OS_TICKS_PER_SEC //如果時鐘節(jié)拍數(shù)定義了,說明要使用ucosII了. OSIntExit(); #endif} #endif

字符串接收與處理（從openMV接收到的數(shù)據(jù)）：

/*涉及到的全局變量float data[3];//x,y,z方向的距離，浮點數(shù)形式unsigned char data_string[3][7];//x,y,z方向的距離，字符串形式*/if(USART_RX_STA&0x8000) { //清空字符串 for(i=0;i<2;i++) { for(j=0;j<6;j++) { data_string[i][j]=' '; } } len=USART_RX_STA&0x3fff;//得到此次接收到的數(shù)據(jù)長度 for(t=0,j=0;t

字符串轉(zhuǎn)化為兩位小數(shù)浮點數(shù)（用于后續(xù)PID控制）:

int myatof(const char *str)//此函數(shù)僅適用于兩位小數(shù)的浮點數(shù)，返回的是乘100后的int值，因float返回有錯誤{ int flag = 1;//表示正數(shù) int res =0; u8 i=1; //小數(shù)點后兩位 while(*str != '') { if( !(*str >= '0' && *str <= '9'))//找到字符串中的第一個數(shù)字 { str++; continue; } if(*(str-1) == '-') { flag=-1;//表示是一個負數(shù) } while(*str >= '0' && *str <= '9') { res = res *10 + (*str - '0'); str++; } if(*str == '.') { str++; res = res *10 + (*str - '0'); str++; res = res *10 + (*str - '0');//保留兩位，故加兩次 return res*flag; } }}

2.3 LCD顯示模塊

LCD模塊用于調(diào)試時觀察數(shù)據(jù)，調(diào)試完成可以刪去，因為顯示屏很耗時，使處理速度變慢
驅(qū)動函數(shù)總覽：

void LCD_GPIO_Init(void);void Lcd_WriteIndex(u8 Index);void Lcd_WriteData(u8 Data);void Lcd_WriteReg(u8 Index,u8 Data);u16 Lcd_ReadReg(u8 LCD_Reg);void Lcd_Reset(void);void Lcd_Init(void);void Lcd_Clear(u16 Color);void Lcd_SetXY(u16 x,u16 y);void Gui_DrawPoint(u16 x,u16 y,u16 Data);unsigned int Lcd_ReadPoint(u16 x,u16 y);void Lcd_SetRegion(u16 x_start,u16 y_start,u16 x_end,u16 y_end);void LCD_WriteData_16Bit(u16 Data);

TFT屏幕初始化：

void TFT_Init_Show(void){ Lcd_Clear(WHITE); Gui_DrawFont_GBK16(16,70,BLACK,WHITE,"by WILL CHAN"); delay_ms(1000); Lcd_Clear(WHITE); Gui_DrawFont_GBK16(3,0,RED,WHITE,"X:"); Gui_DrawFont_GBK16(3,20,RED,WHITE,"Y:"); Gui_DrawFont_GBK16(3,40,RED,WHITE,"Z:"); Gui_DrawFont_GBK16(3,60,RED,WHITE,"speed:");}

字符串顯示函數(shù);

void Gui_DrawFont_GBK16(u16 x, u16 y, u16 fc, u16 bc, u8 *s){ unsigned char i,j; unsigned short k,x0; x0=x; while(*s) { if((*s) < 128) { k=*s; if (k==13) { x=x0; y+=16; } else { if (k>32) k-=32; else k=0; for(i=0;i<16;i++) for(j=0;j<8;j++) { if(asc16[k*16+i]&(0x80>>j)) Gui_DrawPoint(x+j,y+i,fc); else { if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc); } } x+=8; } s++; } else { for (k=0;k>j)) Gui_DrawPoint(x+j,y+i,fc); else { if (fc!=bc) Gui_DrawPoint(x+j,y+i,bc); } } for(j=0;j<8;j++) { if(hz16[k].Msk[i*2+1]&(0x80>>j)) Gui_DrawPoint(x+j+8,y+i,fc); else { if (fc!=bc) Gui_DrawPoint(x+j+8,y+i,bc); } } } } } s+=2;x+=16; } }}

2.4 電機、舵機與編碼器

定時中斷：TIM2，用于修改電機和舵機的PWM占空比
初始化函數(shù)：

//通用定時器2中斷初始化//arr：自動重裝值。//psc：時鐘預(yù)分頻數(shù)//定時器溢出時間計算方法:Tout=((arr+1)*(psc+1))/Ft us.//Ft=定時器工作頻率,單位:Mhz//這里使用的是定時器2!void TIM2_Int_Init(u16 arr,u16 psc){ TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; NVIC_InitTypeDef NVIC_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE); ///使能TIM2時鐘 TIM_TimeBaseInitStructure.TIM_Period = arr; //自動重裝載值 TIM_TimeBaseInitStructure.TIM_Prescaler=psc; //定時器分頻 TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上計數(shù)模式 TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);//初始化TIM3 TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE); //允許定時器2更新中斷 TIM_Cmd(TIM2,ENABLE); //使能定時器2 NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定時器2中斷 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0; //搶占優(yōu)先級1 NVIC_InitStructure.NVIC_IRQChannelSubPriority=2; //子優(yōu)先級3 NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE; NVIC_Init(&NVIC_InitStructure); }

TIM2中斷處理函數(shù)：

void TIM2_IRQHandler(void){ if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中斷 { if(motor_flag==1)//反轉(zhuǎn) { TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定時器的自動重裝載值進行比較，來設(shè)置占空比,引腳：PC6 TIM_SetCompare2(TIM8,0); } if(motor_flag==0)//正轉(zhuǎn) { TIM_SetCompare1(TIM8,0); TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定時器的自動重裝載值進行比較，來設(shè)置占空比，引腳：PC7 } TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//設(shè)置舵機角度，引腳：PA6 } TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中斷標(biāo)志位}

PWM輸出：TIM3（舵機），TIM8（電機）
初始化函數(shù)（以TIM8為例）：

void TIM8_PWM_Init(u32 arr,u32 psc){ //此部分需手動修改IO口設(shè)置 GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; TIM_BDTRInitTypeDef TIM_BDTRInitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8,ENABLE); //TIM8時鐘使能 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOB|RCC_AHB1Periph_GPIOC, ENABLE); //使能PORTA時鐘 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7; //GPIOFA GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //復(fù)用功能 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //速度100MHz GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽復(fù)用輸出 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉 GPIO_Init(GPIOA,&GPIO_InitStructure); //初始化PA7 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; GPIO_Init(GPIOB,&GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8; GPIO_Init(GPIOC,&GPIO_InitStructure); GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM8); //GPIOA8復(fù)用為定時器1 GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOA9復(fù)用為定時器1 GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM8); //GPIOA10復(fù)用為定時器1 GPIO_PinAFConfig(GPIOA,GPIO_PinSource7,GPIO_AF_TIM8); //GPIOB13復(fù)用為定時器1 GPIO_PinAFConfig(GPIOB,GPIO_PinSource0,GPIO_AF_TIM8); //GPIOB14復(fù)用為定時器1 GPIO_PinAFConfig(GPIOB,GPIO_PinSource1,GPIO_AF_TIM8); //GPIOB15復(fù)用為定時器1 TIM_TimeBaseStructure.TIM_Prescaler=psc; //定時器分頻 TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上計數(shù)模式 TIM_TimeBaseStructure.TIM_Period=arr; //自動重裝載值 TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1; TIM_TimeBaseInit(TIM8,&TIM_TimeBaseStructure);//初始化定時器1 //初始化PWM模式 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable; TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset; TIM_OC1Init(TIM8, &TIM_OCInitStructure); TIM_OC2Init(TIM8, &TIM_OCInitStructure); TIM_OC3Init(TIM8, &TIM_OCInitStructure); TIM_OC1PreloadConfig(TIM8,TIM_OCPreload_Enable); TIM_OC2PreloadConfig(TIM8,TIM_OCPreload_Enable); TIM_OC3PreloadConfig(TIM8,TIM_OCPreload_Enable); TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable; TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable; TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF; TIM_BDTRInitStructure.TIM_DeadTime = 0; TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable; TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_Low; TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable; TIM_BDTRConfig(TIM8,&TIM_BDTRInitStructure); TIM_Cmd(TIM8,ENABLE); TIM_CCPreloadControl(TIM8,ENABLE); TIM_CtrlPWMOutputs(TIM8,ENABLE); }

編碼器初始化函數(shù)：

void Encoder_Init_TIM4(void){ GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; NVIC_InitTypeDef NVIC_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);//開啟TIM4時鐘 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);//開啟GPIOB時鐘 GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);//PB6引腳復(fù)用 GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);//PB7引腳服用 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6,GPIOB7 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; //GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_Init(GPIOB,&GPIO_InitStructure); NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; /*它的搶占優(yōu)先級可以盡量設(shè)置低點*/ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//禁用中斷，防止計數(shù)溢出而沒有相應(yīng)函數(shù)，造成卡死 NVIC_Init(&NVIC_InitStructure); TIM_TimeBaseStructure.TIM_Period = 4095; //設(shè)置下一個更新事件裝入活動的自動重裝載寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler = 0; //設(shè)置用來作為TIMx時鐘頻率除數(shù)的預(yù)分頻值 不分頻 TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設(shè)置時鐘分割:TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數(shù)模式 TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising,TIM_ICPolarity_Falling); TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = 10; //輸入濾波器 TIM_ICInit(TIM4, &TIM_ICInitStructure); TIM_ClearFlag(TIM4, TIM_FLAG_Update); //清除所有標(biāo)志位 TIM_ITConfig(TIM4, TIM_IT_Update, DISABLE); //允許中斷更新 TIM4->CNT = 0; TIM_Cmd(TIM4, ENABLE); //使能TIM4}

編碼器返回速度值：

/**************************************************************************函數(shù)功能：單位時間讀取編碼器計數(shù)入口參數(shù)：定時器返回 值：速度值**************************************************************************/float Read_Encoder_Speed(uint8_t TIMX){ int32_t Encoder_TIM; float res = 0; switch (TIMX) { case 5: Encoder_TIM = TIM_GetCounter(TIM5); TIM5->CNT = ENCODER_BASE_COUNT; res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT; break; case 4: Encoder_TIM = TIM_GetCounter(TIM4); TIM4->CNT = ENCODER_BASE_COUNT; res = (int32_t)Encoder_TIM - ENCODER_BASE_COUNT; break; default: Encoder_TIM = 0; res = 0; } if(res>2048.0f) res-=4096.0f; return res*360.0f/4096.0f;}

定時從編碼器取數(shù)，注意，時間不一樣，取回的數(shù)值也不一樣，取決于實際速度以及編碼器線數(shù)。這里50ms取一次：

void TIM7_IRQHandler(void)//頻率20HZ，用于編碼器計數(shù){ if(TIM_GetITStatus(TIM7,TIM_IT_Update)==SET)//溢出中斷 { speed=Read_Encoder_Speed(4); } TIM_ClearITPendingBit(TIM7,TIM_IT_Update); //清除中斷標(biāo)志位}

2.5 PID控制

PID庫函數(shù)：

#define N 2 //需要對多少變量進行pid調(diào)節(jié) const float KP[N]={1.3,1.0};//這里只用了比例調(diào)節(jié)const float KI[N]={0,0};const float KD[N]={0,0}; struct _pid{ float SetVol; //定義設(shè)定值 float ActVol; //定義實際值 float Err; //定義誤差 float Err_Next; //定義上一個誤差 float Err_Last; //定義上上一個誤差 float Kp,Ki,Kd; //定義比例、積分、微分系數(shù) float integral; //定義積分值 float actuator; //定義控制器執(zhí)行變量}pid[N]; void PID_Init(void){ for(int i=0;i

主函數(shù)中的PID調(diào)節(jié)：

z_get=data[2]; x_get=data[0]; if(z_get-z_set>0.5||z_get-z_set<-0.5)//電機PID { LED1=0; //調(diào)節(jié)時燈亮 PID_val_motor=PID_realize(z_set,z_get,0); PID_val_motor=PID_val_motor/10.0; if(PID_val_motor<=0) motor_flag=0;//motor_flag控制電機正反轉(zhuǎn)，PID_val_motor用于改變占空比，范圍0~1 if(PID_val_motor>0) motor_flag=1; PID_val_motor=abs_float(PID_val_motor); if(PID_val_motor>2)PID_val_motor=0;//標(biāo)志太遠，讓車停止 if(PID_val_motor>1&&PID_val_motor<=2)PID_val_motor=1; if(PID_val_motor<0.2)PID_val_motor=0; } if(x_get-x_set>0.1||x_get-x_set<-0.1)//舵機PID { LED1=0; PID_val_servo=PID_realize(x_set,x_get,1); servo_angle=((140-35)/6)*PID_val_servo+35;//線性映射，把PID的值轉(zhuǎn)化為角度35~140的舵機轉(zhuǎn)角 if(servo_angle<35)servo_angle=35; if(servo_angle>140)servo_angle=140; } LED1=1;

定時器TIM2中斷里改變占空比：

void TIM2_IRQHandler(void){ if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET)//溢出中斷 { if(motor_flag==1)//反轉(zhuǎn) { TIM_SetCompare1(TIM8,motor_duty*PID_val_motor*400.0);//和定時器的自動重裝載值進行比較，來設(shè)置占空比,引腳：PC6 TIM_SetCompare2(TIM8,0); } if(motor_flag==0)//正轉(zhuǎn) { TIM_SetCompare1(TIM8,0); TIM_SetCompare2(TIM8,motor_duty*PID_val_motor*400.0);//和定時器的自動重裝載值進行比較，來設(shè)置占空比，引腳：PC7 } TIM_SetCompare1(TIM3,200-(servo_angle/45.0+1)*5);//設(shè)置舵機角度，根據(jù)舵機手冊得到占空比與轉(zhuǎn)角的關(guān)系，引腳：PA6 } TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中斷標(biāo)志位}

電源與電機驅(qū)動

3.1 L298N電機驅(qū)動板

因為后面兩路電機要求同速，故把AB兩通道用線短接，用一路PWM控制兩路電機。

下面是使用說明：

具體控制代碼見上面TIM2中斷處理函數(shù)中，利用兩路定時器輪流輸出PWM（另一路為零），即可控制電機正反轉(zhuǎn)。

3.2 LM2596降壓模塊

手冊中的典型連接：

原理圖如下：

3.3 電源部分注意事項

1.電池用的是12v航模鋰電池，為了防止過放導(dǎo)致電池損壞，必須要在電池輸入端加一個電壓表模塊，如下圖：

2.控制部分電源和電機舵機電源分開，因為電機舵機啟動時會過大電流，導(dǎo)致電壓不穩(wěn)定，影響芯片供電。這里L(fēng)M2596給電機供電，一個LM2596給舵機供電，另一個LM2596給單片機和openMV供電。

3.控制電源和電機舵機電源分別加開關(guān)，下程序的時候先關(guān)閉電機和舵機的電源。因為此時控制器沒有給信號，電機和舵機可能會不受控制的運動。

責(zé)任編輯：lq