阿克曼小车仿真设计运动控制方案
控制/MCU
1878人已加入
描述
一、主要内容
结合阿克曼运动需求，本团队设计了阿克曼运动系统框图如图
主要包括阿克曼小车仿真设计和阿克曼小车实物实现。
图 1 阿克曼运动系统框图
1.1 阿克曼小车仿真设计
本节先介绍阿克曼小车模型，小车仿真设计包括SolidWorks和gazebo三维建模。
1.1.1 阿克曼小车
阿克曼小车是一款经典的车模，小车模型后轮是通过电机直驱，前轮通过舵机控制前轮转向角，前轮部分则是在模型中添加一个竖直的关节，使前轮能围绕此关节转动
1.1.2 SolidWorks三维模型
采用SolidWorks (2016版)设计软件搭建阿克曼小车三维模型如图 2图 3图 4，主体分为底板、电池、控制器、带编码电机的后轮、激光雷达、深度摄像头、阿克曼前轮转向7个部分如图 5。
各部分零件都定义了材料属性，选择取小车底盘中心为模型的原点，配置了小车JRDF文件见附件3。
目前可以导入到gazebo环境中，可以实现小车的前进与后退，但转向无法实现，原因分析由于阿克曼转向结构属于空间四连杆结构 (并联结构不支持)如图 6。
图2正等测
图3前视图
图4俯视图
图5爆炸视图
图6模型导入gazebo效果
1.1.3 gazebo三维建模
为了简化小车的运动，直接调入用阿克曼模型，简化小车的前后轮运动关系，搭配了里程计和摄像头，可以实现小车的转向、直行和后退。
小车分别导入到gazebo和rviz效果分别如图 7图 8建模过程见附件1文档
图 7 加载到gazebo
图8 加载到rvzi
1.2 阿克曼小车实物实现
小车底层搭载STM32F103系列单片机，运动控制算法采用阿克曼算法，解析后分别驱动舵机和编码电机，可以通过审口通讯实现上下位机的人机交互，方便调试我们设计了PS2手柄控制模式。
实现小车的实物制作如图 9。
图9小车实物
1.2.1控制器
小车控制器采用意法半导体STM32F103C6，是一款 ARM 32位 Cortex-M3 微控制器，2MHz 32B 闪存，10KB SRAM，PLL，嵌入式内部 RC 8MHz和 32KHz，实时时钟，嵌套中断控制器，省电式。
JTAG和SWD，2同步,具有输入捕捉、输出比和PWM的16位定时器、16位6通道高级定时器、2个16位看门狗定时器、SysTick定时器、SP112C、2个USART、USB2.0全速接口、CAN2.0B激活、2个12位10通道D转换器。
快速 /0 端口如图 10整体资源满足小车求，10使用情况详细说明，STM32核心板和底板原理图见附件1。
图 10 STM32F103引脚定义图
1.2.2 阿克曼运动算法
阿克曼转向是一种现代汽车的转向方式，在汽车转弯的时候，内外轮转过的角度不一样，内侧轮胎转弯半径小于外侧轮胎。理想的阿克曼转向如图 11，而本车模型采用反向的阿克曼模型。
图 11 理想的阿克曼转向
根据阿克曼转向几何设计转向机构，在车辆沿着弯道转弯时，利用四连杆的相等曲柄，可以使内侧轮的转向角比外侧轮大大约 2~4度，使四个轮子路径的圆心大致上交会于后轴的延长线上瞬时转向中心，从而让车辆可以顺畅的转弯。
阿克曼核心公式如下;
式中:B一汽车前外轮转角，a 一汽车前内轮转角，K一两主销中心距，L一轴距如图 12。
具体实现见附件2中control.c中Kinematic Analysis函数。
图 12 阿克曼数学模型
control.c
#include "control.h"
//#define T 0.245f
//#define L 0.29f
//#define K 14.00f
#define T 0.156f
#define L 0.1445f
#define K 622.8f
u8 Flag_Target,Flag_Change; //相关标志位
//float Voltage_Temp,Voltage_Count,Voltage_All; //电压采样相关变量
int j,sum;
/**************************************************************************
函数功能：小车运动数学模型
入口参数：速度和转角
返回 值：无
**************************************************************************/
void Kinematic_Analysis(float velocity,float angle)
{
Target_A=velocity*(1+T*tan(angle)/2/L);
Target_B=velocity*(1-T*tan(angle)/2/L); //后轮差速
Servo=SERVO_INIT+angle*K; //舵机转向
}
/**************************************************************************
函数功能：所有的控制代码都在这里面
定时中断触发
严格保证采样和数据处理的时间同步
**************************************************************************/
void Control(void)
{
oled_show(); //显示屏打开
Encoder_Left=Read_Encoder(2);
Encoder_Right=-Read_Encoder(3); //读取左右编码器
delay_ms(50); //=====延时等待稳定
if(Turn_Off(Voltage)==0&&Flag_Way==0)
{
jiexi();
Kinematic_Analysis(Velocity,Angle); //小车运动学分析
Motor_A=Target_A*20; //===计算电机A最终PWM
Motor_B=Target_B*20; //===计算电机B最终PWM
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Motor_A,Motor_B,Servo); //===赋值给PWM寄存器
}
else if(Turn_Off(Voltage)==0&&Flag_Way==1) //===如果不存在异常
{
Get_RC();
Kinematic_Analysis(Velocity,Angle); //小车运动学分析
Motor_A=Incremental_PI_Left(Encoder_Left,Target_A); //===速度闭环控制计算电机A最终PWM
Motor_B=Incremental_PI_Right(Encoder_Right,Target_B); //===速度闭环控制计算电机B最终PWM
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Motor_A,Motor_B,Servo); //===赋值给PWM寄存器
}
else Set_Pwm(0,0,SERVO_INIT); //===赋值给PWM寄存器
Voltage_Temp=Get_battery_volt(); //=====读取电池电压
Voltage_Count++; //=====平均值计数器
Voltage_All+=Voltage_Temp; //=====多次采样累积
if(Voltage_Count==10) Voltage=Voltage_All/10,Voltage_All=0,Voltage_Count=0;//=====求平均值
if(Flag_Show==0) Led_Flash(100);
else if(Flag_Show==1) Led_Flash(0); //led闪烁
Key(); //===扫描按键状态 单击双击可以改变小车运行状态
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：PWM
返回 值：无
**************************************************************************/
void Set_Pwm(int motor_a,int motor_b,int servo)
{
if(motor_a<0) PWMA2=7200,PWMA1=7200+motor_a;
else PWMA1=7200,PWMA2=7200-motor_a;
if(motor_b<0) PWMB1=7200,PWMB2=7200+motor_b;
else PWMB2=7200,PWMB1=7200-motor_b;
SERVO=servo;
}
/**************************************************************************
函数功能：限制PWM赋值
入口参数：幅值
返回 值：无
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=6900; //===PWM满幅是7200 限制在6900
if(Motor_A<-Amplitude) Motor_A=-Amplitude;
if(Motor_A>Amplitude) Motor_A=Amplitude;
if(Motor_B<-Amplitude) Motor_B=-Amplitude;
if(Motor_B>Amplitude) Motor_B=Amplitude;
if(Servo<(SERVO_INIT-500)) Servo=SERVO_INIT-500; //舵机限幅
if(Servo>(SERVO_INIT+500)) Servo=SERVO_INIT+500; //舵机限幅
}
/************************************************************************
函数功能：按键修改小车运行状态
入口参数：无
返回 值：无
**************************************************************************/
void Key(void)
{
u8 tmp,tmp2;
tmp=click();
// tmp=click_N_Double(50); //双击，双击等待时间500ms
if(tmp==1)Flag_Stop=!Flag_Stop;//单击控制小车的启停
//if(tmp==2)Flag_Show=!Flag_Show;//双击控制小车的显示状态
tmp2=Long_Press(); //长按
if(tmp2==1)Flag_Show=!Flag_Show;//控制小车的显示状态
}
/**************************************************************************
函数功能：异常关闭电机
入口参数：电压
返回 值：1：异常 0：正常
**************************************************************************/
u8 Turn_Off( int voltage)
{
u8 temp;
if(voltage<740||Flag_Stop==1)//电池电压低于11.1V关闭电机
{
temp=1;
PWMA1=0; //电机控制位清零
PWMB1=0; //电机控制位清零
PWMA2=0; //电机控制位清零
PWMB2=0; //电机控制位清零
}
else
temp=0;
return temp;
}
/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回 值：unsigned int
**************************************************************************/
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函数功能：增量PI控制器
入口参数：编码器测量值，目标速度
返回 值：电机PWM
根据增量式离散PID公式
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差 以此类推
pwm代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI_Left (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Target-Encoder; //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
int Incremental_PI_Right (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Target-Encoder; //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
/**************************************************************************
函数功能：通过指令对小车进行遥控
入口参数：PS2指令
返回 值：无
**************************************************************************/
void Get_RC(void)//PS2控制
{
int Yuzhi=2;
float LY,RX;
LY=PS2_LY-128; //计算偏差
RX=PS2_RX-128;
if(LY>-Yuzhi&&LY-Yuzhi&&RX
1.2.3 舵机驱动
脑机的控制由一个脉冲宽度调制信号(PWM)来实现，该信号由 stm32发出。
通常来说，够机的控制信号周期为 20ms的脉宽调制信号其中脉冲宽度从 0.5-2.5对应盘位置的 0-180度如图 13，呈线性变化，也就是说给它提供一定的脉宽。
它的输出轴就会保持在一定对应角度上，无论外界力矩怎么改变，直到给它提供另外宽度的脉冲信号，它才会改变输出角度到新的位置上。
图 13 舵机输出转角与输入信号脉冲宽度的关系
采用DSServo达盛金属舵机如图 14，型号为 DS3230，适用于航模、车模、船模及机器人的小型机，额定扭矩 3N.m，转动角度 270度。
具体定义见附件2中motor.c，功能实现见control.c中Kinematic Analysis函数
motor .c
#include "motor.h"
void Motor_PWM_Init(u16 arr,u16 psc)
{
RCC->APB1ENR|=1<<2; //TIM4时钟使能
RCC->APB2ENR|=1<<3; //PORTB时钟使能
GPIOB->CRL&=0X00FFFFFF; //PORTB6 7 8 9推挽输出
GPIOB->CRL|=0XBB000000; //PORTB6 7 8 9推挽输出
GPIOB->CRH&=0XFFFFFF00; //PORTB6 7 8 9推挽输出
GPIOB->CRH|=0X000000BB; //PORTB6 7 8 9推挽输出
TIM4->ARR=arr;//设定计数器自动重装值
TIM4->PSC=psc;//预分频器不分频
TIM4->CCMR1|=6<<4;//CH1 PWM1模式
TIM4->CCMR1|=6<<12; //CH2 PWM1模式
TIM4->CCMR2|=6<<4;//CH3 PWM1模式
TIM4->CCMR2|=6<<12; //CH4 PWM1模式
TIM4->CCMR1|=1<<3; //CH1预装载使能
TIM4->CCMR1|=1<<11;//CH2预装载使能
TIM4->CCMR2|=1<<3; //CH3预装载使能
TIM4->CCMR2|=1<<11;//CH4预装载使能
TIM4->CCER|=1<<0; //CH1输出使能
TIM4->CCER|=1<<4; //CH2输出使能
TIM4->CCER|=1<<8; //CH3输出使能
TIM4->CCER|=1<<12; //CH4输出使能
TIM4->CR1=0x80; //ARPE使能
TIM4->CR1|=0x01; //使能定时器
}
/***************** *********************************************************
函数功能：舵机PWM以及定时中断初始化
入口参数：入口参数：arr：自动重装值 psc：时钟预分频数
返回 值：无
**************************************************************************/
void Servo_PWM_Init(u16 arr,u16 psc)
{
RCC->APB2ENR|=1<<11; //使能TIM1时钟
RCC->APB2ENR|=1<<2; //PORTA时钟使能
GPIOA->CRH&=0XFFFF0FFF; //PORTA11复用输出
GPIOA->CRH|=0X0000B000; //PORTA11复用输出
TIM1->ARR=arr; //设定计数器自动重装值
TIM1->PSC=psc; //预分频器不分频
TIM1->CCMR2|=6<<12; //CH4 PWM1模式
TIM1->CCMR2|=1<<11; //CH4预装载使能
TIM1->CCER|=1<<12; //CH4输出使能
TIM1->BDTR |= 1<<15; //TIM1必须要这句话才能输出PWM
TIM1->CR1 = 0x80; //ARPE使能
TIM1->DIER|=1<<0; //允许更新中断
TIM1->CR1|=0x01; //使能定时器1
TIM1->CCR4=1500;
// MY_NVIC_Init(1,1,TIM1_UP_IRQn,2);
}
1.2.4 编码电机驱动
直流减速电机接口方式如图 15，是一个带编码器的直流减速电机，编码器的作用是测速。
一般包括六个接线端子，电机电源输入 M1和电机电源输入 M2是直流电机引脚，电机的旋转和速度调节只需这两个引脚即可。剩下中间的四个引脚是编码器。
图 15 编码电机
STM32F103的高级控制定时器 TIM1和 TIM8在基本定时器的基础上引入了外部脚，可以输入捕获和输出比较功能。
高级定时器能够完成输入捕获和输出比较的功能，输出比较包括翻转、强制为有效电平、PWM1和 PWM2等模式，其中 PWM模时最常用的。
电机采用定时器TIM8，两路编码器分别采用定时器TIM2和TIM3。
电机具体定义见附件2中motr.c，编码器具体定义见附件2中encoder.c，功能实现凡control.c中Kinematic Analysis函数。
encoder.c
#include "encoder.h"
#include "stm32f10x_gpio.h"
/**************************************************************************
函数功能：把TIM2初始化为编码器接口模式
入口参数：无
返回 值：无
**************************************************************************/
void Encoder_Init_TIM2(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);// 需要使能AFIO时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//使能定时器2的时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PA端口时钟
//RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//使能PB端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; //设定计数器自动重装值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上计数
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIM2, &TIM_ICInitStructure);
TIM_ClearFlag(TIM2, TIM_FLAG_Update);//清除TIM的更新标志位
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
//Reset counter
TIM_SetCounter(TIM2,0);
TIM_Cmd(TIM2, ENABLE);
}
/**************************************************************************
函数功能：把TIM3初始化为编码器接口模式
入口参数：无
返回 值：无
**************************************************************************/
void Encoder_Init_TIM3(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_ICInitTypeDef TIM_ICInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);//使能定时器3的时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能PB端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
TIM_TimeBaseStructure.TIM_Prescaler = 0x0; // 预分频器
TIM_TimeBaseStructure.TIM_Period = ENCODER_TIM_PERIOD; //设定计数器自动重装值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;//选择时钟分频：不分频
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;TIM向上计数
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);//使用编码器模式3
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_ICFilter = 10;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
TIM_ClearFlag(TIM3, TIM_FLAG_Update);//清除TIM的更新标志位
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
//Reset counter
TIM_SetCounter(TIM3,0);
TIM_Cmd(TIM3, ENABLE);
}
/**************************************************************************
函数功能：单位时间读取编码器计数
入口参数：定时器
返回 值：速度值
**************************************************************************/
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
case 3: Encoder_TIM= (short)TIM3 -> CNT; TIM3 -> CNT=0;break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
/**************************************************************************
函数功能：TIM3中断服务函数
入口参数：无
返回 值：无
**************************************************************************/
void TIM3_IRQHandler(void)
{
if(TIM3->SR&0X0001)//溢出中断
{
}
TIM3->SR&=~(1<<0);//清除中断标志位
}
/**************************************************************************
函数功能：TIM2中断服务函数
入口参数：无
返回 值：无
**************************************************************************/
void TIM2_IRQHandler(void)
{
if(TIM2->SR&0X0001)//溢出中断
{
}
TIM2->SR&=~(1<<0);//清除中断标志位
}
1.2.5 串口通讯
小车采用有线的串口与电脑通讯,来模拟驱动板通过串口与工控机NANO板完成信息交互，使用STM32F103串口1实现数据的传输。
对应输出脚为 TXD-PA10，RXD-PA9，编写中动板通过审口与工控机,NANO板完成信息交互。
通过轮询的方式将驱动板采集到的传感器数据等发送给工控机，通过中断的方式接收工控机发来的串口控制量从而完成小车的运动控制。
通过CH340G芯片完成电平转换，CH340是一人USB总线的转换芯片，实现 USB转由口，CH340G州片中的 TXD和 RXD轻舟机器人学习教程5。
STM32F103的USART1RXD和USART1TXD相连接。
首先配置口参数及中断优先级，然后编写发送函数，最后使能接收中断。
具体定义见附件2中usart.c，功能实现见DataScope DP.c中USART TX和jiexi函数。
usart.c
#include "usart.h"
//加入以下代码,支持printf函数,而不需要选择use MicroLIB
#if 1
#pragma import(__use_no_semihosting)
//标准库需要的支持函数
struct __FILE
{
int handle;
/* Whatever you require here. If the only file you are using is */
/* standard output using printf() for debugging, no file handling */
/* is required. */
};
/* FILE is typedef’ d in stdio.h. */
FILE __stdout;
//定义_sys_exit()以避免使用半主机模式
_sys_exit(int x)
{
x = x;
}
//重定义fputc函数
int fputc(int ch, FILE *f)
{
while((USART3->SR&0X40)==0);
USART3->DR = (u8) ch;
return ch;
}
#endif
u8 Usart3_Receive=0X5A;
void usart1_init(u32 bound)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1|RCC_APB2Periph_GPIOA, ENABLE); //使能USART1，GPIOA时钟
//USART1_TX GPIOA.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);//初始化GPIOA.9
//USART1_RX GPIOA.10初始化
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;//PA10
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.10
//USART 初始化设置
//UsartNVIC 配置
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0 ;//抢占优先级
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //子优先级
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
USART_InitStructure.USART_BaudRate = bound;//串口波特率
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); //初始化串口1
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启串口接受中断
USART_Cmd(USART1, ENABLE); //使能串口1
}
/**************************************************************************
函数功能：串口1接收中断
入口参数：无
返回 值：无
**************************************************************************/
int USART1_IRQHandler(void)
{
if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) //接收到数据
{
u8 temp;
static u8 count,last_data,last_last_data,Usart_ON_Count;
if(Usart_ON_Flag==0)
{
if(++Usart_ON_Count>10)Usart_ON_Flag=1;
}
temp=USART1->DR;
if(Usart_Flag==0)
{
if(last_data==0x5a&&last_last_data==0xa5)
Usart_Flag=1,count=0;
}
if(Usart_Flag==1)
{
Urxbuf[count]=temp;
count++;
if(count==8)Usart_Flag=0;
}
last_last_data=last_data;
last_data=temp;
}
return 0;
}
//
/**************************实现函数**********************************************
*功 能: usart3发送一个字节
*********************************************************************************/
void usart3_send(u8 data)
{
USART3->DR = data;
while((USART3->SR&0x40)==0);
}
/**************************************************************************
函数功能：串口3初始化
入口参数：bound:波特率
返回 值：无
**************************************************************************/
void usart3_init(u32 bound)
{
//GPIO端口设置
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);// 需要使能AFIO时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); //使能GPIO时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); //使能USART时钟
GPIO_PinRemapConfig(GPIO_PartialRemap_USART3, ENABLE);//引脚重映射
//USART_TX
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; //C10
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_Init(GPIOC, &GPIO_InitStructure);
//USART_RX
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;//PC11
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
GPIO_Init(GPIOC, &GPIO_InitStructure);
//UsartNVIC 配置
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0 ;//抢占优先级
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; //子优先级
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道使能
NVIC_Init(&NVIC_InitStructure); //根据指定的参数初始化VIC寄存器
//USART 初始化设置
USART_InitStructure.USART_BaudRate = bound;//串口波特率
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(USART3, &USART_InitStructure); //初始化串口3
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//开启串口接受中断
USART_Cmd(USART3, ENABLE); //使能串口3
}
/**************************************************************************
函数功能：串口3接收中断
入口参数：无
返回 值：无
**************************************************************************/
void USART3_IRQHandler(void)
{
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) //接收到数据
{
static u8 Flag_PID,i,j,Receive[50];
static float Data;
Usart3_Receive=USART3->DR;
if(Usart3_Receive>=0x41&&Usart3_Receive<=0x48)
Flag_Direction=Usart3_Receive-0x40;
else
Flag_Direction=0;
//以下是与APP调试界面通讯
if(Usart3_Receive==0x7B) Flag_PID=1; //APP参数指令起始位
if(Usart3_Receive==0x7D) Flag_PID=2; //APP参数指令停止位
if(Flag_PID==1) //采集数据
{
Receive[i]=Usart3_Receive;
i++;
}
if(Flag_PID==2) //分析数据
{
if(Receive[3]==0x50) PID_Send=1;
else if(Receive[1]!=0x23)
{
for(j=i;j>=4;j--)
{
Data+=(Receive[j-1]-48)*pow(10,i-j);
}
switch(Receive[1])
{
case 0x30: RC_Velocity=Data;break;
case 0x31: Velocity_KP=Data;break;
case 0x32: Velocity_KI=Data;break;
case 0x33: break;
case 0x34: break;
case 0x35: break;
case 0x36: break;
case 0x37: break; //预留
case 0x38: break; //预留
}
}
Flag_PID=0;//相关标志位清零
i=0;
j=0;
Data=0;
memset(Receive, 0, sizeof(u8)*50);//数组清零
}
}
}
Datascope DP.c
#include "DataScope_DP.h"
unsigned char DataScope_OutPut_Buffer[42] = {0}; //串口发送缓冲区
int send_cnt = 0;
static u8 Send_rasberry[60];
int re_Encoder_Left,re_Encoder_Right;
int Distance_A,Distance_B,Distance_C,Distance_D;
u8 Urxbuf[8],Usart_Flag,x=0;
short accelX,accelY,accelZ,gyroX,gyroY,gyroZ,magX,magY,magZ;
void USART_TX(void)
{
Send_rasberry[0] = 0xA5; // 数据头，固定值
Send_rasberry[1] = 0x5A; // 数据头，固定值
Send_rasberry[2] = 0x33; // 发送数据的长度
re_Encoder_Left = -Encoder_Left;
re_Encoder_Right = -Encoder_Right;
for(send_cnt=0; send_cnt<4; send_cnt++) //左编码器增量值
{
Send_rasberry[3+send_cnt] = ((unsigned char *)&re_Encoder_Left)[send_cnt];
}
for(send_cnt=0; send_cnt<4; send_cnt++) //右编码器增量值
{
Send_rasberry[7+send_cnt] = ((unsigned char *)&re_Encoder_Right)[send_cnt];
}
for(send_cnt=0; send_cnt<4; send_cnt++) //电池电压采样
{
Send_rasberry[11+send_cnt] = ((unsigned char *)&Voltage)[send_cnt];
}
for(send_cnt=0; send_cnt<2; send_cnt++) // X 轴加速度计值
{
Send_rasberry[15+send_cnt] = ((unsigned char *)&accelX)[send_cnt];
}
for(send_cnt=0; send_cnt<2; send_cnt++) // Y 轴加速度计值
{
Send_rasberry[17+send_cnt] = ((unsigned char *)&accelY)[send_cnt];
}
for(send_cnt=0; send_cnt<2; send_cnt++) // Y 轴加速度计值
{
Send_rasberry[19+send_cnt] = ((unsigned char *)&accelZ)[send_cnt];
}
//send gyro X Y Z
for(send_cnt=0; send_cnt<2; send_cnt++) // X 轴角速度值
{
Send_rasberry[21+send_cnt] = ((unsigned char *)&gyroX)[send_cnt];
}
for(send_cnt=0; send_cnt<2; send_cnt++) // Y 轴角速度值
{
Send_rasberry[23+send_cnt] = ((unsigned char *)&gyroY)[send_cnt];
}
for(send_cnt=0; send_cnt<2; send_cnt++) // Z 轴角速度值
{
Send_rasberry[25+send_cnt] = ((unsigned char *)&gyroZ)[send_cnt];
}
//send MAG X Y Z
for(send_cnt=0; send_cnt<2; send_cnt++) // X 轴磁力计值
{
Send_rasberry[27+send_cnt] = ((unsigned char *)&magX)[send_cnt];
}
for(send_cnt=0; send_cnt<2; send_cnt++) // Y 轴磁力计值
{
Send_rasberry[29+send_cnt] = ((unsigned char *)&magY)[send_cnt];
}
for(send_cnt=0; send_cnt<2; send_cnt++) // Z 轴磁力计值
{
Send_rasberry[31+send_cnt] = ((unsigned char *)&magZ)[send_cnt];
}
//send ultrasonic A B C D
for(send_cnt=0; send_cnt<4; send_cnt++) // 超测量距离值 A
{
Send_rasberry[33+send_cnt] = ((unsigned char *)&Distance_A)[send_cnt];
}
for(send_cnt=0; send_cnt<4; send_cnt++) // 超测量距离值 B
{
Send_rasberry[37+send_cnt] = ((unsigned char *)&Distance_B)[send_cnt];
}
for(send_cnt=0; send_cnt<4; send_cnt++) // 超测量距离值 C
{
Send_rasberry[41+send_cnt] = ((unsigned char *)&Distance_C)[send_cnt];
}
for(send_cnt=0; send_cnt<4; send_cnt++) // 超测量距离值 D
{
Send_rasberry[45+send_cnt] = ((unsigned char *)&Distance_D)[send_cnt];
}
Send_rasberry[50]=x;
x++;
//send Send_rasberry
UartASendStr(Send_rasberry,51);
memset(Send_rasberry, 0, sizeof(u8)*51); //数组清零
}
void UartASendStr (u8 *pucStr, u8 ulNum)
{
u8 i;
for(i = 0;i
1.2.6 PS2通讯
为了方便调试，设计了PS2手柄控制。
PS2采用的是SPI通信协议如图 16，SPI是串行外设接口的缩写，是一种高速的、全双工、同步的通信总线，并且在芯片的管脚上只占用四根线(D、DO、CS、CLK)，节约了芯片的管脚，同时为PCB的布局上节省空间。
具体定义见附件2中pstwo.c，功能实现风control.c中Get RC函数。
pstwo.c
#include "pstwo.h"
#define DELAY_TIME delay_us(5);
u16 Handkey; // 按键值读取，零时存储。
u8 Comd[2]={0x01,0x42}; //开始命令。请求数据
u8 Data[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; //数据存储数组
u16 MASK[]={
PSB_SELECT,
PSB_L3,
PSB_R3 ,
PSB_START,
PSB_PAD_UP,
PSB_PAD_RIGHT,
PSB_PAD_DOWN,
PSB_PAD_LEFT,
PSB_L2,
PSB_R2,
PSB_L1,
PSB_R1 ,
PSB_GREEN,
PSB_RED,
PSB_BLUE,
PSB_PINK
}; //按键值与按键明
//c2改成b15 c1改成b14 c3改成c13 a4改成a12
void PS2_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC, ENABLE); //使能端口时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //上拉输入
GPIO_Init(GPIOC, &GPIO_InitStructure); //根据设定参数初始化GPIO
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14|GPIO_Pin_15; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //50M
GPIO_Init(GPIOB, &GPIO_InitStructure); //根据设定参数初始化GPIOB
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_12; //端口配置
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //50M
GPIO_Init(GPIOA, &GPIO_InitStructure); //根据设定参数初始化GPIOA
}
//向手柄发送命令
void PS2_Cmd(u8 CMD)
{
volatile u16 ref=0x01;
Data[1] = 0;
for(ref=0x01;ref<0x0100;ref<<=1)
{
if(ref&CMD)
{
DO_H; //输出一位控制位
}
else DO_L;
CLK_H; //时钟拉高
DELAY_TIME;
CLK_L;
DELAY_TIME;
CLK_H;
if(DI)
Data[1] = ref|Data[1];
}
delay_us(16);
}
//判断是否为红灯模式,0x41=模拟绿灯，0x73=模拟红灯
//返回值；0，红灯模式
// 其他，其他模式
u8 PS2_RedLight(void)
{
CS_L;
PS2_Cmd(Comd[0]); //开始命令
PS2_Cmd(Comd[1]); //请求数据
CS_H;
if( Data[1] == 0X73) return 0 ;
else return 1;
}
//读取手柄数据
void PS2_ReadData(void)
{
volatile u8 byte=0;
volatile u16 ref=0x01;
CS_L;
PS2_Cmd(Comd[0]); //开始命令
PS2_Cmd(Comd[1]); //请求数据
for(byte=2;byte<9;byte++) //开始接受数据
{
for(ref=0x01;ref<0x100;ref<<=1)
{
CLK_H;
DELAY_TIME;
CLK_L;
DELAY_TIME;
CLK_H;
if(DI)
Data[byte] = ref|Data[byte];
}
delay_us(16);
}
CS_H;
}
//对读出来的PS2的数据进行处理,只处理按键部分
//只有一个按键按下时按下为0， 未按下为1
u8 PS2_DataKey()
{
u8 index;
PS2_ClearData();
PS2_ReadData();
Handkey=(Data[4]<<8)|Data[3]; //这是16个按键 按下为0， 未按下为1
for(index=0;index<16;index++)
{
if((Handkey&(1<<(MASK[index]-1)))==0)
return index+1;
}
return 0; //没有任何按键按下
}
//得到一个摇杆的模拟量 范围0~256
u8 PS2_AnologData(u8 button)
{
return Data[button];
}
//清除数据缓冲区
void PS2_ClearData()
{
u8 a;
for(a=0;a<9;a++)
Data[a]=0x00;
}
/******************************************************
Function: void PS2_Vibration(u8 motor1, u8 motor2)
Description: 手柄震动函数，
Calls: void PS2_Cmd(u8 CMD);
Input: motor1:右侧小震动电机 0x00关，其他开
motor2:左侧大震动电机 0x40~0xFF 电机开，值越大 震动越大
******************************************************/
void PS2_Vibration(u8 motor1, u8 motor2)
{
CS_L;
delay_us(16);
PS2_Cmd(0x01); //开始命令
PS2_Cmd(0x42); //请求数据
PS2_Cmd(0X00);
PS2_Cmd(motor1);
PS2_Cmd(motor2);
PS2_Cmd(0X00);
PS2_Cmd(0X00);
PS2_Cmd(0X00);
PS2_Cmd(0X00);
CS_H;
delay_us(16);
}
//short poll
void PS2_ShortPoll(void)
{
CS_L;
delay_us(16);
PS2_Cmd(0x01);
PS2_Cmd(0x42);
PS2_Cmd(0X00);
PS2_Cmd(0x00);
PS2_Cmd(0x00);
CS_H;
delay_us(16);
}
//进入配置
void PS2_EnterConfing(void)
{
CS_L;
delay_us(16);
PS2_Cmd(0x01);
PS2_Cmd(0x43);
PS2_Cmd(0X00);
PS2_Cmd(0x01);
PS2_Cmd(0x00);
PS2_Cmd(0X00);
PS2_Cmd(0X00);
PS2_Cmd(0X00);
PS2_Cmd(0X00);
CS_H;
delay_us(16);
}
//发送模式设置
void PS2_TurnOnAnalogMode(void)
{
CS_L;
PS2_Cmd(0x01);
PS2_Cmd(0x44);
PS2_Cmd(0X00);
PS2_Cmd(0x01); //analog=0x01;digital=0x00 软件设置发送模式
PS2_Cmd(0x03); //Ox03锁存设置，即不可通过按键“MODE”设置模式。
//0xEE不锁存软件设置，可通过按键“MODE”设置模式。
PS2_Cmd(0X00);
PS2_Cmd(0X00);
PS2_Cmd(0X00);
PS2_Cmd(0X00);
CS_H;
delay_us(16);
}
//振动设置
void PS2_VibrationMode(void)
{
CS_L;
delay_us(16);
PS2_Cmd(0x01);
PS2_Cmd(0x4D);
PS2_Cmd(0X00);
PS2_Cmd(0x00);
PS2_Cmd(0X01);
CS_H;
delay_us(16);
}
//完成并保存配置
void PS2_ExitConfing(void)
{
CS_L;
delay_us(16);
PS2_Cmd(0x01);
PS2_Cmd(0x43);
PS2_Cmd(0X00);
PS2_Cmd(0x00);
PS2_Cmd(0x5A);
PS2_Cmd(0x5A);
PS2_Cmd(0x5A);
PS2_Cmd(0x5A);
PS2_Cmd(0x5A);
CS_H;
delay_us(16);
}
//手柄配置初始化
void PS2_SetInit(void)
{
PS2_ShortPoll();
PS2_ShortPoll();
PS2_ShortPoll();
PS2_EnterConfing(); //进入配置模式
PS2_TurnOnAnalogMode(); //“红绿灯”配置模式，并选择是否保存
//PS2_VibrationMode(); //开启震动模式
PS2_ExitConfing(); //完成并保存配置
}
二、设计思路和流程
小车通过前期的仿真设计，结合实验室材料情况，搭建的小车实物。
为了方演示串口通讯和PS二手柄控制，对此设计了控制程序流程图如图 17，来引导小车控制程序的设计。
图 17 控制程序流程图
控制程序流程图理解如下，首先初始化小车，把电机舵机进行归零处理，定义对应的传感器，初始化OLED 屏幕。显示屏会提示模式选怪，模式一是串口控制模式。
点击确认后，小车会自动一传小车状杰信息数提给上位机，一位机可以发送指令小车接收后会执行对应的运动控制，会进行一次判断小车是否异常。
如果正常会进行二次判断小车是否需要结束运动，那么小车与上位机进行正常的人机交互。
通讨串口助手和单片机通讯并下发电机运动指令，片机接收串口助手的指令并驱动电机的正反转和差速控制。
单片机接收串口助手的指令控制能机和电机基于阿克曼运动算法的转动。
如果是最后会初始化小车归零结束程序。模式二是PS2控制模式，会通过显示屏显示小车的运动数据，用户可以通过PS二手柄进行遥控小车。
同样会进行二次判断，当用户发出结束小车运动指令时，小车会归零，最终结束程序。
小车完整程序见附件2中轻舟驱动板demo-OLED8.0
2.1 串口控制模式
串口控制和PS2控制如图 18。在串口控制模式中，首先调用函数，接下来显示屏打开，读取左右编码器，延时消除不稳定。
接下来进行数据解析、卡尔曼算法、限制pwm幅、输出对应的电机和机。
最后电机电压显示和单机单双击可以改变小车的运行状态，以此循环往复，实现人机交互。具体串口协议见附件3中阿克曼运动串口协议。
图 18 串口和PS2控制
2.2 PS2控制模式
PS2控制模式中与串口控制模式有不同的在于PS2数据解析，其他与串口通讯一致。
三、设计效果与呈现
本节综合前面阿克曼小车的仿直设计，实物设计和程席设计。
最后到了实验设计效果展示的环节，本环节由由口控制演示和PS2演示控制两部分组成。
具体的演示见附件4的演示视频。
3.1 串口控制演示
通过安卓数据线连接小车串口1.。本次审口控制演示采用的是友善串口助手如图 19。
串口设置端口连接com8，波特率115200，数据为位，无校验位，一位停止位，无流控，接收设置采用hex自动换行显示发送和显示时间，接收采用hex。串口收发控制如图 20图 21
图 19串口设置
图 20 左转控制
图 21 停止控制
3.2 PS2控制演示
在PS2控制中，首先打开手柄的开关电源，然后按一下stat开始控制小车。
通过摇动左右摇杆实现一个左右前进控制，其中左摇杆前进是油门儿，右摇杆是控制机转向。
PS2控制操作如图 22。
图 22 PS2控制操作
演示视频:
编辑：黄飞
声明：本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人，不代表电子发烧友网立场。文章及其配图仅供工程师学习之用，如有内容侵权或者其他违规问题，请联系本站处理。
举报投诉
-
candance如何仿真阿 ,不会阿?2017-03-04 0
-
用labview怎么写控制小车的连续运动？2019-07-29 0
-
小车电机和舵机控制相关资料分享2021-07-05 0
-
通过STM32输出PWM波给驱动模块进行小车运动状态的控制2021-08-16 0
-
怎么实现两种智能小车的建模与仿真？2021-08-30 0
-
小车运动及速度控制设计2009-03-23 3104
-
52单片机对智能小车的控制设计2019-08-14 5137
-
PLC小车往返运动编程案例分享2020-09-19 52971
-
基于PLC控制的小车往返运动原理图2021-06-21 3652
-
大创：小车运动实验2022-01-13 182
-
阿克曼小车仿真运动控制设计方案2022-12-31 1876
-
运动控制卡能仿真吗？2023-02-22 1308
-
基于扩展卡尔曼滤波EKF的无感控制+Matlab/Simulink仿真案例2023-06-08 4318
全部0条评论
快来发表一下你的评论吧 !
