机器人进阶学习（一）---基于stm32的底层搭建，与上位机通讯及base_control编写

最近一直忙于底层设计，以及上位机的搭建，现在终于告一段落，决定把整个搭建过程写传来。方便日后查阅，同时回馈各路博客上的援助。

底层：
单片机：stm32f103zet6
电机：额定电压24v,60w,的maxon motor,减速比169:1,减速后50r/m
编码器：HEDL 5540#A02 的500线编码器
通讯方式：串口

小车控制思想

控制电机转动及调速
电机的控制我们分为两部分，一部分为电机转动方向的控制，另一个为电机转速的控制。
电机转动的方向可以通过GPIO的引脚控制驱动上的IN1-1N4,如IN_1=1，IN_2=0 时，电机正转；IN_1=0，IN_B=2时，电机反转；IN_1=0，IN_2=0 时，电机停止。
电机速度的控制则需要一个PWM输出引脚，与驱动上的使能引脚相连，如对于电机1,可以通过一段时间内控制ENA=1,调节占空比调节速度。

PID控制
如果我们想控制小车以一米每秒的速度做直线，但由于地面的摩擦阻力的影响，会造成左右轮速度与我们想控制的速度不同，所以会走不直，这时我们就需要加入PID控制。
PID控制的思想就是我实时的把轮子真正的速度采集回来和控制的速度对比，差则补，多则减。
这样基本就可以实现理想控制。

小车转弯控制。

    一般我们要是想控制小车以多少的速度前进或者后退，我们只需要PID控制两个轮子的速度一致就可以基本做到。如果要控制小车以确定的速度以及固定的角度转弯，则需要了解下差速轮的运动控制。下面从三个公式开始简单说下，大家最好自己推导下。

在这里，用v表示线速度，w表示角速度，r表示半径。

在这里，首先要说明几个坐标系
1. 里程计坐标系，是以机器人上电时刻为原点，进行坐标和角度的累加。
2. 机器人坐标系，是以机器人的中心轴为z轴，以逆时针旋转方向为旋转坐标系的正方向，以右手坐标系为参考系。
我们现在用极限的思想去解决运动距离以及角度的描述：

当我们在极短的单位时间内移动很短的距离，转动很小的角度时，根据微元思想，轮子的运动可以看作直线。
∆d=(∆d1+∆d2)/2 //距离变化
∆Ɵ=∆d1-∆d2)/L //角度变化，L为轴距

做第二个轮子反向延长线，可以看到等腰三角形，两个内角之和等于一个不相邻的外角，所以得到一个∆Ɵ/2的变化角
这样可以得到里程计坐标系下的变化值。
∆x= ∆d*cos(Ɵ+ ∆Ɵ/2)
∆y= ∆d*sin(Ɵ+ ∆Ɵ/2)
要求∆d，就要求∆d1,∆d2
我们的编码器是500线的，与169:1的减速器相连，通过A，B相输入，一个周期检测上升沿，下降沿一共四个，这样电机转一圈产生M=500*169*4 个脉冲。∆d1,∆d2可以通过∆t时间内，编码器产生的脉冲计算。若在∆t时间内，左轮产生脉冲个数Nl,右轮产生脉冲个数为Nr,轮子直径为D，
∆d1=Nl*3.14*D /M，∆d2=Nr*3.14*D/M
∆d=(Nr+Nl)*3.14D/2M,∆Ɵ=(Nr-Nl)*3.14D/L*M
基本上所有的差速轮的里程计坐标下的数据计算都是基于上述思想。

（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

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;
    }
}

}

主要参考：
http://blog.csdn.net/david_han008/article/details/78573616