一切为了实现利用ros通过串口控制小车简单运动
下载串口通信的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 <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
//以下为串口通讯需要的头文件
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#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;i<4;i++) //将左右轮速度存入数组中发送给串口
{
speed_data[i]=right_speed_data.data[i];
speed_data[i+4]=left_speed_data.data[i];
}
//在写入串口的左右轮速度数据后加入”/r/n“
speed_data[8]=data_terminal0;
speed_data[9]=data_terminal1;
//写入数据到串口
my_serial.write(speed_data,10);
}
int main(int argc, char **argv)
{
string port("/dev/ttyUSB0");//小车串口号
unsigned long baud = 115200;//小车串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));//配置串口
ros::init(argc, argv, "base_controller");//初始化串口节点
ros::NodeHandle n; //定义节点进程句柄
ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅/cmd_vel主题
ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20); //定义要发布/odom主题
static tf::TransformBroadcaster odom_broadcaster;//定义tf对象
geometry_msgs::TransformStamped odom_trans;//创建一个tf发布需要使用的TransformStamped类型消息
nav_msgs::Odometry odom;//定义里程计对象
geometry_msgs::Quaternion odom_quat; //四元数变量
//定义covariance矩阵，作用为解决文职和速度的不同测量的不确定性
float covariance[36] = {0.01, 0, 0, 0, 0, 0, // covariance on gps_x
0, 0.01, 0, 0, 0, 0, // covariance on gps_y
0, 0, 99999, 0, 0, 0, // covariance on gps_z
0, 0, 0, 99999, 0, 0, // large covariance on rot x
0, 0, 0, 0, 99999, 0, // large covariance on rot y
0, 0, 0, 0, 0, 0.01}; // large covariance on rot z
//载入covariance矩阵
for(int i = 0; i < 36; i++)
{
odom.pose.covariance[i] = covariance[i];;
}
ros::Rate loop_rate(10);//设置周期休眠时间
while(ros::ok())
{
rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据
const char *receive_data=rec_buffer.data(); //保存串口发送来的数据
if(rec_buffer.length()==21) //串口接收的数据长度正确就处理并发布里程计数据消息
{
for(int i=0;i<4;i++)//提取X，Y坐标，方向，线速度，角速度
{
position_x.data[i]=receive_data[i];
position_y.data[i]=receive_data[i+4];
oriention.data[i]=receive_data[i+8];
vel_linear.data[i]=receive_data[i+12];
vel_angular.data[i]=receive_data[i+16];
}
//将X，Y坐标，线速度缩小1000倍
position_x.d/=1000; //m
position_y.d/=1000; //m
vel_linear.d/=1000; //m/s
//里程计的偏航角需要转换成四元数才能发布
odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//将偏航角转换成四元数
//载入坐标（tf）变换时间戳
odom_trans.header.stamp = ros::Time::now();
//发布坐标变换的父子坐标系
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
//tf位置数据：x,y,z,方向
odom_trans.transform.translation.x = position_x.d;
odom_trans.transform.translation.y = position_y.d;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//发布tf坐标变化
odom_broadcaster.sendTransform(odom_trans);
//载入里程计时间戳
odom.header.stamp = ros::Time::now();
//里程计的父子坐标系
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
//里程计位置数据：x,y,z,方向
odom.pose.pose.position.x = position_x.d;
odom.pose.pose.position.y = position_y.d;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//载入线速度和角速度
odom.twist.twist.linear.x = vel_linear.d;
//odom.twist.twist.linear.y = odom_vy;
odom.twist.twist.angular.z = vel_angular.d;
//发布里程计
odom_pub.publish(odom);
ros::spinOnce();//周期执行
loop_rate.sleep();//周期休眠
}
//程序周期性调用
//ros::spinOnce(); //callback函数必须处理所有问题时，才可以用到
}
return 0;
}
修改 CMakeLists.txt ：
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
serial
tf
nav_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES base_controller
CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
include_directories(
${catkin_INCLUDE_DIRS}
${serial_INCLUDE_DIRS}
)
add_executable(base_controller src/base_controller.cpp)
target_link_libraries(base_controller ${catkin_LIBRARIES})
检查确认底盘的串口号，如若不是ttyUSB0则在base_controller.cpp文件中修改串口号
$ ls -l /dev |grep ttyUSB
在终端执行
$cd ~/catkin_ws
$catkin_make
$ roscore
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
$ rosrun base_controller base_controller
#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.编码器处理
#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.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<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
//}
[ 不会独立建立一个完整的stm32工程？？？]
基于ROS平台的STM32小车-3-小车底盘与ROS的通信
https://blog.csdn.net/weixin_39752599/article/details/86552189
ROS平台与底盘通信协议
ROS平台与小车底盘的通信一般是通过串口或者CAN总线。
我这里采用的是串口，以下为我自定义的通信数据格式：
——底盘串口部分：
串口接收
（1）内容：小车左右轮速度,单位:mm/s（所有数据都为 float型，float型占4字节）
（2）格式：10字节 [右轮速度4字节][左轮速度4字节][结束符"\r\n"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字节]——ROS平台串口节点部分：
写入串口
（1）内容：左右轮速度，单位为mm/s
（2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节]读取串口
（1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s
（2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节]
串口通信处理程序
（1）小车底盘串口处理程序：
底盘串口处理的程序主要写在了 main.c 文件中。
底盘向串口发送里程计代码：
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;//执行计算里程计数据步骤
}
2.底盘接收串口发来的速度代码：
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;//清楚接收标志位
}
（2）ROS平台串口处理程序
ROS平台串口处理程序主要写在 base_controller.cpp 文件中。
ROS平台向串口发送速度代码：
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;i<4;i++) //将左右轮速度存入数组中发送给串口
{
speed_data[i]=right_speed_data.data[i];
speed_data[i+4]=left_speed_data.data[i];
}
//在写入串口的左右轮速度数据后加入”/r/n“
speed_data[8]=data_terminal0;
speed_data[9]=data_terminal1;
//写入数据到串口
my_serial.write(speed_data,10);
}
2.ROS平台接收串口发来的里程计代码：
rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据
const char *receive_data=rec_buffer.data(); //保存串口发送来的数据
[ 这些代码又该何去何从？？？]
rosserial通信协议简介
https://blog.csdn.net/x_r_su/article/details/52734403
1 概述
rosserial是一种ROS串行通信协议，通过串行传输介质实现ROS的P2P通信。这种协议通过简单的添加包头和包尾可以实现了多主题或者服务共享串行通信介质（如串口，网络socket）。
2 协议包格式
1st Byte - Sync Flag (Value: 0xff)
2nd Byte - Sync Flag / Protocol version
3rd Byte - Message Length (N) - Low Byte
4th Byte -Message Length (N) - High Byte
5th Byte - Checksum over message length
6th Byte - Topic ID - Low Byte
7th Byte - Topic ID - High Byte
N Bytes - Serialized Message Data Byte
N+8 - Checksum over Topic ID and Message Data
不同ROS发行版对应不同协议版本字段定义
（0xff：ROS Groovy, 0xfe on ROS Hydro, Indigo, and Jade.）。
Topics ID 0-100为系统功能专用主题使用, 这些主题类似于消息 rosserial_msgs/TopicInfo 中定义的那些特定主题。
长度和data的checksum字段用于确保包的完整性，data的checksum可以按照如下公式计算：
255 - ( (Topic ID Low Byte +
Topic ID High Byte +
data byte values) % 256)
3 主题协商
在数据传输之前，PC/平板一侧必须先向Arduino或者其它嵌入式设备发送主题查询请求，确定将要发送或者订阅的主题的名字和消息类型。
主题协商由主题查询请求，响应和主题定义组成。主题查询请求使用的topic ID为0。
主题查询请求类似于如下所示:
0xff 0xfe 0x00 0x00 0xff 0x00 0x00 0xff
主题查询响应包 (消息类型为rosserial_msgs/TopicInfo） 包含了特定主题信息，使用如下的数据信息：
uint16 topic_id
string topic_name
string message_type
string md5sum
int32 buffer_size
上面的topic_name是主题名称，如 “cmd_vel”, message_type是消息类型，如"geometry_msgs/Twist"。
注意：如果响应包未收到，查询会重发。
4 同步
相互之间的时间同步通过发送消息 std_msgs::Time实现。 嵌入式设备可以向PC/平板发送空的时间消息获取当前时间，响应返回的时间可以用于时间同步（检查时钟偏差）。
5 rosserial相关包
5.1 客户端库
通过这些库用户可以方便的让ROS节点在各种系统上启动和运行。下面这些库是通用ANSI C++ rosserial_client 库的具体化，目前包括如下：
rosserial_arduino
Arduino, especially UNO and Leonardo
rosserial_embeddedlinux
support for Embedded Linux (eg, routers)
rosserial_windows
support for communicating with Windows applications
rosserial_mbed
support for mbed platforms
rosserial_tivac
support for TI’s Launchpad boards, TM4C123GXL and TM4C1294XL
5.2 ROS厕接口
运行rosserial的众多设备需要通过在主机上运行的节点来把它们桥接起来，接入更广的ROS网络拓扑。
rosserial_python
A Python-based implementation (recommended for PC usage).
rosserial_server
A C++ implementation based on the ShapeShifter message, some limitations compared to rosserial_python but recommended for high-performance applications.
rosserial_java
A Java-based implementation. This implementation is only recommended when you need a Java OSGI based rosserial module or when you want to use rosserial with the Android ADK.
[ 看了也不知道重点在哪怎么用系列？？？]
https://blog.csdn.net/qq_36349536/article/details/82773064
本文主要介绍ROS下使用rosserial和STM32（ST库）进行通信，移植网上各位大神的代码，实现自己想要的功能
主要参考：https://www.baidu.com/link?url=HHBcr34K6SbLnst52P-4mSGPKxvCAQXGwGbHb5C_cp97Oe8f8cDQ8My__1_I3D-B0MezdtSdFuXy8awy6odoeqcmc8YiFrvOT8nCAFGr-YqwF1TCLtuqvRBkzquqXlP0&wd=&eqid=b7c144b80000b29c000000065ba1fb47
rosserial的详细介绍：http://wiki.ros.org/rosserial
rosserial_client的介绍：http://wiki.ros.org/rosserial_client
rosserial_client的教程程：http://wiki.ros.org/rosserial_client/Tutorials
本文配置的串口是串口1波特率是57600
写好底盘的代码后我们在我们的Ubuntu（ROS系统）中使用:
git clone https://github.com/ros-drivers/rosserial.git
下载rosserial的包然后使用catkin_make进行编译，
编译完成后先运行roscore （已完成至此）
然后再运行rosrun rosserial_python serial_node.py /dev/ttyUSB0
如果出现 "robot_Star Connected!"则说明连接成功。
https://blog.csdn.net/ykevin0510/article/details/72725633?locationNum=7&fps=1
关于stm32下的ROS开发环境介绍说明，此开发环境是在Linux下使用stm32的标准库“STM32F10x_StdPeriph_Driver3.5”，进行stm32开发，整体开发框架已搭建完成，用户开发简单，只需要按自己的方式开发代码即可，它集成了ros_lib，让开发ros底层像arduino一样操作，让广大机友从写stm32解析器结点中解放出来
一、开发环境的配置(ubuntu16.04系统)
1、安装编译工具链
$sudo apt-get install -y git build-essential gcc-arm-none-eabi
如果提示找不到相关安装包，请执行下面操作
$sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa
$sudo apt-get update
$sudo apt-get install -y git build-essential gcc-arm-none-eabi libusb-1.0-0-dev
2、安装st-link 烧写器驱动
$git clone https://github.com/texane/stlink.git
$cd stlink
$ make
$ cd build/Release
$ sudo make install
（已完成至此）
二、怎样添加自己的代码
1、往代码目录那面的Src、Bsp、Driver目录下面添加源码后，代码可支持C与C++，编写好代码后，（可直接参考上一篇下载好的程序）
请在Makefile文件中“OBJS += ./Driver/xxx.o
（？？？makefile在ros里面哪，具体如何添加）
”的样式添加，其中“xxx”就是你代码的文件名。
（这篇文章可能有帮助http://www.360doc.com/content/14/0307/15/7991404_358531203.shtml
高级~看不明白）
2、编译程序，进入工程主目录，执行
3、如果是添加C代码时，进行混编译，请注意.c中（这个是修改keil里面的程序吗？？？）请按下面格式编写代码，请注意只是.c代码需要添加，如果.c文件对应的有.h文件，则只需要在.h文件添加即可，.cpp代码不需要，此处作用，用户可以自己去了解，我就不赘述
#ifdef __cplusplus extern “C” {
#endif/*添加自己编写代码区域*/#ifdef __cplusplus } #endif
make
3、确认st-link驱动是否安装好，插入st-link V2 烧写器，执行下面命令，如果有“STMicroelectronics ST-LINK/V2”，则说明st-link烧写器已被系统识别
lsusb
4、进入工程主目录，执行
make flash
三、关于项目代码结构
1、 Bsp目录，关于驱动的配置与串口的驱动文件都放在此目录
2、 Driver目录，关于模块的驱动文件都放在此目录
3、Src目录，main程序入口文件放在此目录
4、Libs，里面放了ros_lib 与 stm32 标准库
四、关于开发板的测试使用
用户购买到开发板后，一般都是烧写好测试程序的，拿到手后可直接测试，测试流程如下
1、用micro usb（一定是能传输数据的usb）将开发板与PC端的ROS系统(indigo以上版本系统,如果是indigo版本系统请先删除系统默认的rosserial包，下载最新的rosserial，重新编译)相连接，连接好后检查是否识别到ttyUSB0，如果有，则说明连接正常，然后打开四个终端依次在每个终端运行
$roscore
$ rosrun rosserial_python serial_node.py /dev/ttyUSB0
运行下面命令，则会反馈系统的供电电压值，如下图
$ rostopic echo /battery
运行下面命令，板子上的LED会以0.1s的频率闪烁
$ rostopic pub -r 10 led std_msgs/Float64 – -0.001
（跳过第二步进行第四步得到的结果如下图）
问题解决—Unable to sync with device; possible link problem or link software version mismatch such as hyd
https://blog.csdn.net/wangguchao/article/details/86598059
（这篇文章波特率是115200，但是对于这个程序波特率应该就是57600）
2.1 下位机程序波特率看一下对不对，是不是115200;（没有下位机程序？？？）
2.2 上位机ROS查看波特率设置，例如在launch文件中查看参数设置，（哪个launch文件？？？stm32程序里面用的波特率确实为57600）
< node pkg="rosserial_python" type="serial_node.py" name="serial_node"
>
< param name="port" value="/dev/ttyUSB0" / >
< param name="baud" value="115200" / >
< /node >
2.3 设置用到的USB波特率（就是57600应该不用修改）
查看usb的属性：
stty -F /dev/ttyUSB0
如果不是115200，,修改usb的波特率为115200,方法如下：
stty -F /dev/ttyUSB0 115200
五、关于使用中的问题
$sudo chmod 777 /dev/ttyUSB0
1、永久解决串口权限问题, 其中riki是你系统的用户名，请替换，然后重启
$sudo usermod -aG dialout riki
2、“/dev/ttyUSB0: Input/output error” 此种问题是驱动问题，请安装我提供的驱动，将驱动源码放到ubuntu系统中
$unzip CH341SER_LINUX.zip
$ cd CH341SER_LINUX
$ make
上面编译后会生ch34x.ko文件，如果你已经能识别usb说明已装了老驱动，此时将它删除，加载新驱动
$sudo rmmod ch341
$sudo insmod ch34x.ko
要开机启动时自己加载驱动怎么办？
$sudo cp ch34x.ko /lib/modules/$(uname -r)/kernel/drivers/usb/serial
$sudo depmod
$sudo rm /lib/modules/$(uname -r)/kernel/drivers/usb/serial/ch341.ko
3、重启系统后，执行下面命令，如果驱动有ch34x，则说明安装成功
lsmod | grep ch
六、没有st-link的在linux下用ISP烧写程序
1、安装烧写环境
$sudo apt-get install stm32flash
（这一步已完成）
2、用usb串口烧写程序,烧写前请将Boot0设置为高，BOOT1设置为低,main.bin就是你要烧写的二进制文件，请替换，烧写时请按复位后，立即执行下面烧写命令，速度要快，不然会跳转失败，烧完请恢复默认设置。
$sudo stm32flash -w main.bin -v -g 0x0 /dev/ttyUSB0 -b 115200
（可能有帮助http://www.elecfans.com/emb/app/20171116580349_a.html）
———————————————————————————————————————
将rikirobot的程序下载到舵机小车里后蓝灯不再闪烁，单片机无法正常工作，再次在虚拟机控制还是不行