基於ROS平臺的移動機器人-2-小車底盤控制
說明
本博文將介紹小車底盤控制的原理，如PID控制，控制程序的編寫等。
小車控制思想
- 控制電機轉動。電機的控制我們分爲兩部分，一部分爲電機轉動方向的控制，另一個爲電機轉速的控制。電機轉動的方向我們用兩個MCU引腳來控制，假如PIN_A=1，PIN_B=0 時，電機正轉；PIN_A=0，PIN_B=1 時，電機反轉；PIN_A=0，PIN_B=0 時，電機停止。電機速度的控制則需要一個PWM輸出引腳，我們通過控制輸出不同的PWM值來控制電機轉動的速度。
- PID控制。如果我們想控制小車以一米每秒的速度做直線，但由於地面的摩擦阻力的影響，會造成左右輪速度與我們想控制的速度不同，所以會走不直，這時我們就需要加入PID控制，PID控制的思想就是我實時的把輪子真正的速度採集回來和控制的速度對比，差則補，多則減。這樣基本就可以實現理想控制。詳細PID介紹查看。
- 小車轉彎控制。一般我們要是想控制小車以多少的速度前進或者後退，我們只需要PID控制兩個輪子的速度一致就可以基本做到。但如果要想控制小車以多少的角速度轉彎，我們需要做一定的計算，如圖所示：
推算過程這裏就不算了，我們可以得到左右輪速度和線速度角速度之間的關係如下：
通過以上的公式我們就可以控制小車的任意行走了。
程序結構
以下爲我的STM32工程主要文件
1.main.c 接收和發送串口數據，控制電機
/*********************************************** 說明 *****************************************************************
*
* 1.串口接收
* （1）內容：小車左右輪速度,單位:mm/s（所有數據都爲 float型，float型佔4字節）
* （2）格式：10字節 [右輪速度4字節][左輪速度4字節][結束符"\r\n"2字節]
*
* 2.串口發送
* （1）內容：里程計（x,y座標、線速度、角速度和方向角，單位依次爲：mm,mm,mm/s,rad/s,rad，所有數據都爲float型，float型佔4字節）
* （2）格式：21字節 [x座標4字節][y座標4字節][方向角4字節][線速度4字節][角速度4字節][結束符"\n"1字節]
*
************************************************************************************************************************/
#include "stm32f10x.h"
#include "stm32f10x_it.h"
#include "delay.h"
#include "nvic_conf.h"
#include "rcc_conf.h"
#include "usart.h"
#include "encoder.h"
#include "contact.h"
#include "gpio_conf.h"
#include "tim2_5_6.h"
#include "odometry.h"
#include "tim2_5_6.h"
#include "stdbool.h"
#include <stdio.h>
#include "string.h"
#include "math.h"
/*********************************************** 輸出 *****************************************************************/
char odometry_data[21]={0}; //發送給串口的里程計數據數組
float odometry_right=0,odometry_left=0;//串口得到的左右輪速度
/*********************************************** 輸入 *****************************************************************/
extern float position_x,position_y,oriention,velocity_linear,velocity_angular; //計算得到的里程計數值
extern u8 USART_RX_BUF[USART_REC_LEN]; //串口接收緩衝,最大USART_REC_LEN個字節.
extern u16 USART_RX_STA; //串口接收狀態標記
extern float Milemeter_L_Motor,Milemeter_R_Motor; //dt時間內的左右輪速度,用於里程計計算
/*********************************************** 變量 *****************************************************************/
u8 main_sta=0; //用作處理主函數各種if，去掉多餘的flag（1打印里程計）（2調用計算里程計數據函數）（3串口接收成功）（4串口接收失敗）
union recieveData //接收到的數據
{
float d; //左右輪速度
unsigned char data[4];
}leftdata,rightdata; //接收的左右輪數據
union odometry //里程計數據共用體
{
float odoemtry_float;
unsigned char odometry_char[4];
}x_data,y_data,theta_data,vel_linear,vel_angular; //要發佈的里程計數據，分別爲：X，Y方向移動的距離，當前角度，線速度，角速度
/****************************************************************************************************************/
int main(void)
{
u8 t=0;
u8 i=0,j=0,m=0;
RCC_Configuration(); //系統時鐘配置
NVIC_Configuration(); //中斷優先級配置
GPIO_Configuration(); //電機方向控制引腳配置
USART1_Config(); //串口1配置
TIM2_PWM_Init(); //PWM輸出初始化
ENC_Init(); //電機處理初始化
TIM5_Configuration(); //速度計算定時器初始化
TIM1_Configuration(); //里程計發佈定時器初始化
while (1)
{
if(main_sta&0x01)//執行發送里程計數據步驟
{
//里程計數據獲取
x_data.odoemtry_float=position_x;//單位mm
y_data.odoemtry_float=position_y;//單位mm
theta_data.odoemtry_float=oriention;//單位rad
vel_linear.odoemtry_float=velocity_linear;//單位mm/s
vel_angular.odoemtry_float=velocity_angular;//單位rad/s
//將所有里程計數據存到要發送的數組
for(j=0;j<4;j++)
{
odometry_data[j]=x_data.odometry_char[j];
odometry_data[j+4]=y_data.odometry_char[j];
odometry_data[j+8]=theta_data.odometry_char[j];
odometry_data[j+12]=vel_linear.odometry_char[j];
odometry_data[j+16]=vel_angular.odometry_char[j];
}
odometry_data[20]='\n';//添加結束符
//發送數據要串口
for(i=0;i<21;i++)
{
USART_ClearFlag(USART1,USART_FLAG_TC); //在發送第一個數據前加此句，解決第一個數據不能正常發送的問題
USART_SendData(USART1,odometry_data[i]);//發送一個字節到串口
while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); //等待發送結束
}
main_sta&=0xFE;//執行計算里程計數據步驟
}
if(main_sta&0x02)//執行計算里程計數據步驟
{
odometry(Milemeter_R_Motor,Milemeter_L_Motor);//計算里程計
main_sta&=0xFD;//執行發送里程計數據步驟
}
if(main_sta&0x08) //當發送指令沒有正確接收時
{
USART_ClearFlag(USART1,USART_FLAG_TC); //在發送第一個數據前加此句，解決第一個數據不能正常發送的問題
for(m=0;m<3;m++)
{
USART_SendData(USART1,0x00);
while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);
}
USART_SendData(USART1,'\n');
while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET);
main_sta&=0xF7;
}
if(USART_RX_STA&0x8000) // 串口1接收函數
{
//接收左右輪速度
for(t=0;t<4;t++)
{
rightdata.data[t]=USART_RX_BUF[t];
leftdata.data[t]=USART_RX_BUF[t+4];
}
//儲存左右輪速度
odometry_right=rightdata.d;//單位mm/s
odometry_left=leftdata.d;//單位mm/s
USART_RX_STA=0;//清楚接收標誌位
}
car_control(rightdata.d,leftdata.d); //將接收到的左右輪速度賦給小車
}//end_while
}//end main
/*********************************************END OF FILE**************************************************/
2.odometry.c 里程計計算
#include "odometry.h"
/*********************************************** 輸出 *****************************************************************/
float position_x=0,position_y=0,oriention=0,velocity_linear=0,velocity_angular=0;
/*********************************************** 輸入 *****************************************************************/
extern float odometry_right,odometry_left;//串口得到的左右輪速度
/*********************************************** 變量 *****************************************************************/
float wheel_interval= 268.0859f;// 272.0f; // 1.0146
//float wheel_interval=276.089f; //軸距校正值=原軸距/0.987
float multiplier=4.0f; //倍頻數
float deceleration_ratio=90.0f; //減速比
float wheel_diameter=100.0f; //輪子直徑，單位mm
float pi_1_2=1.570796f; //π/2
float pi=3.141593f; //π
float pi_3_2=4.712389f; //π*3/2
float pi_2_1=6.283186f; //π*2
float dt=0.005f; //採樣時間間隔5ms
float line_number=4096.0f; //碼盤線數
float oriention_interval=0; //dt時間內方向變化值
float sin_=0; //角度計算值
float cos_=0;
float delta_distance=0,delta_oriention=0; //採樣時間間隔內運動的距離
float const_frame=0,const_angle=0,distance_sum=0,distance_diff=0;
float oriention_1=0;
u8 once=1;
/****************************************************************************************************************/
//里程計計算函數
void odometry(float right,float left)
{
if(once) //常數僅計算一次
{
const_frame=wheel_diameter*pi/(line_number*multiplier*deceleration_ratio);
const_angle=const_frame/wheel_interval;
once=0;
}
distance_sum = 0.5f*(right+left);//在很短的時間內，小車行駛的路程爲兩輪速度和
distance_diff = right-left;//在很短的時間內，小車行駛的角度爲兩輪速度差
//根據左右輪的方向，糾正短時間內，小車行駛的路程和角度量的正負
if((odometry_right>0)&&(odometry_left>0)) //左右均正
{
delta_distance = distance_sum;
delta_oriention = distance_diff;
}
else if((odometry_right<0)&&(odometry_left<0)) //左右均負
{
delta_distance = -distance_sum;
delta_oriention = -distance_diff;
}
else if((odometry_right<0)&&(odometry_left>0)) //左正右負
{
delta_distance = -distance_diff;
delta_oriention = -2.0f*distance_sum;
}
else if((odometry_right>0)&&(odometry_left<0)) //左負右正
{
delta_distance = distance_diff;
delta_oriention = 2.0f*distance_sum;
}
else
{
delta_distance=0;
delta_oriention=0;
}
oriention_interval = delta_oriention * const_angle;//採樣時間內走的角度
oriention = oriention + oriention_interval;//計算出里程計方向角
oriention_1 = oriention + 0.5f * oriention_interval;//里程計方向角數據位數變化，用於三角函數計算
sin_ = sin(oriention_1);//計算出採樣時間內y座標
cos_ = cos(oriention_1);//計算出採樣時間內x座標
position_x = position_x + delta_distance * cos_ * const_frame;//計算出里程計x座標
position_y = position_y + delta_distance * sin_ * const_frame;//計算出里程計y座標
velocity_linear = delta_distance*const_frame / dt;//計算出里程計線速度
velocity_angular = oriention_interval / dt;//計算出里程計角速度
//方向角角度糾正
if(oriention > pi)
{
oriention -= pi_2_1;
}
else
{
if(oriention < -pi)
{
oriention += pi_2_1;
}
}
}
3.PID.c PID處理函數
#include "PID.h"
extern int span;
float MaxValue=9;//輸出最大限幅
float MinValue=-9;//輸出最小限幅
float OutputValue;//PID輸出暫存變量,用於積分飽和抑制
float PID_calculate(struct PID *Control,float CurrentValue_left )//位置PID計算B
{
float Value_Kp;//比例分量
float Value_Ki;//積分分量
float Value_Kd;//微分分量
Control->error_0 = Control->OwenValue - CurrentValue_left + 0*span;//基波分量，Control->OwenValue爲想要的速度，CurrentValue_left爲電機真實速度
Value_Kp = Control->Kp * Control->error_0 ;
Control->Sum_error += Control->error_0;
/***********************積分飽和抑制********************************************/
OutputValue = Control->OutputValue;
if(OutputValue>5 || OutputValue<-5)
{
Control->Ki = 0;
}
/*******************************************************************/
Value_Ki = Control->Ki * Control->Sum_error;
Value_Kd = Control->Kd * ( Control->error_0 - Control->error_1);
Control->error_1 = Control->error_0;//保存一次諧波
Control->OutputValue = Value_Kp + Value_Ki + Value_Kd;//輸出值計算，注意加減
//限幅
if( Control->OutputValue > MaxValue)
Control->OutputValue = MaxValue;
if (Control->OutputValue < MinValue)
Control->OutputValue = MinValue;
return (Control->OutputValue) ;
}
4.encoder.c 電機編碼處理
#include "encoder.h"
/****************************************************************************************************************/
s32 hSpeed_Buffer2[SPEED_BUFFER_SIZE]={0}, hSpeed_Buffer1[SPEED_BUFFER_SIZE]={0};//左右輪速度緩存數組
static unsigned int hRot_Speed2;//電機A平均轉速緩存
static unsigned int hRot_Speed1;//電機B平均轉速緩存
unsigned int Speed2=0; //電機A平均轉速 r/min，PID調節
unsigned int Speed1=0; //電機B平均轉速 r/min，PID調節
static volatile u16 hEncoder_Timer_Overflow1;//電機B編碼數採集
static volatile u16 hEncoder_Timer_Overflow2;//電機A編碼數採集
//float A_REMP_PLUS;//電機APID調節後的PWM值緩存
float pulse = 0;//電機A PID調節後的PWM值緩存
float pulse1 = 0;//電機B PID調節後的PWM值緩存
int span;//採集回來的左右輪速度差值
static bool bIs_First_Measurement2 = true;//電機A以清除速度緩存數組標誌位
static bool bIs_First_Measurement1 = true;//電機B以清除速度緩存數組標誌位
struct PID Control_left ={0.01,0.1,0.75,0,0,0,0,0,0};//左輪PID參數，適於新電機4096
struct PID Control_right ={0.01,0.1,0.75,0,0,0,0,0,0};//右輪PID參數，適於新電機4096
/****************************************************************************************************************/
s32 hPrevious_angle2, hPrevious_angle1;
/****************************************************************************************************************/
void ENC_Init2(void)//電機A碼盤採集定時器，TIM4編碼器模式
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
TIM_DeInit(ENCODER2_TIMER);
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_Period = (4*ENCODER2_PPR)-1;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(ENCODER2_TIMER, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(ENCODER2_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;
TIM_ICInit(ENCODER2_TIMER, &TIM_ICInitStructure);
TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update);
TIM_ITConfig(ENCODER2_TIMER, TIM_IT_Update, ENABLE);
TIM_Cmd(ENCODER2_TIMER, ENABLE);
}
void ENC_Init1(void)//電機B碼盤採集定時器，TIM3編碼器模式
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
TIM_DeInit(ENCODER1_TIMER);
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_Period = (4*ENCODER1_PPR)-1;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(ENCODER1_TIMER, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(ENCODER1_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER;
TIM_ICInit(ENCODER1_TIMER, &TIM_ICInitStructure);
TIM_ClearFlag(ENCODER1_TIMER, TIM_FLAG_Update);
TIM_ITConfig(ENCODER1_TIMER, TIM_IT_Update, ENABLE);
TIM_Cmd(ENCODER1_TIMER, ENABLE);
}
/****************************************************************************************************************/
s16 ENC_Calc_Rot_Speed2(void)//計算電機A的編碼數
{
s32 wDelta_angle;
u16 hEnc_Timer_Overflow_sample_one;
u16 hCurrent_angle_sample_one;
s32 temp;
s16 haux;
if (!bIs_First_Measurement2)//電機A以清除速度緩存數組
{
hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow2;
hCurrent_angle_sample_one = ENCODER2_TIMER->CNT;
hEncoder_Timer_Overflow2 = 0;
haux = ENCODER2_TIMER->CNT;
if ( (ENCODER2_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down)
{
// encoder timer down-counting 反轉的速度計算
wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR) -(hCurrent_angle_sample_one - hPrevious_angle2));
}
else
{
//encoder timer up-counting 正轉的速度計算
wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle2 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR));
}
temp=wDelta_angle;
}
else
{
bIs_First_Measurement2 = false;//電機A以清除速度緩存數組標誌位
temp = 0;
hEncoder_Timer_Overflow2 = 0;
haux = ENCODER2_TIMER->CNT;
}
hPrevious_angle2 = haux;
return((s16) temp);
}
s16 ENC_Calc_Rot_Speed1(void)//計算電機B的編碼數
{
s32 wDelta_angle;
u16 hEnc_Timer_Overflow_sample_one;
u16 hCurrent_angle_sample_one;
s32 temp;
s16 haux;
if (!bIs_First_Measurement1)//電機B以清除速度緩存數組
{
hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow1; //得到採樣時間內的編碼數
hCurrent_angle_sample_one = ENCODER1_TIMER->CNT;
hEncoder_Timer_Overflow1 = 0;//清除脈衝數累加
haux = ENCODER1_TIMER->CNT;
if ( (ENCODER1_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down)
{
// encoder timer down-counting 反轉的速度計算
wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER1_PPR) -(hCurrent_angle_sample_one - hPrevious_angle1));
}
else
{
//encoder timer up-counting 正轉的速度計算
wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle1 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER1_PPR));
}
temp=wDelta_angle;
}
else
{
bIs_First_Measurement1 = false;//電機B以清除速度緩存數組標誌位
temp = 0;
hEncoder_Timer_Overflow1 = 0;
haux = ENCODER1_TIMER->CNT;
}
hPrevious_angle1 = haux;
return((s16) temp);
}
/****************************************************************************************************************/
void ENC_Clear_Speed_Buffer(void)//速度存儲器清零
{
u32 i;
//清除左右輪速度緩存數組
for (i=0;i<SPEED_BUFFER_SIZE;i++)
{
hSpeed_Buffer2[i] = 0;
hSpeed_Buffer1[i] = 0;
}
bIs_First_Measurement2 = true;//電機A以清除速度緩存數組標誌位
bIs_First_Measurement1 = true;//電機B以清除速度緩存數組標誌位
}
void ENC_Calc_Average_Speed(void)//計算三次電機的平均編碼數
{
u32 i;
signed long long wtemp3=0;
signed long long wtemp4=0;
//累加緩存次數內的速度值
for (i=0;i<SPEED_BUFFER_SIZE;i++)
{
wtemp4 += hSpeed_Buffer2[i];
wtemp3 += hSpeed_Buffer1[i];
}
//取平均，平均脈衝數單位爲 個/s
wtemp3 /= (SPEED_BUFFER_SIZE);
wtemp4 /= (SPEED_BUFFER_SIZE); //平均脈衝數 個/s
//將平均脈衝數單位轉爲 r/min
wtemp3 = (wtemp3 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER1_PPR);
wtemp4 = (wtemp4 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER2_PPR);
hRot_Speed2= ((s16)(wtemp4));//平均轉速 r/min
hRot_Speed1= ((s16)(wtemp3));//平均轉速 r/min
Speed2=hRot_Speed2;//平均轉速 r/min
Speed1=hRot_Speed1;//平均轉速 r/min
}
/****************************************************************************************************************/
void Gain2(void)//設置電機A PID調節 PA2
{
//static float pulse = 0;
span=1*(Speed1-Speed2);//採集回來的左右輪速度差值
pulse= pulse + PID_calculate(&Control_right,hRot_Speed2);//PID調節
//pwm幅度抑制
if(pulse > 3600) pulse = 3600;
if(pulse < 0) pulse = 0;
//A_REMP_PLUS=pulse;//電機APID調節後的PWM值緩存
}
void Gain1(void)//設置電機B PID調節 PA1
{
//static float pulse1 = 0;
span=1*(Speed2-Speed1);//採集回來的左右輪速度差值
pulse1= pulse1 + PID_calculate(&Control_left,hRot_Speed1);//PID調節
////pwm 幅度抑制
if(pulse1 > 3600) pulse1 = 3600;
if(pulse1 < 0) pulse1 = 0;
TIM2->CCR2 = pulse1;//電機B賦值PWM
//TIM2->CCR3 = A_REMP_PLUS;//電機A賦值PWM
TIM2->CCR3 = pulse;//電機A賦值PWM
}
/****************************************************************************************************************/
void ENC_Init(void)//電機處理初始化
{
ENC_Init2(); //設置電機A TIM4編碼器模式PB6 PB7 右電機
ENC_Init1(); //設置電機B TIM3編碼器模式PA6 PA7 左電機
ENC_Clear_Speed_Buffer();//速度存儲器清零
}
/****************************************************************************************************************/
void TIM4_IRQHandler (void)//執行TIM4(電機A編碼器採集)計數中斷
{
TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update);
if (hEncoder_Timer_Overflow2 != U16_MAX)//不超範圍
{
hEncoder_Timer_Overflow2++; //脈衝數累加
}
}
void TIM3_IRQHandler (void)//執行TIM3(電機B編碼器採集)計數中斷
{
TIM_ClearFlag(ENCODER1_TIMER, TIM_FLAG_Update);
if (hEncoder_Timer_Overflow1 != U16_MAX)//不超範圍
{
hEncoder_Timer_Overflow1++; //脈衝數累加
}
}
5.contact.c 電機控制函數
#include "contact.h"
/*********************************************** 輸出 *****************************************************************/
/*********************************************** 輸入 *****************************************************************/
extern struct PID Control_left;//左輪PID參數，適於新電機4096
extern struct PID Control_right;//右輪PID參數，適於新電機4096
/*********************************************** 變量 *****************************************************************/
/*******************************************************************************************************************/
void LeftMovingSpeedW(unsigned int val)//左輪方向和速度控制函數
{
if(val>10000)
{
GPIO_SetBits(GPIOC, GPIO_Pin_6);
GPIO_ResetBits(GPIOC, GPIO_Pin_7);
Control_left.OwenValue=(val-10000);//PID調節的目標編碼數
}
else if(val<10000)
{
GPIO_SetBits(GPIOC, GPIO_Pin_7);
GPIO_ResetBits(GPIOC, GPIO_Pin_6);
Control_left.OwenValue=(10000-val);//PID調節的目標編碼數
}
else
{
GPIO_SetBits(GPIOC, GPIO_Pin_6);
GPIO_SetBits(GPIOC, GPIO_Pin_7);
Control_left.OwenValue=0;//PID調節的目標編碼數
}
}
void RightMovingSpeedW(unsigned int val2)//右輪方向和速度控制函數
{
if(val2>10000)
{
/* motor A 正轉*/
GPIO_SetBits(GPIOC, GPIO_Pin_10);
GPIO_ResetBits(GPIOC, GPIO_Pin_11);
Control_right.OwenValue=(val2-10000);//PID調節的目標編碼數
}
else if(val2<10000)
{
/* motor A 反轉*/
GPIO_SetBits(GPIOC, GPIO_Pin_11);
GPIO_ResetBits(GPIOC, GPIO_Pin_10);
Control_right.OwenValue=(10000-val2);//PID調節的目標編碼數
}
else
{
GPIO_SetBits(GPIOC, GPIO_Pin_10);
GPIO_SetBits(GPIOC, GPIO_Pin_11);
Control_right.OwenValue=0;//PID調節的目標編碼數
}
}
void car_control(float rightspeed,float leftspeed)//小車速度轉化和控制函數
{
float k2=17.179; //速度轉換比例,轉/分鐘
//將從串口接收到的速度轉換成實際控制小車的速度？還是PWM？
int right_speed=(int)k2*rightspeed;
int left_speed=(int)k2*leftspeed;
RightMovingSpeedW(right_speed+10000);
LeftMovingSpeedW(left_speed+10000);
}
//void Contact_Init(void)//左右輪方向和速度初始化
//{
// LeftMovingSpeedW(12000); //電機B
// RightMovingSpeedW(12000);//電機A
//}