一、主要內(nèi)容

結(jié)合阿克曼運(yùn)動(dòng)需求，本團(tuán)隊(duì)設(shè)計(jì)了阿克曼運(yùn)動(dòng)系統(tǒng)框圖如圖

主要包括阿克曼小車(chē)仿真設(shè)計(jì)和阿克曼小車(chē)實(shí)物實(shí)現(xiàn)。

圖 1 阿克曼運(yùn)動(dòng)系統(tǒng)框圖

1.1 阿克曼小車(chē)仿真設(shè)計(jì)

本節(jié)先介紹阿克曼小車(chē)模型，小車(chē)仿真設(shè)計(jì)包括SolidWorks和gazebo三維建模。

1.1.1 阿克曼小車(chē)

阿克曼小車(chē)是一款經(jīng)典的車(chē)模，小車(chē)模型后輪是通過(guò)電機(jī)直驅(qū)，前輪通過(guò)舵機(jī)控制前輪轉(zhuǎn)向角，前輪部分則是在模型中添加一個(gè)豎直的關(guān)節(jié)，使前輪能?chē)@此關(guān)節(jié)轉(zhuǎn)動(dòng)

1.1.2 SolidWorks三維模型

采用SolidWorks (2016版)設(shè)計(jì)軟件搭建阿克曼小車(chē)三維模型如圖 2圖 3圖 4，主體分為底板、電池、控制器、帶編碼電機(jī)的后輪、激光雷達(dá)、深度攝像頭、阿克曼前輪轉(zhuǎn)向7個(gè)部分如圖 5。

各部分零件都定義了材料屬性，選擇取小車(chē)底盤(pán)中心為模型的原點(diǎn)，配置了小車(chē)JRDF文件見(jiàn)附件3。

目前可以導(dǎo)入到gazebo環(huán)境中，可以實(shí)現(xiàn)小車(chē)的前進(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)化小車(chē)的運(yùn)動(dòng)，直接調(diào)入用阿克曼模型，簡(jiǎn)化小車(chē)的前后輪運(yùn)動(dòng)關(guān)系，搭配了里程計(jì)和攝像頭，可以實(shí)現(xiàn)小車(chē)的轉(zhuǎn)向、直行和后退。

小車(chē)分別導(dǎo)入到gazebo和rviz效果分別如圖 7圖 8建模過(guò)程見(jiàn)附件1文檔

圖 7 加載到gazebo

圖8 加載到rvzi

1.2 阿克曼小車(chē)實(shí)物實(shí)現(xiàn)

小車(chē)底層搭載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)小車(chē)的實(shí)物制作如圖 9。

圖9小車(chē)實(shí)物

1.2.1控制器

小車(chē)控制器采用意法半導(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位看門(mén)狗定時(shí)器、SysTick定時(shí)器、SP112C、2個(gè)USART、USB2.0全速接口、CAN2.0B激活、2個(gè)12位10通道D轉(zhuǎn)換器。

快速 /0 端口如圖 10整體資源滿(mǎn)足小車(chē)求，10使用情況詳細(xì)說(shuō)明，STM32核心板和底板原理圖見(jiàn)附件1。

圖 10 STM32F103引腳定義圖

1.2.2 阿克曼運(yùn)動(dòng)算法

阿克曼轉(zhuǎn)向是一種現(xiàn)代汽車(chē)的轉(zhuǎn)向方式，在汽車(chē)轉(zhuǎn)彎的時(shí)候，內(nèi)外輪轉(zhuǎn)過(guò)的角度不一樣，內(nèi)側(cè)輪胎轉(zhuǎn)彎半徑小于外側(cè)輪胎。理想的阿克曼轉(zhuǎn)向如圖 11，而本車(chē)模型采用反向的阿克曼模型。

圖 11 理想的阿克曼轉(zhuǎn)向

根據(jù)阿克曼轉(zhuǎn)向幾何設(shè)計(jì)轉(zhuǎn)向機(jī)構(gòu)，在車(chē)輛沿著彎道轉(zhuǎn)彎時(shí)，利用四連桿的相等曲柄，可以使內(nèi)側(cè)輪的轉(zhuǎn)向角比外側(cè)輪大大約 2~4度，使四個(gè)輪子路徑的圓心大致上交會(huì)于后軸的延長(zhǎng)線(xiàn)上瞬時(shí)轉(zhuǎn)向中心，從而讓車(chē)輛可以順暢的轉(zhuǎn)彎。

阿克曼核心公式如下;

式中:B一汽車(chē)前外輪轉(zhuǎn)角，a 一汽車(chē)前內(nèi)輪轉(zhuǎn)角，K一兩主銷(xiāo)中心距，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ù)功能：小車(chē)運(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); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //小車(chē)運(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); ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //小車(chē)運(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) 單擊雙擊可以改變小車(chē)運(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滿(mǎn)幅是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ù)功能：按鍵修改小車(chē)運(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;//單擊控制小車(chē)的啟停
 ?//if(tmp==2)Flag_Show=!Flag_Show;//雙擊控制小車(chē)的顯示狀態(tài)
 ?tmp2=Long_Press(); ?//長(zhǎng)按 ? ? ? ?
 ?if(tmp2==1)Flag_Show=!Flag_Show;//控制小車(chē)的顯示狀態(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)代表上一次的偏差 ?以此類(lèi)推 
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ì)小車(chē)進(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)盤(pán)位置的 0-180度如圖 13，呈線(xiàn)性變化，也就是說(shuō)給它提供一定的脈寬。

	它的輸出軸就會(huì)保持在一定對(duì)應(yīng)角度上，無(wú)論外界力矩怎么改變，直到給它提供另外寬度的脈沖信號(hào)，它才會(huì)改變輸出角度到新的位置上。

	

	圖 13 舵機(jī)輸出轉(zhuǎn)角與輸入信號(hào)脈沖寬度的關(guān)系

	采用DSServo達(dá)盛金屬舵機(jī)如圖 14，型號(hào)為 DS3230，適用于航模、車(chē)模、船模及機(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必須要這句話(huà)才能輸出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è)接線(xiàn)端子，電機(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 串口通訊

	小車(chē)采用有線(xiàn)的串口與電腦通訊,來(lái)模擬驅(qū)動(dòng)板通過(guò)串口與工控機(jī)NANO板完成信息交互，使用STM32F103串口1實(shí)現(xiàn)數(shù)據(jù)的傳輸。

	對(duì)應(yīng)輸出腳為 TXD-PA10，RXD-PA9，編寫(xiě)中動(dòng)板通過(guò)審口與工控機(jī),NANO板完成信息交互。

	通過(guò)輪詢(xún)的方式將驅(qū)動(dòng)板采集到的傳感器數(shù)據(jù)等發(fā)送給工控機(jī)，通過(guò)中斷的方式接收工控機(jī)發(fā)來(lái)的串口控制量從而完成小車(chē)的運(yùn)動(dòng)控制。

	通過(guò)CH340G芯片完成電平轉(zhuǎn)換，CH340是一人USB總線(xiàn)的轉(zhuǎn)換芯片，實(shí)現(xiàn) USB轉(zhuǎn)由口，CH340G州片中的 TXD和 RXD輕舟機(jī)器人學(xué)習(xí)教程5。

	STM32F103的USART1RXD和USART1TXD相連接。

	首先配置口參數(shù)及中斷優(yōu)先級(jí)，然后編寫(xiě)發(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è)接口的縮寫(xiě)，是一種高速的、全雙工、同步的通信總線(xiàn)，并且在芯片的管腳上只占用四根線(xiàn)(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ì)思路和流程

	小車(chē)通過(guò)前期的仿真設(shè)計(jì)，結(jié)合實(shí)驗(yàn)室材料情況，搭建的小車(chē)實(shí)物。

	為了方演示串口通訊和PS二手柄控制，對(duì)此設(shè)計(jì)了控制程序流程圖如圖 17，來(lái)引導(dǎo)小車(chē)控制程序的設(shè)計(jì)。

	

	圖 17 控制程序流程圖

	控制程序流程圖理解如下，首先初始化小車(chē)，把電機(jī)舵機(jī)進(jìn)行歸零處理，定義對(duì)應(yīng)的傳感器，初始化OLED 屏幕。顯示屏?xí)崾灸Ｊ竭x怪，模式一是串口控制模式。

	點(diǎn)擊確認(rèn)后，小車(chē)會(huì)自動(dòng)一傳小車(chē)狀杰信息數(shù)提給上位機(jī)，一位機(jī)可以發(fā)送指令小車(chē)接收后會(huì)執(zhí)行對(duì)應(yīng)的運(yùn)動(dòng)控制，會(huì)進(jìn)行一次判斷小車(chē)是否異常。

	如果正常會(huì)進(jìn)行二次判斷小車(chē)是否需要結(jié)束運(yùn)動(dòng)，那么小車(chē)與上位機(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ì)初始化小車(chē)歸零結(jié)束程序。模式二是PS2控制模式，會(huì)通過(guò)顯示屏顯示小車(chē)的運(yùn)動(dòng)數(shù)據(jù)，用戶(hù)可以通過(guò)PS二手柄進(jìn)行遙控小車(chē)。

	同樣會(huì)進(jìn)行二次判斷，當(dāng)用戶(hù)發(fā)出結(jié)束小車(chē)運(yùn)動(dòng)指令時(shí)，小車(chē)會(huì)歸零，最終結(jié)束程序。

	小車(chē)完整程序見(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ī)單雙擊可以改變小車(chē)的運(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é)綜合前面阿克曼小車(chē)的仿直設(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ù)線(xiàn)連接小車(chē)串口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)始控制小車(chē)。

	通過(guò)搖動(dòng)左右搖桿實(shí)現(xiàn)一個(gè)左右前進(jìn)控制，其中左搖桿前進(jìn)是油門(mén)兒，右搖桿是控制機(jī)轉(zhuǎn)向。

	PS2控制操作如圖 22。

	

	圖 22 PS2控制操作

	演示視頻:

	

	編輯：黃飛

	?