一、主要內(nèi)容
結(jié)合阿克曼運(yùn)動(dòng)需求,本團(tuán)隊(duì)設(shè)計(jì)了阿克曼運(yùn)動(dòng)系統(tǒng)框圖如圖
主要包括阿克曼小車仿真設(shè)計(jì)和阿克曼小車實(shí)物實(shí)現(xiàn)。
圖 1 阿克曼運(yùn)動(dòng)系統(tǒng)框圖
1.1 阿克曼小車仿真設(shè)計(jì)
本節(jié)先介紹阿克曼小車模型,小車仿真設(shè)計(jì)包括SolidWorks和gazebo三維建模。
1.1.1 阿克曼小車
阿克曼小車是一款經(jīng)典的車模,小車模型后輪是通過(guò)電機(jī)直驅(qū),前輪通過(guò)舵機(jī)控制前輪轉(zhuǎn)向角,前輪部分則是在模型中添加一個(gè)豎直的關(guān)節(jié),使前輪能圍繞此關(guān)節(jié)轉(zhuǎn)動(dòng)
1.1.2 SolidWorks三維模型
采用SolidWorks (2016版)設(shè)計(jì)軟件搭建阿克曼小車三維模型如圖 2圖 3圖 4,主體分為底板、電池、控制器、帶編碼電機(jī)的后輪、激光雷達(dá)、深度攝像頭、阿克曼前輪轉(zhuǎn)向7個(gè)部分如圖 5。
各部分零件都定義了材料屬性,選擇取小車底盤中心為模型的原點(diǎn),配置了小車JRDF文件見(jiàn)附件3。
目前可以導(dǎo)入到gazebo環(huán)境中,可以實(shí)現(xiàn)小車的前進(jìn)與后退,但轉(zhuǎn)向無(wú)法實(shí)現(xiàn),原因分析由于阿克曼轉(zhuǎn)向結(jié)構(gòu)屬于空間四連桿結(jié)構(gòu) (并聯(lián)結(jié)構(gòu)不支持)如圖 6。
圖2正等測(cè)
圖3前視圖
圖4俯視圖
圖5爆炸視圖
圖6模型導(dǎo)入gazebo效果
1.1.3 gazebo三維建模
為了簡(jiǎn)化小車的運(yùn)動(dòng),直接調(diào)入用阿克曼模型,簡(jiǎn)化小車的前后輪運(yùn)動(dòng)關(guān)系,搭配了里程計(jì)和攝像頭,可以實(shí)現(xiàn)小車的轉(zhuǎn)向、直行和后退。
小車分別導(dǎo)入到gazebo和rviz效果分別如圖 7圖 8建模過(guò)程見(jiàn)附件1文檔
圖 7 加載到gazebo
圖8 加載到rvzi
1.2 阿克曼小車實(shí)物實(shí)現(xiàn)
小車底層搭載STM32F103系列單片機(jī),運(yùn)動(dòng)控制算法采用阿克曼算法,解析后分別驅(qū)動(dòng)舵機(jī)和編碼電機(jī),可以通過(guò)審口通訊實(shí)現(xiàn)上下位機(jī)的人機(jī)交互,方便調(diào)試我們?cè)O(shè)計(jì)了PS2手柄控制模式。
實(shí)現(xiàn)小車的實(shí)物制作如圖 9。
圖9小車實(shí)物
1.2.1控制器
小車控制器采用意法半導(dǎo)體STM32F103C6,是一款 ARM 32位 Cortex-M3 微控制器,2MHz 32B 閃存,10KB SRAM,PLL,嵌入式內(nèi)部 RC 8MHz和 32KHz,實(shí)時(shí)時(shí)鐘,嵌套中斷控制器,省電式。
JTAG和SWD,2同步,具有輸入捕捉、輸出比和PWM的16位定時(shí)器、16位6通道高級(jí)定時(shí)器、2個(gè)16位看門狗定時(shí)器、SysTick定時(shí)器、SP112C、2個(gè)USART、USB2.0全速接口、CAN2.0B激活、2個(gè)12位10通道D轉(zhuǎn)換器。
快速 /0 端口如圖 10整體資源滿足小車求,10使用情況詳細(xì)說(shuō)明,STM32核心板和底板原理圖見(jiàn)附件1。
圖 10 STM32F103引腳定義圖
1.2.2 阿克曼運(yùn)動(dòng)算法
阿克曼轉(zhuǎn)向是一種現(xiàn)代汽車的轉(zhuǎn)向方式,在汽車轉(zhuǎn)彎的時(shí)候,內(nèi)外輪轉(zhuǎn)過(guò)的角度不一樣,內(nèi)側(cè)輪胎轉(zhuǎn)彎半徑小于外側(cè)輪胎。理想的阿克曼轉(zhuǎn)向如圖 11,而本車模型采用反向的阿克曼模型。
圖 11 理想的阿克曼轉(zhuǎn)向
根據(jù)阿克曼轉(zhuǎn)向幾何設(shè)計(jì)轉(zhuǎn)向機(jī)構(gòu),在車輛沿著彎道轉(zhuǎn)彎時(shí),利用四連桿的相等曲柄,可以使內(nèi)側(cè)輪的轉(zhuǎn)向角比外側(cè)輪大大約 2~4度,使四個(gè)輪子路徑的圓心大致上交會(huì)于后軸的延長(zhǎng)線上瞬時(shí)轉(zhuǎn)向中心,從而讓車輛可以順暢的轉(zhuǎn)彎。
阿克曼核心公式如下;
式中:B一汽車前外輪轉(zhuǎn)角,a 一汽車前內(nèi)輪轉(zhuǎn)角,K一兩主銷中心距,L一軸距如圖 12。
具體實(shí)現(xiàn)見(jiàn)附件2中control.c中Kinematic Analysis函數(shù)。
圖 12 阿克曼數(shù)學(xué)模型
control.c
?
?
#include "control.h" ? //#define T 0.245f //#define L 0.29f //#define K 14.00f #define T 0.156f #define L 0.1445f #define K 622.8f u8 Flag_Target,Flag_Change; ?//相關(guān)標(biāo)志位 //float Voltage_Temp,Voltage_Count,Voltage_All; ?//電壓采樣相關(guān)變量 int j,sum; /************************************************************************** 函數(shù)功能:小車運(yùn)動(dòng)數(shù)學(xué)模型 入口參數(shù):速度和轉(zhuǎn)角 返回 ?值:無(wú) **************************************************************************/ void Kinematic_Analysis(float velocity,float angle) { ? ?Target_A=velocity*(1+T*tan(angle)/2/L); ? ?Target_B=velocity*(1-T*tan(angle)/2/L); ? ? ?//后輪差速 ? ?Servo=SERVO_INIT+angle*K; ? ? ? ? ? ? ? ? ? ?//舵機(jī)轉(zhuǎn)向 ? } /************************************************************************** 函數(shù)功能:所有的控制代碼都在這里面 ? ? ? ? 定時(shí)中斷觸發(fā) ? ? ? ? 嚴(yán)格保證采樣和數(shù)據(jù)處理的時(shí)間同步 ? ? ? ? **************************************************************************/ void Control(void) { ?oled_show(); ? ? ? ? ? ? ? //顯示屏打開(kāi) ?Encoder_Left=Read_Encoder(2); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?Encoder_Right=-Read_Encoder(3); ? ? ?//讀取左右編碼器 ? ?delay_ms(50); ? ? ? ? ? ? ? ? ?//=====延時(shí)等待穩(wěn)定 ? ?if(Turn_Off(Voltage)==0&&Flag_Way==0) ? { ? ? ? ?jiexi(); ? ? ?Kinematic_Analysis(Velocity,Angle); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //小車運(yùn)動(dòng)學(xué)分析 ? ? ?Motor_A=Target_A*20; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //===計(jì)算電機(jī)A最終PWM ? ? ?Motor_B=Target_B*20; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//===計(jì)算電機(jī)B最終PWM ? ? ?Xianfu_Pwm(); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//===PWM限幅 ? ? ?Set_Pwm(Motor_A,Motor_B,Servo); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//===賦值給PWM寄存器 ? ? ? ? } ? ? ? ?else if(Turn_Off(Voltage)==0&&Flag_Way==1) ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//===如果不存在異常 ? ?{ ? ? ?Get_RC(); ? ? ?Kinematic_Analysis(Velocity,Angle); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //小車運(yùn)動(dòng)學(xué)分析 ? ? ?Motor_A=Incremental_PI_Left(Encoder_Left,Target_A); ? ? ? ? ? ? ? ? ? //===速度閉環(huán)控制計(jì)算電機(jī)A最終PWM ? ? ?Motor_B=Incremental_PI_Right(Encoder_Right,Target_B); ? ? ? ? ? ? ? ? ?//===速度閉環(huán)控制計(jì)算電機(jī)B最終PWM ? ? ?Xianfu_Pwm(); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//===PWM限幅 ? ? ?Set_Pwm(Motor_A,Motor_B,Servo); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//===賦值給PWM寄存器 ? ? ?} ?else Set_Pwm(0,0,SERVO_INIT); ?//===賦值給PWM寄存器 ? ? ?Voltage_Temp=Get_battery_volt(); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //=====讀取電池電壓 ? ? ?Voltage_Count++; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //=====平均值計(jì)數(shù)器 ?Voltage_All+=Voltage_Temp; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //=====多次采樣累積 ?if(Voltage_Count==10) Voltage=Voltage_All/10,Voltage_All=0,Voltage_Count=0;//=====求平均值 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?if(Flag_Show==0) ? ? ? ?Led_Flash(100); ?else if(Flag_Show==1) ?Led_Flash(0); ?//led閃爍 ?Key(); ? ?//===掃描按鍵狀態(tài) 單擊雙擊可以改變小車運(yùn)行狀態(tài) } /************************************************************************** 函數(shù)功能:賦值給PWM寄存器 入口參數(shù):PWM 返回 ?值:無(wú) **************************************************************************/ void Set_Pwm(int motor_a,int motor_b,int servo) { ? ? ?if(motor_a<0) ? ? ?PWMA2=7200,PWMA1=7200+motor_a; ? ? ?else ? ? ? ? ? ? ? PWMA1=7200,PWMA2=7200-motor_a; ? ? ? ? ?if(motor_b<0) ? ? ?PWMB1=7200,PWMB2=7200+motor_b; ? ? ?else ? ? ? ? ? ? ? PWMB2=7200,PWMB1=7200-motor_b; ? ? SERVO=servo; ? } /************************************************************************** 函數(shù)功能:限制PWM賦值 入口參數(shù):幅值 返回 ?值:無(wú) **************************************************************************/ void Xianfu_Pwm(void) { ? ? ?int Amplitude=6900; ? ?//===PWM滿幅是7200 限制在6900 ? ?if(Motor_A<-Amplitude) Motor_A=-Amplitude; ? ? ?if(Motor_A>Amplitude) ?Motor_A=Amplitude; ? ? ?if(Motor_B<-Amplitude) Motor_B=-Amplitude; ? ? ?if(Motor_B>Amplitude) ?Motor_B=Amplitude; ? ? ? ?if(Servo<(SERVO_INIT-500)) ? ? Servo=SERVO_INIT-500; ? ?//舵機(jī)限幅 ? ?if(Servo>(SERVO_INIT+500)) ? ? Servo=SERVO_INIT+500; ? ? ?//舵機(jī)限幅 } /************************************************************************ 函數(shù)功能:按鍵修改小車運(yùn)行狀態(tài) 入口參數(shù):無(wú) 返回 ?值:無(wú) **************************************************************************/ void Key(void) { ? ?u8 tmp,tmp2; ?tmp=click(); // ?tmp=click_N_Double(50); //雙擊,雙擊等待時(shí)間500ms ?if(tmp==1)Flag_Stop=!Flag_Stop;//單擊控制小車的啟停 ?//if(tmp==2)Flag_Show=!Flag_Show;//雙擊控制小車的顯示狀態(tài) ?tmp2=Long_Press(); ?//長(zhǎng)按 ? ? ? ? ?if(tmp2==1)Flag_Show=!Flag_Show;//控制小車的顯示狀態(tài) ? ? ? ? ? ? ? ? } /************************************************************************** 函數(shù)功能:異常關(guān)閉電機(jī) 入口參數(shù):電壓 返回 ?值:1:異常 ?0:正常 **************************************************************************/ u8 Turn_Off( int voltage) { ? ? ?u8 temp; ? ? ?if(voltage<740||Flag_Stop==1)//電池電壓低于11.1V關(guān)閉電機(jī) ? ? ?{ ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?temp=1; ? ? ? ?PWMA1=0; //電機(jī)控制位清零 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?PWMB1=0; //電機(jī)控制位清零 ? ? ?PWMA2=0; //電機(jī)控制位清零 ? ? ?PWMB2=0; //電機(jī)控制位清零 ? ? ? ? ? ? ? ? ?} ? ? ?else ? ? ?temp=0; ? ? ?return temp; ? ? } /************************************************************************** 函數(shù)功能:絕對(duì)值函數(shù) 入口參數(shù):int 返回 ?值:unsigned int **************************************************************************/ int myabs(int a) { ? ? ? ? ? ?int temp; ? ?if(a<0) ?temp=-a; ? ? ?else temp=a; ? ?return temp; } /************************************************************************** 函數(shù)功能:增量PI控制器 入口參數(shù):編碼器測(cè)量值,目標(biāo)速度 返回 ?值:電機(jī)PWM 根據(jù)增量式離散PID公式 pwm+=Kp[e(k)-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)] e(k)代表本次偏差 e(k-1)代表上一次的偏差 ?以此類推 pwm代表增量輸出 在我們的速度控制閉環(huán)系統(tǒng)里面,只使用PI控制 pwm+=Kp[e(k)-e(k-1)]+Ki*e(k) **************************************************************************/ int Incremental_PI_Left (int Encoder,int Target) { ? ? static int Bias,Pwm,Last_bias; ? Bias=Target-Encoder; ? ? ? ? ? ? ? ?//計(jì)算偏差 ? Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; ? //增量式PI控制器 ? Last_bias=Bias; ? ? ? ? ? ? ? ? ? ? //保存上一次偏差 ? return Pwm; ? ? ? ? ? ? ? ? ? ? ? ? //增量輸出 } int Incremental_PI_Right (int Encoder,int Target) { ? ? static int Bias,Pwm,Last_bias; ? Bias=Target-Encoder; ? ? ? ? ? ? ? ?//計(jì)算偏差 ? Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; ? //增量式PI控制器 ? Last_bias=Bias; ? ? ? ? ? ? ? ? ? ? //保存上一次偏差 ? return Pwm; ? ? ? ? ? ? ? ? ? ? ? ? //增量輸出 } /************************************************************************** 函數(shù)功能:通過(guò)指令對(duì)小車進(jìn)行遙控 入口參數(shù):PS2指令 返回 ?值:無(wú) **************************************************************************/ void Get_RC(void)//PS2控制 { ?int Yuzhi=2; ?float LY,RX; ?LY=PS2_LY-128; ? ? //計(jì)算偏差 ?RX=PS2_RX-128; ?if(LY>-Yuzhi&&LY-Yuzhi&&RX ?
?
1.2.3 舵機(jī)驅(qū)動(dòng)
腦機(jī)的控制由一個(gè)脈沖寬度調(diào)制信號(hào)(PWM)來(lái)實(shí)現(xiàn),該信號(hào)由 stm32發(fā)出。
通常來(lái)說(shuō),夠機(jī)的控制信號(hào)周期為 20ms的脈寬調(diào)制信號(hào)其中脈沖寬度從 0.5-2.5對(duì)應(yīng)盤位置的 0-180度如圖 13,呈線性變化,也就是說(shuō)給它提供一定的脈寬。
它的輸出軸就會(huì)保持在一定對(duì)應(yīng)角度上,無(wú)論外界力矩怎么改變,直到給它提供另外寬度的脈沖信號(hào),它才會(huì)改變輸出角度到新的位置上。
圖 13 舵機(jī)輸出轉(zhuǎn)角與輸入信號(hào)脈沖寬度的關(guān)系
采用DSServo達(dá)盛金屬舵機(jī)如圖 14,型號(hào)為 DS3230,適用于航模、車模、船模及機(jī)器人的小型機(jī),額定扭矩 3N.m,轉(zhuǎn)動(dòng)角度 270度。
具體定義見(jiàn)附件2中motor.c,功能實(shí)現(xiàn)見(jiàn)control.c中Kinematic Analysis函數(shù)
motor .c
?
?
#include "motor.h" void Motor_PWM_Init(u16 arr,u16 psc) { ? ? ? ? ?RCC->APB1ENR|=1<<2; ? ? ? //TIM4時(shí)鐘使能 ? ? ?RCC->APB2ENR|=1<<3; ? ? ? //PORTB時(shí)鐘使能 ? ?GPIOB->CRL&=0X00FFFFFF; ? //PORTB6 7 ?8 9推挽輸出 ?GPIOB->CRL|=0XBB000000; ? //PORTB6 7 ?8 9推挽輸出 ?GPIOB->CRH&=0XFFFFFF00; ? //PORTB6 7 ?8 9推挽輸出 ?GPIOB->CRH|=0X000000BB; ? //PORTB6 7 ?8 9推挽輸出 ?TIM4->ARR=arr;//設(shè)定計(jì)數(shù)器自動(dòng)重裝值 ?TIM4->PSC=psc;//預(yù)分頻器不分頻 ?TIM4->CCMR1|=6<<4;//CH1 PWM1模式 ? ?TIM4->CCMR1|=6<<12; //CH2 PWM1模式 ? ?TIM4->CCMR2|=6<<4;//CH3 PWM1模式 ? ?TIM4->CCMR2|=6<<12; //CH4 PWM1模式 ? ? ?TIM4->CCMR1|=1<<3; //CH1預(yù)裝載使能 ? ? ?TIM4->CCMR1|=1<<11;//CH2預(yù)裝載使能 ? ?TIM4->CCMR2|=1<<3; //CH3預(yù)裝載使能 ? ? ?TIM4->CCMR2|=1<<11;//CH4預(yù)裝載使能 ? ?TIM4->CCER|=1<<0; ?//CH1輸出使能 ? ?TIM4->CCER|=1<<4; ?//CH2輸出使能 ? ? ?TIM4->CCER|=1<<8; ?//CH3輸出使能 ? ?TIM4->CCER|=1<<12; //CH4輸出使能 ? ? ?TIM4->CR1=0x80; ? ?//ARPE使能 ?TIM4->CR1|=0x01; ? //使能定時(shí)器 } /***************** ? ********************************************************* 函數(shù)功能:舵機(jī)PWM以及定時(shí)中斷初始化 入口參數(shù):入口參數(shù):arr:自動(dòng)重裝值 psc:時(shí)鐘預(yù)分頻數(shù) 返回 ?值:無(wú) **************************************************************************/ void Servo_PWM_Init(u16 arr,u16 psc) ? { ? ? ?RCC->APB2ENR|=1<<11; ? ? ? //使能TIM1時(shí)鐘 ? ? ?RCC->APB2ENR|=1<<2; ? ? ? ?//PORTA時(shí)鐘使能 ?GPIOA->CRH&=0XFFFF0FFF; ? ?//PORTA11復(fù)用輸出 ?GPIOA->CRH|=0X0000B000; ? ?//PORTA11復(fù)用輸出 ?TIM1->ARR=arr; ? ? ? ? ? ? //設(shè)定計(jì)數(shù)器自動(dòng)重裝值 ?TIM1->PSC=psc; ? ? ? ? ? ? //預(yù)分頻器不分頻 ?TIM1->CCMR2|=6<<12; ? ? ? ?//CH4 PWM1模式 ? ?TIM1->CCMR2|=1<<11; ? ? ? ?//CH4預(yù)裝載使能 ? ? ?TIM1->CCER|=1<<12; ? ? ? ? //CH4輸出使能 ? ? ?TIM1->BDTR |= 1<<15; ? ? ? //TIM1必須要這句話才能輸出PWM ?TIM1->CR1 = 0x80; ? ? ? ? ? //ARPE使能 ?TIM1->DIER|=1<<0; ? ? ? ? //允許更新中斷 ? ?TIM1->CR1|=0x01; ? ? ? ? ?//使能定時(shí)器1 ? ? ?TIM1->CCR4=1500; ? // ?MY_NVIC_Init(1,1,TIM1_UP_IRQn,2); }?
?
1.2.4 編碼電機(jī)驅(qū)動(dòng)
直流減速電機(jī)接口方式如圖 15,是一個(gè)帶編碼器的直流減速電機(jī),編碼器的作用是測(cè)速。
一般包括六個(gè)接線端子,電機(jī)電源輸入 M1和電機(jī)電源輸入 M2是直流電機(jī)引腳,電機(jī)的旋轉(zhuǎn)和速度調(diào)節(jié)只需這兩個(gè)引腳即可。剩下中間的四個(gè)引腳是編碼器。
圖 15 編碼電機(jī)
STM32F103的高級(jí)控制定時(shí)器 TIM1和 TIM8在基本定時(shí)器的基礎(chǔ)上引入了外部腳,可以輸入捕獲和輸出比較功能。
高級(jí)定時(shí)器能夠完成輸入捕獲和輸出比較的功能,輸出比較包括翻轉(zhuǎn)、強(qiáng)制為有效電平、PWM1和 PWM2等模式,其中 PWM模時(shí)最常用的。
電機(jī)采用定時(shí)器TIM8,兩路編碼器分別采用定時(shí)器TIM2和TIM3。
電機(jī)具體定義見(jiàn)附件2中motr.c,編碼器具體定義見(jiàn)附件2中encoder.c,功能實(shí)現(xiàn)凡control.c中Kinematic Analysis函數(shù)。
encoder.c
?
?
#include "encoder.h" #include "stm32f10x_gpio.h" /************************************************************************** 函數(shù)功能:把TIM2初始化為編碼器接口模式 入口參數(shù):無(wú) 返回 值:無(wú) **************************************************************************/ void Encoder_Init_TIM2(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);// 需要使能AFIO時(shí)鐘 RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定時(shí)器2的時(shí)鐘 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PA端口時(shí)鐘 //RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//使能PB端口時(shí)鐘 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; //端口配置 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空輸入 GPIO_Init(GPIOA, &GPIO_InitStructure); //根據(jù)設(shè)定參數(shù)初始化GPIOA TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 預(yù)分頻器 TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; //設(shè)定計(jì)數(shù)器自動(dòng)重裝值 TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//選擇時(shí)鐘分頻:不分頻 TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上計(jì)數(shù) TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用編碼器模式3 TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = 10; TIM_ICInit(TIM2, &TIM_ICInitStructure); TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新標(biāo)志位 TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //Reset counter TIM_SetCounter(TIM2,0); TIM_Cmd(TIM2, ENABLE); } /************************************************************************** 函數(shù)功能:把TIM3初始化為編碼器接口模式 入口參數(shù):無(wú) 返回 值:無(wú) **************************************************************************/ void Encoder_Init_TIM3(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);//使能定時(shí)器3的時(shí)鐘 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PB端口時(shí)鐘 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //端口配置 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空輸入 GPIO_Init(GPIOA, &GPIO_InitStructure); //根據(jù)設(shè)定參數(shù)初始化GPIOA TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 預(yù)分頻器 TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; //設(shè)定計(jì)數(shù)器自動(dòng)重裝值 TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//選擇時(shí)鐘分頻:不分頻 TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上計(jì)數(shù) TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用編碼器模式3 TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = 10; TIM_ICInit(TIM3, &TIM_ICInitStructure); TIM_ClearFlag(TIM3, TIM_FLAG_Update);//清除TIM的更新標(biāo)志位 TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); //Reset counter TIM_SetCounter(TIM3,0); TIM_Cmd(TIM3, ENABLE); } /************************************************************************** 函數(shù)功能:?jiǎn)挝粫r(shí)間讀取編碼器計(jì)數(shù) 入口參數(shù):定時(shí)器 返回 值:速度值 **************************************************************************/ int Read_Encoder(u8 TIMX) { int Encoder_TIM; switch(TIMX) { case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break; case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0;break; default: Encoder_TIM=0; } return Encoder_TIM; } /************************************************************************** 函數(shù)功能:TIM3中斷服務(wù)函數(shù) 入口參數(shù):無(wú) 返回 值:無(wú) **************************************************************************/ void TIM3_IRQHandler(void) { if(TIM3->SR&0X0001)//溢出中斷 { } TIM3->SR&=~(1<<0);//清除中斷標(biāo)志位 } /************************************************************************** 函數(shù)功能:TIM2中斷服務(wù)函數(shù) 入口參數(shù):無(wú) 返回 值:無(wú) **************************************************************************/ void TIM2_IRQHandler(void) { if(TIM2->SR&0X0001)//溢出中斷 { } TIM2->SR&=~(1<<0);//清除中斷標(biāo)志位 }?
?
1.2.5 串口通訊
小車采用有線的串口與電腦通訊,來(lái)模擬驅(qū)動(dòng)板通過(guò)串口與工控機(jī)NANO板完成信息交互,使用STM32F103串口1實(shí)現(xiàn)數(shù)據(jù)的傳輸。
對(duì)應(yīng)輸出腳為 TXD-PA10,RXD-PA9,編寫中動(dòng)板通過(guò)審口與工控機(jī),NANO板完成信息交互。
通過(guò)輪詢的方式將驅(qū)動(dòng)板采集到的傳感器數(shù)據(jù)等發(fā)送給工控機(jī),通過(guò)中斷的方式接收工控機(jī)發(fā)來(lái)的串口控制量從而完成小車的運(yùn)動(dòng)控制。
通過(guò)CH340G芯片完成電平轉(zhuǎn)換,CH340是一人USB總線的轉(zhuǎn)換芯片,實(shí)現(xiàn) USB轉(zhuǎn)由口,CH340G州片中的 TXD和 RXD輕舟機(jī)器人學(xué)習(xí)教程5。
STM32F103的USART1RXD和USART1TXD相連接。
首先配置口參數(shù)及中斷優(yōu)先級(jí),然后編寫發(fā)送函數(shù),最后使能接收中斷。
具體定義見(jiàn)附件2中usart.c,功能實(shí)現(xiàn)見(jiàn)DataScope DP.c中USART TX和jiexi函數(shù)。
usart.c
?
?
#include "usart.h" ? ? //加入以下代碼,支持printf函數(shù),而不需要選擇use MicroLIB ? ? #if 1 #pragma import(__use_no_semihosting) ? ? ? ? ? ? //標(biāo)準(zhǔn)庫(kù)需要的支持函數(shù) ? ? ? ? ? ? ? ? struct __FILE { ?int handle; ?/* Whatever you require here. If the only file you are using is */ ?/* standard output using printf() for debugging, no file handling */ ?/* is required. */ }; /* FILE is typedef’ d in stdio.h. */ FILE __stdout; ? ? ? //定義_sys_exit()以避免使用半主機(jī)模式 ? ? _sys_exit(int x) { ?x = x; } //重定義fputc函數(shù) int fputc(int ch, FILE *f) { ? ? ? ?while((USART3->SR&0X40)==0); ?USART3->DR = (u8) ch; ? ? ? ?return ch; } #endif u8 Usart3_Receive=0X5A; void usart1_init(u32 bound) { ?//GPIO端口設(shè)置 ?GPIO_InitTypeDef GPIO_InitStructure; ?USART_InitTypeDef USART_InitStructure; ?NVIC_InitTypeDef NVIC_InitStructure; ?RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1|RCC_APB2Periph_GPIOA, ENABLE); ?//使能USART1,GPIOA時(shí)鐘 ? ?//USART1_TX ? GPIOA.9 ?GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //PA.9 ?GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; ?GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; ?//復(fù)用推挽輸出 ?GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.9 ? ?//USART1_RX ? ?GPIOA.10初始化 ?GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;//PA10 ?GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空輸入 ?GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.10 ? ? //USART 初始化設(shè)置 //UsartNVIC 配置 ?NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; ?NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0 ;//搶占優(yōu)先級(jí) ?NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; ? ?//子優(yōu)先級(jí) ?NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; ? ? ?//IRQ通道使能 ?NVIC_Init(&NVIC_InitStructure); ?//根據(jù)指定的參數(shù)初始化VIC寄存器 ? ?USART_InitStructure.USART_BaudRate = bound;//串口波特率 ?USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字長(zhǎng)為8位數(shù)據(jù)格式 ?USART_InitStructure.USART_StopBits = USART_StopBits_1;//一個(gè)停止位 ?USART_InitStructure.USART_Parity = USART_Parity_No;//無(wú)奇偶校驗(yàn)位 ?USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//無(wú)硬件數(shù)據(jù)流控制 ?USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; ?//收發(fā)模式 ?USART_Init(USART1, &USART_InitStructure); //初始化串口1 ?USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//開(kāi)啟串口接受中斷 ?USART_Cmd(USART1, ENABLE); ? ? ? ? ? ? ? ? ? ?//使能串口1 } ? /************************************************************************** 函數(shù)功能:串口1接收中斷 入口參數(shù):無(wú) 返回 ?值:無(wú) **************************************************************************/ int USART1_IRQHandler(void) { ? ?if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收到數(shù)據(jù) ?{ ? ? ? ? ? ? ? ? ? ?u8 temp; ? ? ? ?static u8 count,last_data,last_last_data,Usart_ON_Count; ? ? ? ?if(Usart_ON_Flag==0) ? ? ? ?{ ? ? ? ? ?if(++Usart_ON_Count>10)Usart_ON_Flag=1; ? ? ? ?} ? ? ? ?temp=USART1->DR; ? ? ? ? if(Usart_Flag==0) ? ? ? ? ?{ ? ? ? ? ? ?if(last_data==0x5a&&last_last_data==0xa5) ? ? ? ? ? ? ?Usart_Flag=1,count=0; ? ? ? ? ? ?} ? ? ? ? if(Usart_Flag==1) ? ? ? ? ?{ ? ? ? ? ? ? ?Urxbuf[count]=temp; ? ? ? ? ? ? ? ?count++; ? ? ? ? ? ? ? ? ? ? ? ? ? ?if(count==8)Usart_Flag=0; ? ? ? ? ?} ? ? ? ? ?last_last_data=last_data; ? ? ? ? ?last_data=temp; ? } return 0; ? } // /**************************實(shí)現(xiàn)函數(shù)********************************************** *功 ? ?能: ? ?usart3發(fā)送一個(gè)字節(jié) *********************************************************************************/ void usart3_send(u8 data) { ?USART3->DR = data; ?while((USART3->SR&0x40)==0); ? } /************************************************************************** 函數(shù)功能:串口3初始化 入口參數(shù):bound:波特率 返回 ?值:無(wú) **************************************************************************/ void usart3_init(u32 bound) { ? ? ? ?//GPIO端口設(shè)置 ?GPIO_InitTypeDef GPIO_InitStructure; ?USART_InitTypeDef USART_InitStructure; ?NVIC_InitTypeDef NVIC_InitStructure; ? ?RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);// 需要使能AFIO時(shí)鐘 ?RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); ?//使能GPIO時(shí)鐘 ?RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); ?//使能USART時(shí)鐘 ? ?GPIO_PinRemapConfig(GPIO_PartialRemap_USART3, ENABLE);//引腳重映射 ?//USART_TX ? ?GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; //C10 ?GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; ?GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; ?//復(fù)用推挽輸出 ?GPIO_Init(GPIOC, &GPIO_InitStructure); ? ?//USART_RX ? ? ?GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;//PC11 ?GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空輸入 ?GPIO_Init(GPIOC, &GPIO_InitStructure); ?//UsartNVIC 配置 ?NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn; ?NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0 ;//搶占優(yōu)先級(jí) ?NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; ? ?//子優(yōu)先級(jí) ?NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; ? ? ?//IRQ通道使能 ?NVIC_Init(&NVIC_InitStructure); ?//根據(jù)指定的參數(shù)初始化VIC寄存器 ? //USART 初始化設(shè)置 ?USART_InitStructure.USART_BaudRate = bound;//串口波特率 ?USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字長(zhǎng)為8位數(shù)據(jù)格式 ?USART_InitStructure.USART_StopBits = USART_StopBits_1;//一個(gè)停止位 ?USART_InitStructure.USART_Parity = USART_Parity_No;//無(wú)奇偶校驗(yàn)位 ?USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//無(wú)硬件數(shù)據(jù)流控制 ?USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; ?//收發(fā)模式 ?USART_Init(USART3, &USART_InitStructure); ? ? //初始化串口3 ?USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//開(kāi)啟串口接受中斷 ?USART_Cmd(USART3, ENABLE); ? ? ? ? ? ? ? ? ? ?//使能串口3 } /************************************************************************** 函數(shù)功能:串口3接收中斷 入口參數(shù):無(wú) 返回 ?值:無(wú) **************************************************************************/ void USART3_IRQHandler(void) { ? ?if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) //接收到數(shù)據(jù) ?{ ? ? ?static u8 Flag_PID,i,j,Receive[50]; ? ? ?static float Data; ? ? ?Usart3_Receive=USART3->DR; ? ? ?if(Usart3_Receive>=0x41&&Usart3_Receive<=0x48) ? ? ? ?Flag_Direction=Usart3_Receive-0x40; ? ? ?else ? ? ? ? ?Flag_Direction=0; ? ? ? ? ? ? ? ?//以下是與APP調(diào)試界面通訊 ? ?if(Usart3_Receive==0x7B) Flag_PID=1; ? //APP參數(shù)指令起始位 ? ?if(Usart3_Receive==0x7D) Flag_PID=2; ? //APP參數(shù)指令停止位 ? ? if(Flag_PID==1) ?//采集數(shù)據(jù) ? ? { ? ? ?Receive[i]=Usart3_Receive; ? ? ?i++; ? ? } ? ? if(Flag_PID==2) ?//分析數(shù)據(jù) ? ? { ? ? ? ? ? if(Receive[3]==0x50) ? ?PID_Send=1; ? ? ? ? ? else ?if(Receive[1]!=0x23) ? ? ? ? ? { ? ? ? ? ? ? ? ? ? ? ? ? ? ?for(j=i;j>=4;j--) ? ? ? ? ? ?{ ? ? ? ? ? ? ?Data+=(Receive[j-1]-48)*pow(10,i-j); ? ? ? ? ? ?} ? ? ? ? ? ?switch(Receive[1]) ? ? ? ? ? ? { ? ? ? ? ? ? ? case 0x30: ?RC_Velocity=Data;break; ? ? ? ? ? ? ? case 0x31: ?Velocity_KP=Data;break; ? ? ? ? ? ? ? case 0x32: ?Velocity_KI=Data;break; ? ? ? ? ? ? ? case 0x33: ?break; ? ? ? ? ? ? ? case 0x34: ?break; ? ? ? ? ? ? ? case 0x35: ?break; ? ? ? ? ? ? ? case 0x36: ?break; ? ? ? ? ? ? ? case 0x37: ?break; //預(yù)留 ? ? ? ? ? ? ? case 0x38: ?break; //預(yù)留 ? ? ? ? ? ? } ? ? ? ? ? } ? ? ? ? ? ? ? ? ? Flag_PID=0;//相關(guān)標(biāo)志位清零 ? ? ? ? ? i=0; ? ? ? ? ? j=0; ? ? ? ? ? Data=0; ? ? ? ? ? memset(Receive, 0, sizeof(u8)*50);//數(shù)組清零 ? ? } ? ? ? ? ? ?} ? ? ? ? ? ? ? ? ? ? ? ? }?
?
Datascope DP.c
?
?
#include "DataScope_DP.h" unsigned char DataScope_OutPut_Buffer[42] = {0}; ? ? //串口發(fā)送緩沖區(qū) int send_cnt = 0; static u8 Send_rasberry[60]; int re_Encoder_Left,re_Encoder_Right; int Distance_A,Distance_B,Distance_C,Distance_D; u8 Urxbuf[8],Usart_Flag,x=0; short accelX,accelY,accelZ,gyroX,gyroY,gyroZ,magX,magY,magZ; void USART_TX(void) { ?Send_rasberry[0] = 0xA5; // 數(shù)據(jù)頭,固定值 ?Send_rasberry[1] = 0x5A; // 數(shù)據(jù)頭,固定值 ?Send_rasberry[2] = 0x33; // 發(fā)送數(shù)據(jù)的長(zhǎng)度 ?re_Encoder_Left = -Encoder_Left; ?re_Encoder_Right = -Encoder_Right; ?for(send_cnt=0; send_cnt<4; send_cnt++) //左編碼器增量值 ?{ ?Send_rasberry[3+send_cnt] = ((unsigned char *)&re_Encoder_Left)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<4; send_cnt++) //右編碼器增量值 ?{ ?Send_rasberry[7+send_cnt] = ((unsigned char *)&re_Encoder_Right)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<4; send_cnt++) //電池電壓采樣 ?{ ? ?Send_rasberry[11+send_cnt] = ((unsigned char *)&Voltage)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<2; send_cnt++) // X 軸加速度計(jì)值 ?{ ?Send_rasberry[15+send_cnt] = ((unsigned char *)&accelX)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<2; send_cnt++) // Y 軸加速度計(jì)值 ?{ ?Send_rasberry[17+send_cnt] = ((unsigned char *)&accelY)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<2; send_cnt++) // Y 軸加速度計(jì)值 ?{ ?Send_rasberry[19+send_cnt] = ((unsigned char *)&accelZ)[send_cnt]; ?} ?//send gyro X Y Z ?for(send_cnt=0; send_cnt<2; send_cnt++) // X 軸角速度值 ?{ ?Send_rasberry[21+send_cnt] = ((unsigned char *)&gyroX)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<2; send_cnt++) // Y 軸角速度值 ?{ ?Send_rasberry[23+send_cnt] = ((unsigned char *)&gyroY)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<2; send_cnt++) // Z 軸角速度值 ?{ ?Send_rasberry[25+send_cnt] = ((unsigned char *)&gyroZ)[send_cnt]; ?} ?//send MAG X Y Z ?for(send_cnt=0; send_cnt<2; send_cnt++) // X 軸磁力計(jì)值 ?{ ?Send_rasberry[27+send_cnt] = ((unsigned char *)&magX)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<2; send_cnt++) // Y 軸磁力計(jì)值 ?{ ?Send_rasberry[29+send_cnt] = ((unsigned char *)&magY)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<2; send_cnt++) // Z 軸磁力計(jì)值 ?{ ?Send_rasberry[31+send_cnt] = ((unsigned char *)&magZ)[send_cnt]; ?} ? //send ultrasonic A B C D ?for(send_cnt=0; send_cnt<4; send_cnt++) // 超測(cè)量距離值 A ?{ ?Send_rasberry[33+send_cnt] = ((unsigned char *)&Distance_A)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<4; send_cnt++) // 超測(cè)量距離值 B ?{ ?Send_rasberry[37+send_cnt] = ((unsigned char *)&Distance_B)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<4; send_cnt++) // 超測(cè)量距離值 C ?{ ?Send_rasberry[41+send_cnt] = ((unsigned char *)&Distance_C)[send_cnt]; ?} ?for(send_cnt=0; send_cnt<4; send_cnt++) // 超測(cè)量距離值 D ?{ ?Send_rasberry[45+send_cnt] = ((unsigned char *)&Distance_D)[send_cnt]; ?} ?Send_rasberry[50]=x; ?x++; ?//send Send_rasberry ? UartASendStr(Send_rasberry,51); ?memset(Send_rasberry, 0, sizeof(u8)*51); //數(shù)組清零 } void UartASendStr (u8 *pucStr, u8 ulNum) { ?u8 i; ?for(i = 0;i?
?
1.2.6 PS2通訊
為了方便調(diào)試,設(shè)計(jì)了PS2手柄控制。
PS2采用的是SPI通信協(xié)議如圖 16,SPI是串行外設(shè)接口的縮寫,是一種高速的、全雙工、同步的通信總線,并且在芯片的管腳上只占用四根線(D、DO、CS、CLK),節(jié)約了芯片的管腳,同時(shí)為PCB的布局上節(jié)省空間。
具體定義見(jiàn)附件2中pstwo.c,功能實(shí)現(xiàn)風(fēng)control.c中Get RC函數(shù)。
pstwo.c
?
?
#include "pstwo.h" #define DELAY_TIME ?delay_us(5); u16 Handkey; ?// 按鍵值讀取,零時(shí)存儲(chǔ)。 u8 Comd[2]={0x01,0x42}; ?//開(kāi)始命令。請(qǐng)求數(shù)據(jù) u8 Data[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; //數(shù)據(jù)存儲(chǔ)數(shù)組 u16 MASK[]={ ? ?PSB_SELECT, ? ?PSB_L3, ? ?PSB_R3 , ? ?PSB_START, ? ?PSB_PAD_UP, ? ?PSB_PAD_RIGHT, ? ?PSB_PAD_DOWN, ? ?PSB_PAD_LEFT, ? ?PSB_L2, ? ?PSB_R2, ? ?PSB_L1, ? ?PSB_R1 , ? ?PSB_GREEN, ? ?PSB_RED, ? ?PSB_BLUE, ? ?PSB_PINK ?}; ?//按鍵值與按鍵明 //c2改成b15 ?c1改成b14 ?c3改成c13 a4改成a12 void PS2_Init(void) { ?GPIO_InitTypeDef GPIO_InitStructure; ?RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC, ENABLE); //使能端口時(shí)鐘 ?GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13; ? ? ? ? ? ? ?//端口配置 ?GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; ? ? ? ? //上拉輸入 ?GPIO_Init(GPIOC, &GPIO_InitStructure); ? ? ? ? ? ? ? ?//根據(jù)設(shè)定參數(shù)初始化GPIO ? ?GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14|GPIO_Pin_15; ?//端口配置 ?GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; ? ? ?//推挽輸出 ?GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; ? ? //50M ?GPIO_Init(GPIOB, &GPIO_InitStructure); ? ? ? ? ? ? ? ?//根據(jù)設(shè)定參數(shù)初始化GPIOB ? ?GPIO_InitStructure.GPIO_Pin =GPIO_Pin_12; ? ? ? ? ? ? ?//端口配置 ?GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; ? ? ?//推挽輸出 ?GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; ? ? //50M ?GPIO_Init(GPIOA, &GPIO_InitStructure); ? ? ? ? ? ? ? ?//根據(jù)設(shè)定參數(shù)初始化GPIOA ? ? } //向手柄發(fā)送命令 void PS2_Cmd(u8 CMD) { ?volatile u16 ref=0x01; ?Data[1] = 0; ?for(ref=0x01;ref<0x0100;ref<<=1) ?{ ? ?if(ref&CMD) ? ?{ ? ? ?DO_H; ? ? ? ? ? ? ? ? ? //輸出一位控制位 ? ?} ? ?else DO_L; ? ?CLK_H; ? ? ? ? ? ? ? ? ? ? ? ?//時(shí)鐘拉高 ? ?DELAY_TIME; ? ?CLK_L; ? ?DELAY_TIME; ? ?CLK_H; ? ?if(DI) ? ? ?Data[1] = ref|Data[1]; ?} ?delay_us(16); } //判斷是否為紅燈模式,0x41=模擬綠燈,0x73=模擬紅燈 //返回值;0,紅燈模式 // ? ? ?其他,其他模式 u8 PS2_RedLight(void) { ?CS_L; ?PS2_Cmd(Comd[0]); ?//開(kāi)始命令 ?PS2_Cmd(Comd[1]); ?//請(qǐng)求數(shù)據(jù) ?CS_H; ?if( Data[1] == 0X73) ? return 0 ; ?else return 1; } //讀取手柄數(shù)據(jù) void PS2_ReadData(void) { ?volatile u8 byte=0; ?volatile u16 ref=0x01; ?CS_L; ?PS2_Cmd(Comd[0]); ?//開(kāi)始命令 ?PS2_Cmd(Comd[1]); ?//請(qǐng)求數(shù)據(jù) ?for(byte=2;byte<9;byte++) ? ? ? ? ?//開(kāi)始接受數(shù)據(jù) ?{ ? ?for(ref=0x01;ref<0x100;ref<<=1) ? ?{ ? ? ?CLK_H; ? ? ?DELAY_TIME; ? ? ?CLK_L; ? ? ?DELAY_TIME; ? ? ?CLK_H; ? ? ? ? ?if(DI) ? ? ? ? ?Data[byte] = ref|Data[byte]; ? ?} ? ? ? ?delay_us(16); ?} ?CS_H; } //對(duì)讀出來(lái)的PS2的數(shù)據(jù)進(jìn)行處理,只處理按鍵部分 ? //只有一個(gè)按鍵按下時(shí)按下為0, 未按下為1 u8 PS2_DataKey() { ?u8 index; ?PS2_ClearData(); ?PS2_ReadData(); ?Handkey=(Data[4]<<8)|Data[3]; ? ? //這是16個(gè)按鍵 ?按下為0, 未按下為1 ?for(index=0;index<16;index++) ?{ ? ? ? ? ?if((Handkey&(1<<(MASK[index]-1)))==0) ? ?return index+1; ?} ?return 0; ? ? ? ? ?//沒(méi)有任何按鍵按下 } //得到一個(gè)搖桿的模擬量 ? 范圍0~256 u8 PS2_AnologData(u8 button) { ?return Data[button]; } //清除數(shù)據(jù)緩沖區(qū) void PS2_ClearData() { ?u8 a; ?for(a=0;a<9;a++) ? ?Data[a]=0x00; } /****************************************************** Function: ? ?void PS2_Vibration(u8 motor1, u8 motor2) Description: 手柄震動(dòng)函數(shù), Calls: ? ? void PS2_Cmd(u8 CMD); Input: motor1:右側(cè)小震動(dòng)電機(jī) 0x00關(guān),其他開(kāi) ? ? motor2:左側(cè)大震動(dòng)電機(jī) 0x40~0xFF 電機(jī)開(kāi),值越大 震動(dòng)越大 ******************************************************/ void PS2_Vibration(u8 motor1, u8 motor2) { ?CS_L; ?delay_us(16); ? ?PS2_Cmd(0x01); ?//開(kāi)始命令 ?PS2_Cmd(0x42); ?//請(qǐng)求數(shù)據(jù) ?PS2_Cmd(0X00); ?PS2_Cmd(motor1); ?PS2_Cmd(motor2); ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?CS_H; ?delay_us(16); ? } //short poll void PS2_ShortPoll(void) { ?CS_L; ?delay_us(16); ?PS2_Cmd(0x01); ? ?PS2_Cmd(0x42); ? ?PS2_Cmd(0X00); ?PS2_Cmd(0x00); ?PS2_Cmd(0x00); ?CS_H; ?delay_us(16); ? } //進(jìn)入配置 void PS2_EnterConfing(void) { ? ?CS_L; ?delay_us(16); ?PS2_Cmd(0x01); ? ?PS2_Cmd(0x43); ? ?PS2_Cmd(0X00); ?PS2_Cmd(0x01); ?PS2_Cmd(0x00); ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?CS_H; ?delay_us(16); } //發(fā)送模式設(shè)置 void PS2_TurnOnAnalogMode(void) { ?CS_L; ?PS2_Cmd(0x01); ? ?PS2_Cmd(0x44); ? ?PS2_Cmd(0X00); ?PS2_Cmd(0x01); //analog=0x01;digital=0x00 ?軟件設(shè)置發(fā)送模式 ?PS2_Cmd(0x03); //Ox03鎖存設(shè)置,即不可通過(guò)按鍵“MODE”設(shè)置模式。 ? ? ? ? ? //0xEE不鎖存軟件設(shè)置,可通過(guò)按鍵“MODE”設(shè)置模式。 ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?PS2_Cmd(0X00); ?CS_H; ?delay_us(16); } //振動(dòng)設(shè)置 void PS2_VibrationMode(void) { ?CS_L; ?delay_us(16); ?PS2_Cmd(0x01); ? ?PS2_Cmd(0x4D); ? ?PS2_Cmd(0X00); ?PS2_Cmd(0x00); ?PS2_Cmd(0X01); ?CS_H; ?delay_us(16); ? } //完成并保存配置 void PS2_ExitConfing(void) { ? ?CS_L; ?delay_us(16); ?PS2_Cmd(0x01); ? ?PS2_Cmd(0x43); ? ?PS2_Cmd(0X00); ?PS2_Cmd(0x00); ?PS2_Cmd(0x5A); ?PS2_Cmd(0x5A); ?PS2_Cmd(0x5A); ?PS2_Cmd(0x5A); ?PS2_Cmd(0x5A); ?CS_H; ?delay_us(16); } //手柄配置初始化 void PS2_SetInit(void) { ?PS2_ShortPoll(); ?PS2_ShortPoll(); ?PS2_ShortPoll(); ?PS2_EnterConfing(); ? ?//進(jìn)入配置模式 ?PS2_TurnOnAnalogMode(); ?//“紅綠燈”配置模式,并選擇是否保存 ?//PS2_VibrationMode(); ?//開(kāi)啟震動(dòng)模式 ?PS2_ExitConfing(); ? ?//完成并保存配置 }?
?
二、設(shè)計(jì)思路和流程
小車通過(guò)前期的仿真設(shè)計(jì),結(jié)合實(shí)驗(yàn)室材料情況,搭建的小車實(shí)物。
為了方演示串口通訊和PS二手柄控制,對(duì)此設(shè)計(jì)了控制程序流程圖如圖 17,來(lái)引導(dǎo)小車控制程序的設(shè)計(jì)。
圖 17 控制程序流程圖
控制程序流程圖理解如下,首先初始化小車,把電機(jī)舵機(jī)進(jìn)行歸零處理,定義對(duì)應(yīng)的傳感器,初始化OLED 屏幕。顯示屏?xí)崾灸J竭x怪,模式一是串口控制模式。
點(diǎn)擊確認(rèn)后,小車會(huì)自動(dòng)一傳小車狀杰信息數(shù)提給上位機(jī),一位機(jī)可以發(fā)送指令小車接收后會(huì)執(zhí)行對(duì)應(yīng)的運(yùn)動(dòng)控制,會(huì)進(jìn)行一次判斷小車是否異常。
如果正常會(huì)進(jìn)行二次判斷小車是否需要結(jié)束運(yùn)動(dòng),那么小車與上位機(jī)進(jìn)行正常的人機(jī)交互。
通討串口助手和單片機(jī)通訊并下發(fā)電機(jī)運(yùn)動(dòng)指令,片機(jī)接收串口助手的指令并驅(qū)動(dòng)電機(jī)的正反轉(zhuǎn)和差速控制。
單片機(jī)接收串口助手的指令控制能機(jī)和電機(jī)基于阿克曼運(yùn)動(dòng)算法的轉(zhuǎn)動(dòng)。
如果是最后會(huì)初始化小車歸零結(jié)束程序。模式二是PS2控制模式,會(huì)通過(guò)顯示屏顯示小車的運(yùn)動(dòng)數(shù)據(jù),用戶可以通過(guò)PS二手柄進(jìn)行遙控小車。
同樣會(huì)進(jìn)行二次判斷,當(dāng)用戶發(fā)出結(jié)束小車運(yùn)動(dòng)指令時(shí),小車會(huì)歸零,最終結(jié)束程序。
小車完整程序見(jiàn)附件2中輕舟驅(qū)動(dòng)板demo-OLED8.0
2.1 串口控制模式
串口控制和PS2控制如圖 18。在串口控制模式中,首先調(diào)用函數(shù),接下來(lái)顯示屏打開(kāi),讀取左右編碼器,延時(shí)消除不穩(wěn)定。
接下來(lái)進(jìn)行數(shù)據(jù)解析、卡爾曼算法、限制pwm幅、輸出對(duì)應(yīng)的電機(jī)和機(jī)。
最后電機(jī)電壓顯示和單機(jī)單雙擊可以改變小車的運(yùn)行狀態(tài),以此循環(huán)往復(fù),實(shí)現(xiàn)人機(jī)交互。具體串口協(xié)議見(jiàn)附件3中阿克曼運(yùn)動(dòng)串口協(xié)議。
圖 18 串口和PS2控制
2.2 PS2控制模式
PS2控制模式中與串口控制模式有不同的在于PS2數(shù)據(jù)解析,其他與串口通訊一致。
三、設(shè)計(jì)效果與呈現(xiàn)
本節(jié)綜合前面阿克曼小車的仿直設(shè)計(jì),實(shí)物設(shè)計(jì)和程席設(shè)計(jì)。
最后到了實(shí)驗(yàn)設(shè)計(jì)效果展示的環(huán)節(jié),本環(huán)節(jié)由由口控制演示和PS2演示控制兩部分組成。
具體的演示見(jiàn)附件4的演示視頻。
3.1?串口控制演示
通過(guò)安卓數(shù)據(jù)線連接小車串口1.。本次審口控制演示采用的是友善串口助手如圖 19。
串口設(shè)置端口連接com8,波特率115200,數(shù)據(jù)為位,無(wú)校驗(yàn)位,一位停止位,無(wú)流控,接收設(shè)置采用hex自動(dòng)換行顯示發(fā)送和顯示時(shí)間,接收采用hex。串口收發(fā)控制如圖 20圖 21
圖 19串口設(shè)置
圖 20 左轉(zhuǎn)控制
圖 21 停止控制
3.2 PS2控制演示
在PS2控制中,首先打開(kāi)手柄的開(kāi)關(guān)電源,然后按一下stat開(kāi)始控制小車。
通過(guò)搖動(dòng)左右搖桿實(shí)現(xiàn)一個(gè)左右前進(jìn)控制,其中左搖桿前進(jìn)是油門兒,右搖桿是控制機(jī)轉(zhuǎn)向。
PS2控制操作如圖 22。
圖 22 PS2控制操作
演示視頻:
編輯:黃飛
?
評(píng)論
查看更多