基于ROS平台的STM32小车

您所在的位置：网站首页 › ros小车仿真 › 基于ROS平台的STM32小车

基于ROS平台的STM32小车

2024-07-17 17:33| 来源: 网络整理| 查看: 265

一切为了实现利用ros通过串口控制小车简单运动

基于ROS平台的STM32小车-4-上位机控制器 https://blog.csdn.net/weixin_39752599/article/details/86552511

下载串口通信的ROS包

cd ~/catkin_ws/src git clone https://github.com/ncnynl/serial.git

下载键盘控制的ROS包

cd ~/catkin_ws/src git clone https://github.com/ncnynl/teleop_twist_keyboard.git

进入下载好的ROS包的文件夹，选中 keyboard_teleop_zbot.py （没有此文件，只有teleop_twist_keyboard.py？？？），右键->设为可执行文件。

最后编译

cd ~/catkin_ws catkin_make

在上位机上搭建一个控制器：新建 base_controller ROS 包：

$ cd ~/catkin_ws/src $ catkin_create_pkg base_controller roscpp $ cd catkin_ws/src/base_controller $ mkdir src $ touch src/base_controller.cpp $ gedit src/base_controller.cpp

基于串口通信的ROS小车基础控制器，功能如下： 1.实现ros控制数据通过固定的格式和串口通信，从而达到控制小车的移动 2.订阅了/cmd_vel主题，只要向该主题发布消息，就能实现对控制小车的移动 3.发布里程计主题/odm 串口通信说明： 1.写入串口（1）内容：左右轮速度，单位为mm/s （2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节] 2.读取串口（1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s （2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节]

base_control.cpp代码如下：

#include "ros/ros.h" //ros需要的头文件 #include #include #include //以下为串口通讯需要的头文件 #include #include #include #include #include #include "serial/serial.h" /****************************************************************************/ using std::string; using std::exception; using std::cout; using std::cerr; using std::endl; using std::vector; /*****************************************************************************/ float ratio = 1000.0f ; //转速转换比例，执行速度调整比例 float D = 0.2680859f ; //两轮间距，单位是m float linear_temp=0,angular_temp=0;//暂存的线速度和角速度 /****************************************************/ unsigned char data_terminal0=0x0d; //“/r"字符 unsigned char data_terminal1=0x0a; //“/n"字符 unsigned char speed_data[10]={0}; //要发给串口的数据 string rec_buffer; //串口数据接收变量 //发送给下位机的左右轮速度，里程计的坐标和方向 union floatData //union的作用为实现char数组和float之间的转换 { float d; unsigned char data[4]; }right_speed_data,left_speed_data,position_x,position_y,oriention,vel_linear,vel_angular; /************************************************************/ void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数 { string port("/dev/ttyUSB0"); //小车串口号 unsigned long baud = 115200; //小车串口波特率 serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口 angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s linear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s //将转换好的小车速度分量为左右轮速度 left_speed_data.d = linear_temp - 0.5f*angular_temp*D ; right_speed_data.d = linear_temp + 0.5f*angular_temp*D ; //存入数据到要发布的左右轮速度消息 left_speed_data.d*=ratio; //放大１０００倍，mm/s right_speed_data.d*=ratio;//放大１０００倍，mm/s for(int i=0;ierror_0 ; Control->Sum_error += Control->error_0; /***********************积分饱和抑制********************************************/ OutputValue = Control->OutputValue; if(OutputValue>5 || OutputValueKi = 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.pid处理

#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 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(val10000) { /* motor A 正转*/ GPIO_SetBits(GPIOC, GPIO_Pin_10); GPIO_ResetBits(GPIOC, GPIO_Pin_11); Control_right.OwenValue=(val2-10000);//PID调节的目标编码数 } else if(val2

【本文地址】

基于ROS平台的STM32小车

基于ROS平台的STM32小车

今日新闻

推荐新闻