搭建ROS小车底盘B-小车底盘控制
搭建ROS小车底盘B-小车底盘控制
说明
- 本博文将介绍小车底盘控制的原理，如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
//}
- github： https://github.com/ncnynl/ros-car-stm32.git
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号