ROS小车搭建（一）——底层STM32使用

最近在弄ROS小车，做一些分享吧。

由于实验室的需求需要搭建一个智能移动平台，平台是两个轮子的差速运动模型。同时也是比较常见的模型，在创客智造上也有相关的代码和说明。自己也在此基础上做了修改，同时也也有之前实验室的师兄留下的代码，做了一些修改。

1前期准备工作

需要准备的材料：STM32板子（stm32vet6,100引脚）、伺服直流电机以及驱动器、减速器、轮子、串口转USB的线、USB-CAN分析仪，上位机电脑、keil5软件、串口调试助手、USB-CAN监控软件（CAN_TEST）等。

因为相关的机械结构已经制作好了，不需要再多余的安装相关机械设施，主要把重点集中在STM32的调试上。首先需要说一下整体的思路如下图所示：

                                                                      

 其实原理上是很简单的，就是通过上位机发送通过串口发送指令给STM32，STM32通过检测到串口发送的命令进行分析并通过CAN分配给两个电机一定的速度，同时STM32通过CAN接收到电机的位置反馈信息，经过计算得到里程计信息再经过串口发送给上位机。

• 驱动器电机配置

因为驱动器和伺服电机已经是采购好的，所以需要通过厂家给定的上位机软件对其进行设置即可，具体如下图所示。

 上图中黄色框部分我基本上没有改动，厂家给定的参数即可。蓝色框部分是我用到的部分，需要选择模式，上方的是PC端输入，下方是速度控制即转速控制。中间黑色框部分是设置CAN报文的时间即多长时间发送报文，我设置的是0，因为我在STM32身上会发送相关请求指令，关键的是CAN的波特率一定和STM32的对应上，否则会有一些麻烦，还有就是可以再软件上设置CAN的ID号和组号，通过设置ID可以使STM32识别不同的电机并且发送相关指令。右侧的红色框是PID调节部分，这一部分需要自己调节，一般厂家也会设置好，这里面很方便不会占用很长时间。一般PID调节的首先需要使ID为0，开始调节P值，从小到大，然后在接近目标速度时需要将此时的值记下并乘以0.75，然后开始调节I，使波动范围最小确定下此时的I值，最后调节PI以及D的值使其更精确，一般都用PI调节。最右侧的红色框表示对电压以及电流的转速等的监控。最后驱动器连接好的如下图所示。

上图中左侧的“网线”是和上位机进行232通讯，配置相关参数即上位机软件，此口也会用到和STM32进行CAN通讯的连接。中间的连线接电机，右侧红黑线是电源。相关的参数是电机48V，200W，编码器码盘线数2500，4倍频。其实以上说的这些都是厂家给的资料信息，其中有些接线是自己制作的。

• CAN的连线部分

一般CAN的部分网上有很多，我觉得这位博主的还行关于CAN，需要注意的是终端电阻的配置，一般为120欧。其次是伺服直流电机驱动器和STM32的连接情况，就是“高接高，低接低”，CAN_H和CAN_H对应，CAN_L和CAN_L对应。其次就是CAN-USB的分析仪，这个设备在CAN调试上有着至关重要的作用。给个图，嘿嘿。这个图是在网上找的，我实际上用的和这个差不多，不过比这个圆润多了。

前期的准备工作已经差不多了，主要是在各种接线和一些初始化的配置。这个根据每个人不同的情况来定。接下来就是在STM32上进行编程设计了。

2CAN的配置和使用

STM32这个工控板是含有一个CAN通讯接口的，下面就需要在软件上对其进行一定的配置。具体代码如下：

void CAN1_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure; 
    NVIC_InitTypeDef NVIC_InitStructure;
    CAN_InitTypeDef        CAN_InitStructure;
    CAN_FilterInitTypeDef  CAN_FilterInitStructure;

    /* 复用功能和GPIOB端口时钟使能*/	 
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOB, ENABLE);	                        											 

    /* CAN1 模块时钟使能 */
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE); 

    /* Configure CAN pin: RX */	 // PB8
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;	    // 上拉输入
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    /* Configure CAN pin: TX */   // PB9
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;     // 复用推挽输出
    GPIO_Init(GPIOB, &GPIO_InitStructure);  

    //#define GPIO_Remap_CAN    GPIO_Remap1_CAN1 本实验没有用到重映射I/O
    GPIO_PinRemapConfig(GPIO_Remap1_CAN1, ENABLE);

    //CAN_NVIC_Configuration(); //CAN中断初始化   
    /* Configure the NVIC Preemption Priority Bits */  
    //NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);

    #ifdef  VECT_TAB_RAM  
    /* Set the Vector Table base location at 0x20000000 */ 
        NVIC_SetVectorTable(NVIC_VectTab_RAM, 0x0); 
    #else  /* VECT_TAB_FLASH  */
    /* Set the Vector Table base location at 0x08000000 */ 
        NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x0);   
    #endif

    /* enabling interrupt */
    NVIC_InitStructure.NVIC_IRQChannel=USB_LP_CAN1_RX0_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);
    
    //CAN_INIT();//CA初始化N模块	
    /* CAN register init */
    CAN_DeInit(CAN1);	//将外设CAN的全部寄存器重设为缺省值
    CAN_StructInit(&CAN_InitStructure);//把CAN_InitStruct中的每一个参数按缺省值填入

    /* CAN cell init */
    CAN_InitStructure.CAN_TTCM=DISABLE;         //没有使能时间触发模式
    CAN_InitStructure.CAN_ABOM=DISABLE;         //没有使能自动离线管理
    CAN_InitStructure.CAN_AWUM=DISABLE;         //没有使能自动唤醒模式
    CAN_InitStructure.CAN_NART=DISABLE;         //没有使能非自动重传模式
    CAN_InitStructure.CAN_RFLM=DISABLE;         //没有使能接收FIFO锁定模式
    CAN_InitStructure.CAN_TXFP=DISABLE;         //没有使能发送FIFO优先级
    CAN_InitStructure.CAN_Mode=CAN_Mode_Normal;//CAN设置为正常模式
    CAN_InitStructure.CAN_SJW=CAN_SJW_1tq;      //重新同步跳跃宽度1个时间单位
    CAN_InitStructure.CAN_BS1=CAN_BS1_3tq;      //时间段1为3个时间单位
    CAN_InitStructure.CAN_BS2=CAN_BS2_2tq;      //时间段2为2个时间单位
    CAN_InitStructure.CAN_Prescaler=6;         //时间单位长度为60	
    CAN_Init(CAN1,&CAN_InitStructure);          //波特率为：72M/2/6(1+3+2)=1 即波特率为1000KBPs

    //CAN filter init 过滤器，已经设置为任意，可以通过ExtId标识符区分从机代号
    CAN_FilterInitStructure.CAN_FilterNumber=1;                     //指定过滤器为1
    CAN_FilterInitStructure.CAN_FilterMode=CAN_FilterMode_IdMask;   //指定过滤器为标识符屏蔽位模式
    CAN_FilterInitStructure.CAN_FilterScale=CAN_FilterScale_32bit;  //过滤器位宽为32位
    
    CAN_FilterInitStructure.CAN_FilterIdHigh=0x0000;                //过滤器标识符的高16位值
    CAN_FilterInitStructure.CAN_FilterIdLow=CAN_ID_EXT|CAN_RTR_DATA;//过滤器标识符的低16位值
    CAN_FilterInitStructure.CAN_FilterMaskIdHigh=0x0000;            //过滤器屏蔽标识符的高16位值
    CAN_FilterInitStructure.CAN_FilterMaskIdLow=0x0000;             //过滤器屏蔽标识符的低16位值
    
    CAN_FilterInitStructure.CAN_FilterFIFOAssignment=CAN_FIFO0;     //设定了指向过滤器的FIFO为0
    CAN_FilterInitStructure.CAN_FilterActivation=ENABLE;            //使能过滤器
    CAN_FilterInit(&CAN_FilterInitStructure);                       //按上面的参数初始化过滤器

    /* CAN FIFO0 message pending interrupt enable */ 
    CAN_ITConfig(CAN1,CAN_IT_FMP0, ENABLE);                         //使能FIFO0消息挂号中断
}

此段代码为CAN的初始化设置。一般在网络上都可以找到的。接下来呢就是需要通过CAN给电机发送指令使电机转起来啊，这部分呢是根据厂家的具体CAN协议配置的发送指令的。 例如像如下情况，可以对ID位和数据进行设置。根据厂家手册进行设置即可，这里不一一说明了。

接下来就是CAN得发送指令部分，如下代码，这里使用的是标准标识。

/* 发送数据*/
u8 CAN_SendMsg(uint32_t ID, u8* data)
{ 
    u8 mbox;
    u16 i=0; 
    CanTxMsg TxMessage;  
    
    TxMessage.StdId=ID;	 	//标准标识符为ID
    TxMessage.ExtId=0x0000;         	//扩展标识符0x0000
    TxMessage.IDE=CAN_ID_STD;   	//使用扩展标识符
    TxMessage.RTR=CAN_RTR_DATA; 	//为数据帧
    TxMessage.DLC=8;	        		//消息的数据长度为2个字节
    for(i=0;i<8;i++)
        TxMessage.Data[i] = data[i];

    //发送数据
    mbox= CAN_Transmit(CAN1, &TxMessage);  
    while((CAN_TransmitStatus(CAN1, mbox)==CAN_TxStatus_Failed)&&(i<0XFFF))
        i++;	//等待发送结束
    if(i>=0XFFF)
        return 0;//发送失败
    return 1;//发送成功	
}

这个函数是为发送速度指令所服务的，以上的代码是在can.c文件中的，还需要设置控制电机转速的函数即需要在motor.c函数中设置转速，代码如下

void Motor_Init(void)
{
	//使用速度模式
	CommandData[0]=0x00; 
	CommandData[1]=FCW_One2One; 
	CommandData[2]=Register_Mode; 
	CommandData[3]=0x00; 
	CommandData[4]=0xc4;
	CommandData[5]=Register_Mode; 
	CommandData[6]=0x00; 
	CommandData[7]=0xc4;	
	CAN_SendMsg(ID_RightMotor, CommandData);//右侧轮子
	CAN_SendMsg(ID_LeftMotor, CommandData);//左侧轮子

//	//设置电机的加减速参数,软件上设置
//	CommandData[0]=0x00; 
//	CommandData[1]=FCW_One2Many; //点对点模式
//	CommandData[2]=Resister_AccTimeInSpeed; 
//	CommandData[3]=0x03; 
//	CommandData[4]=0x03;
//	CommandData[5]=Resister_AccTimeInSpeed; 
//	CommandData[6]=0x03; 
//	CommandData[7]=0x03;	
////	CAN_SendMsg(ID_LeftMotor, CommandData);
////	CAN_SendMsg(ID_RightMotor, CommandData);

	//设置电机启动使能,两个轮子共同使用
	CommandData[0]=0x00; 
	CommandData[1]=FCW_One2One; 
	CommandData[2]=Register_SpeedSet; 
	CommandData[3]=0x01; 
	CommandData[4]=0xff;
	CommandData[5]=Register_Moter; 
	CommandData[6]=0x00; 
	CommandData[7]=0x00;	
//	CAN_SendMsg(ID_Master, CommandData);
	CAN_SendMsg(ID_RightMotor, CommandData);
	CAN_SendMsg(ID_LeftMotor, CommandData);
}


//设置目标转速,直接输入的是转速值
void Set_SpeedTarget(uint32_t ID, float target_speed)
{
//	int speed = Speed2RPM((target_speed/8192.0)*3000);//????这么计算不知道有没有问题
	int speed = (target_speed/3000)*8192;
	CommandData[0]=0x00; 
	CommandData[1]=FCW_One2One; 
	CommandData[2]=Register_SpeedSet; 
	CommandData[3]=(speed >> 8) & 0XFF;	 //将speed左移8个单位在和0xff与，取speed的前8位	
	CommandData[4]=speed & 0XFF;	       //将speed和0xff与，取speed的后8位	
	CommandData[5]=Register_Moter; 
	CommandData[6]=0x00;	
	CommandData[7]=0x01;	
	CAN_SendMsg(ID, CommandData);

先进行初始化，设置电机的工作模式，使能等（这些根据厂家资料进行配置），主要的函数在Set_SpeedTarget函数上。此时就可以启动电机给其发送相关的速度了。 这里需要注意的是在CAN得工作模式是可以根据协议进行选择的，分为一对一和一对多的模式，根据实际情况进行分析。

最后就是关于CAN的中断函数了，我在CAN得中断函数中，实现的功能是读取电机的电流和脉冲等信息，此处判断的协议是根据驱动器给定的参数地址而定的；如何使STM32之间的通讯则需要设置主从机以及相关的地址。这里应该用到的CAN的扩展帧。可以参考这个STM32之间的CAN通讯。结合上述的代码，我的can.c文件余下部分如下：

CanRxMsg RxMessage;
u8 CAN_RX_BUF[CAN_RX_LEN]={0};     //接收缓冲,最大USART_REC_LEN个字节.

/* USB中断和CAN接收中断服务程序，USB跟CAN公用I/O，这里只用到CAN的中断。 */
void USB_LP_CAN1_RX0_IRQHandler(void)
{
	
	u16 i =0,j=0;//u8不能显示全,
	float k = 0 ;//反应电流的值
//	GPIO_WriteBit(GPIOB,GPIO_Pin_13,1-(GPIO_ReadOutputDataBit(GPIOB,GPIO_Pin_13)));//检测是否进入CAN的中断,D2
	LED1 = !LED1;//检测是否进入CAN的中断,D2
	Can_data_init();  //清除CAN中断数据
	CAN_Receive(CAN1,CAN_FIFO0, &RxMessage);
//返回里程计信息

//OLED显示电机转速和电流
//电机5
	if(RxMessage.StdId == ID_LeftMotor)
	{
			OLED_ShowString(12, 10,"ID5s:",16);
			//显示转速
			if(RxMessage.Data[5] == Resister_RSpeed)
			{
				i = RxMessage.Data[6] << 8 | RxMessage.Data[7];
				//判断正反转
				if(i>8192)
				{
					i = -(RxMessage.Data[6] << 8 | RxMessage.Data[7]);//取反进行操作，取出负的转速
					i = (i/8192.0)*3000;//转速为负
					OLED_ShowString(6,12,"-",16);
					OLED_ShowNum(14,12,i,5,16);
				}
				else
				{
					i = (i/8192.0)*3000;
					OLED_ShowNum(12,12,i,5,16);
				}
			
			}
			//显示电流
			if(RxMessage.Data[2]==Resister_Current)
			{
				Rx_flag = 1;
				k =  RxMessage.Data[3] << 8 | RxMessage.Data[4];
				k = k/100;
				OLED_ShowNumber(12,14,k);
			}
			//显示脉冲个数
			if((RxMessage.Data[2]==Resister_ROdomH)&&(RxMessage.Data[1]==FCR_One2One_Ri))
			{
		//求解里程信息
				now_L = (RxMessage.Data[3] << 32 | RxMessage.Data[4]<< 16|RxMessage.Data[6] << 8 | RxMessage.Data[7]);//？？？？注意反馈的位置由脉冲数表示？？？转换成里程m
		//		*odom = (now - past) / ((double)PlusePerRound*90) * (PI*Diameter);//里程计信息，采样时间内轮子行走的距离
				left_odom = (now_L - past_L);//发送脉冲个数
				past_L = now_L;//时间更新
				flagcan1 =1;
			}
	}
//电机6
	else if(RxMessage.StdId == ID_RightMotor)
	{
		OLED_ShowString(80, 10,"ID6s:",16);
		//显示转速
		if(RxMessage.Data[5] == Resister_RSpeed)
		{
			j = RxMessage.Data[6] << 8 | RxMessage.Data[7];
			//判断正反转，最大设置为3000转
			if(j>8192)
			{	
				j = -((RxMessage.Data[6]) << 8 | RxMessage.Data[7]);//取反进行操作，取出负的转速
				j = (j/8192.0)*3000;
				OLED_ShowString(70,12,"-",16);
				OLED_ShowNum(80,12,j,5,16);
			}
			else
			{
				j = (j/8192.0)*3000;
				OLED_ShowNum(80,12,j,5,16);
			}
		}
		//显示电流
		if(RxMessage.Data[2]==Resister_Current)
		{
			Rx_flag = 1;
			k =  RxMessage.Data[3] << 8 | RxMessage.Data[4];
			k = k/100;
			OLED_ShowNumber(80,14,k);
		}
		//显示里程计
		if((RxMessage.Data[2]==Resister_ROdomH)&&(RxMessage.Data[1]==FCR_One2One_Ri))
		{
			//求解里程信息
			now_R = (RxMessage.Data[3] << 32 | RxMessage.Data[4]<< 16|RxMessage.Data[6] << 8 | RxMessage.Data[7]);//？？？？注意反馈的位置由脉冲数表示？？？转换成里程m
	//		*odom = (now - past) / ((double)PlusePerRound*90) * (PI*Diameter);//里程计信息，采样时间内轮子行走的距离
			right_odom = (now_R - past_R);//发送脉冲个数
			past_R = now_R;//时间更新
			flagcan2 = 2;
		}
	}
}

//CAN清除处理
void Can_data_init(void)
{
  RxMessage.StdId=0x00;
  RxMessage.ExtId=0x00;
  RxMessage.IDE=0;
  RxMessage.DLC=0;
  RxMessage.FMI=0;
  RxMessage.Data[0]=0x00;
  RxMessage.Data[1]=0x00;   
  RxMessage.Data[2]=0x00;
  RxMessage.Data[3]=0x00;
  RxMessage.Data[4]=0x00;
  RxMessage.Data[5]=0x00;
  RxMessage.Data[6]=0x00;
  RxMessage.Data[7]=0x00;
}

上述电机设置的ID号为5和6，这里需要根据自己的实际情况更改即可 

3串口的使用 

串口这一部分是和上位机进行通讯的部分，可以分为两个部分进行，一部分为串口接收数据，一部分为串口读取数据。首先需要说的是串口接收数据，这部分的调试是依靠上位机的串口助手进行，上位机助手通过发送一系列的指令给STM32，然后STM32通过这些指令分析出两个轮子的速度并通过CAN给其发送转速命令。需要的工具贴张图，比较常见的设备。

接下来就是串口的代码部分，其中包含初始化，接收中断，和发送。 

#include "usart.h"	  
#include "contact.h" 
//#include "oled.h"


#if SYSTEM_SUPPORT_UCOS
#include "includes.h"					//ucos 使用	  
#endif
 

//////////////////////////////////////////////////////////////////
//加入以下代码,支持printf函数,而不需要选择use MicroLIB	  
#if 1
#pragma import(__use_no_semihosting)             
//标准库需要的支持函数                 
struct __FILE 
{ 
	int handle; 

}; 

FILE __stdout;       
//定义_sys_exit()以避免使用半主机模式    
void _sys_exit(int x) 
{ 
	x = x; 
} 
//重定义fputc函数 
int fputc(int ch, FILE *f)
{      
	while((USART1->SR&0X40)==0)
        ;//循环发送,直到发送完毕   
    USART1->DR = (u8) ch;      
	return ch;
}
#endif 
 

//串口1中断服务程序
//注意,读取USARTx->SR能避免莫名其妙的错误   	
u8 USART_RX_BUF[USART_REC_MAXLEN]={0};     //接收缓冲,最大USART_REC_LEN个字节.
//u16 USART_RX_STA=0;       //接收状态标记	  
u8 len=0;
u8 j=0;
u8 m;
//初始化IO 串口1 
//bound:波特率
void USART1_Init(u32 bound)
{
    //GPIO端口设置
    GPIO_InitTypeDef GPIO_InitStructure;    //串口端口配置结构体变量
	USART_InitTypeDef USART_InitStructure;  //串口参数配置结构体变量
	NVIC_InitTypeDef NVIC_InitStructure;
	 
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE);	 //打开GPIOA时钟和GPIOA复用时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);	//打开串口复用时钟
	USART_DeInit(USART1);  //复位串口1
    
//USART1_TX   PA.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;	//复用推挽输出
	GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化PA9
 
	//USART1_RX	  PA.10
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
	GPIO_Init(GPIOA, &GPIO_InitStructure);  //初始化PA10

   //Usart1 NVIC 配置
    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0;//抢占优先级3
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;		//子优先级3
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			//IRQ通道使能
	NVIC_Init(&NVIC_InitStructure);	//根据指定的参数初始化VIC寄存器
  
   //USART 初始化设置
	USART_InitStructure.USART_BaudRate = bound;//一般设置为9600;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
	USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//收发模式
  USART_Init(USART1, &USART_InitStructure); //初始化串口
    
	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启中断
	USART_Cmd(USART1, ENABLE);                    //使能串口 
	 
	USART_ClearFlag(USART1, USART_FLAG_TC);                //清串口1发送标志
}

union float2uchar LeftMotor_TargetVel, RightMotor_TargetVel;
union int2uchar LeftMotor, RightMotor;


void USART1_IRQHandler(void)
{
	OLED_ShowNumber(80, 12, len);
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断(接收到的数据必须是0x0d 0x0a结尾)
	{

//		USART_ClearITPendingBit(USART1,USART_IT_RXNE);
		USART_RX_BUF[len] = USART_ReceiveData(USART1);//(USART1->DR);	//读取接收到的数据
		len++;
//		Set_SpeedTarget(ID_RightMotor, (float)USART_ReceiveData(USART1));

		if(USART_RX_BUF[0]== 0x44)//判断包头
		{
			if(USART_RX_BUF[9] == 0x0a)//接收完毕标志
			{
	//			OLED_ShowNumber(119, 0, 5);
					OLED_clearRow(119,0,130);
					OLED_ShowNumber(119, 0, 4);
					for(m = 0 ; m < 4 ; m++)
					{
						 LeftMotor_TargetVel.data[m] = USART_RX_BUF[m+1];   
						 RightMotor_TargetVel.data[m] = USART_RX_BUF[m+4+1];   
					}	
	//				Set_SpeedTarget(ID_LeftMotor, LeftMotor_TargetVel.value);//int型的试探
	//				Set_SpeedTarget(ID_RightMotor, RightMotor_TargetVel.value);
					Set_SpeedTarget(ID_LeftMotor, LeftMotor_TargetVel.value);
					Set_SpeedTarget(ID_RightMotor, RightMotor_TargetVel.value);	
					
//					OLED_clearRow(60,14,130);
					OLED_ShowNumber(70,14, LeftMotor_TargetVel.value);
					OLED_ShowNumber(100,14, RightMotor_TargetVel.value);
			}
		}
		else
		{

			len = 0;
		}
	
	}
	if((len>=10)||(len>USART_REC_MAXLEN))
	{
		
//		OLED_ShowNumber(30, 14, USART_RX_BUF[0]);
//		OLED_ShowNumber(50, 14, USART_RX_BUF[9]);
		Clear_serialBuffer();
	}
	
	//如果发生溢出先读SR，在读DR寄存器则可清除出不断如中断的问题
	if(USART_GetFlagStatus(USART1,USART_FLAG_ORE) == SET)
	{
		USART_ReceiveData(USART1);
		USART_ClearFlag(USART1,USART_FLAG_ORE);
	 }
  
}

//发送数据
void UART1SendByte(char SendData)
{	   
		USART_SendData(USART1,SendData);
		while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);	    
}
void Clear_serialBuffer(void)
{
    USART_RX_STA=0;//清楚接收标志位
		len =0;
    memset(USART_RX_BUF, 0, sizeof(u8)*USART_REC_MAXLEN);//清空缓冲区
}

这个和创客智造的有些区别，我做了一些修改，效果还可以，不过还需要再进一步的验证了，主要在中断函数那，串口接收数据是按照一个字节进行接收，这里我设定了发送的串口数据包头和包尾，以字符‘D’为头，‘\n’为尾，以16进制发送，就是0x44和0x0a了然后中间的8位是两个轮子的转速数据，这里涉及到一个Union共同体的知识，这个需要加深一下理解。大家可以参考Unino共同体 ，主要在内存分配的空间上，以及数据怎么转换上，最好用编译软件敲下代码这样很容易理解。

union float2uchar  //接收到的数据
{
	float value;    //轮速度
	unsigned char data[4];
};

 上面贴的代码就是一个Union共同体了，其中float型数据占用四个字节，unsignedchar型占用一个字节，其实value的地址和data[4]的首地址是相同的，当然值也是可以共同使用的。不过在调试过程中需要注意的是，在flaot和char数组转换的过程中不是和正常思维一样的，具体怎么转换我也不是很明白，但是如果数组的元素值设置不正确会使转换的值为0，这样下发的速度值为零，为调试带来很多不便。所以在利用串口调试助手调试的过程中最好用代码实现下，还是很简单的。如果STM32直接和Linux系统的上位连接，使用ROS发布串口信息可能此处不需要串口助手，视情况而定了。

上述的结果请可以忽略很多ff情况，取后两位即可，具体原因我也不是很明白，谁知道请在评论告诉我，谢谢了！还有就是数组元素代表的地位和高位，我这个是c[0]是最低位，c[3]是最高位，一次增高，这个根据不同的操作系统可能不同，这个需要考证一下。Union共同体部分使用完毕了。 这里串口接收到数据直接利用CAN发送给电机，实时性更好一些。

下一个部分是串口发送部分，这个部分的主要功能是把里程计信息发送给上位机，直接上代码。

extern float left_odom, right_odom; 
char DF2ROS[22]={0};   //数据帧，发送给上位机的里程计数据
u8 send_odoemtry(void)
{
    u8 i=0;
    double angle_vel,angle_yaw;
    union float2uchar position_x,position_y,oriention,velocity_linear,velocity_angular;       //里程计值
    DF2ROS[0] = 'D';
		//if(Get_Odometry(ID_LeftMotor, &left_odom) && Get_Odometry(ID_RightMotor, &right_odom))
    //if(Check_canRX())
	
	//获取里程计和转速电流信?
			Get_Odometry(ID_LeftMotor);
			Get_Odometry(ID_RightMotor);
	
	
			OLED_clearRow(12,12,90);
			OLED_ShowNumber(12,12,left_odom/((double)PlusePerRound*90) * (PI*Diameter));
	
			OLED_clearRow(12,14,90 );
			OLED_ShowNumber(12,14,right_odom/((double)PlusePerRound*90) * (PI*Diameter));
		

			compute_odometry(left_odom,right_odom);
			get_odometry(&(position_x.value), &(position_y.value), &(oriention.value), &(velocity_linear.value), &(velocity_angular.value));
        //将所有里程计数据存到要发送的数组
		for(i=0;i<4;i++)
		{
				DF2ROS[i+1]=position_x.data[i];
				DF2ROS[i+5]=position_y.data[i];
				DF2ROS[i+9]=oriention.data[i];
				DF2ROS[i+13]=velocity_linear.data[i];
				DF2ROS[i+17]=velocity_angular.data[i];
		}

				DF2ROS[21]='\n';
        //发送数据到串口
				for(i=0;i<22;i++)
        {
            USART_ClearFlag(USART1,USART_FLAG_TC);  //在发送第一个数据前加此句，解决第一个数据不能正常发送的问题					
						UART1SendByte(DF2ROS[i]);//发送到传口
        }        
    
    return serial_flag;
}

声明的 left_odom, right_odom在主函数中定义了，目的是获取里程计信息，还有比较重要的部分是里程计的获取和计算，其中获取部分在CAN中断函数中有说明了，我这里理解的是获取的是脉冲数，这里需要了解一下编码器的相关知识，我们用的是增量式编码器，包括一些码盘线数和倍频数需要根据自己实际情况而定。下面的代码是motor.c文件中的获取里程计命令，由CAN发送。

int Get_Odometry(uint32_t ID)
{
	CommandData[0]=0x00; 
	CommandData[1]=FCR_One2One; 
	CommandData[2]=Resister_ROdomH; 
	CommandData[3]=0;
	CommandData[4]=0;	       
	CommandData[5]=Resister_ROdomL; 
	CommandData[6]=0;	
	CommandData[7]=0;	
	CAN_SendMsg(ID, CommandData);
	
	return 0;
}

下面代码是odometry.c文件，主要参考的创客智造智造的原理，这里需要了解一下差速小车的运动模型了，航迹推算这部分由比较有名的博主白巧克亦唯心写的，大家可以关注下，挺厉害的以为帅哥。还有对SLAM感兴趣的也可以看一看他的博客，里面有一些底层和上层技术的博文，个人感觉很好。下面就是代码了

#include "odometry.h"
#include "math.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 wheel_interval= 620.0f;

float multiplier=4.0f;           //倍频数
float deceleration_ratio=90.0f;  //减速比
//float wheel_diameter=100.0f;     //轮子直径，单位mm
float wheel_diameter=200.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 line_number=2500; 				//码盘线数，新
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;

//union float2uchar left_current,left_speed,right_current,right_speed;

u8 once=1;

/****************************************************************************************************************/

//里程计计算函

void compute_odometry(float odom_right,float odom_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*(abs(odom_right)+abs(odom_left));//在很短的时间内，小车行驶的路程为两轮速度和
	distance_diff = abs(odom_right)-abs(odom_left);//在很短的时间内，小车行驶的角度为两轮速度差	
	
	
    //根据左右轮的方向，纠正短时间内，小车行驶的路程和角度量的正负
	if((odom_left>0)&&(odom_right<0))            //左右均正?/，新:前进,左正右负
	{
		delta_distance = distance_sum;
		delta_oriention = distance_diff;
//		OLED_ShowNumber(119, 0, 0);
	}
	else if((odom_left<0)&&(odom_right>0))       //左右均负//，新:后退，左负右正
	{
		delta_distance = -distance_sum;
		delta_oriention = -distance_diff;
//		OLED_ShowNumber(119, 0, 1);
	}
	else if((odom_left<0)&&(odom_right<0))       //左正右负//，新：逆时针左右均负
	{
		delta_distance = -distance_diff;
		delta_oriention = -2.0f*distance_sum;		
//		OLED_ShowNumber(119, 0, 2);
	}
	else if((odom_left>0)&&(odom_right>0))       //左负右zheng//新：顺时针，左右均正
	{
		delta_distance = distance_diff;
		delta_oriention = 2.0f*distance_sum;
//		OLED_ShowNumber(119, 0, 3);
	}
	else
	{
		delta_distance=0;
		delta_oriention=0;
//		OLED_ShowNumber(119, 0, 4);
	}
    
	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坐标
	
//	OLED_ShowNumber(70,12,cos_);
//	OLED_ShowNumber(70,14,sin_);

	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;//计算出里程计角速度
	
//	OLED_ShowNumber(70,14, velocity_angular);
/*
//	//采样时间内走的角度
//	oriention_interval = delta_oriention / wheel_interval;
//	oriention = oriention + oriention_interval;//计算出里程计方向角
//	
//	//计算出里程计线速度
//	velocity_linear = distance_sum / dt;
//	velocity_angular = oriention_interval / dt;//计算出里程计角速度
//	//
//	
*/
	
    //方向角角度纠正
	if(oriention > pi)
	{
		oriention -= pi_2_1;
	}
	else
	{
		if(oriention < -pi)
		{
			oriention += pi_2_1;
		}
	}
	
}


void get_odometry(float *x, float *y, float *ori, float *vel_linear, float *vel_angular)
{
	*x = position_x;
	*y = position_y;
	*ori = oriention;
	*vel_linear = velocity_linear;
	*vel_angular = velocity_angular;
}

这个代码没怎么修改了。 

最后贴出个效果图，当然只是利用串口助手调试的图了，串口给发送速度指令，并接收回来里程计信息。对了，这里波特率以及一些奇偶校验位，停止位，数据位要和STM32串口设置的一样。还有就是这里是多条发送，否则的话接收会出现错误，这里需要注意一下。

 

我认为整体上比较重要的点就是上面所说的这些了，有些代码不是原创，但是在修改上也花了很大一部分时间，我这也算是初学，有错误的地方希望大家指正！在ROS小车底层部分说完了，可能也不是很详细，自己遇到的一些小问题和大家分享下。下一步打算在Linux下使用ROS让这个小车动起来！！