五、STM32ZET6控制程序设计
单片机作为下位机，功能是：
（1）接收树莓派通过串口发来的速度信息，并将速度信息转换成左右轮的速度，并控制电机转动。
（2）接收imu的姿态信息，并将位姿发送给树莓派（该部分功能尚未在代码中实现，因为目前还未用到imu的信息进行融合）
main.cpp代码：
#include "stm32f10x.h" #include "delay.h" #include "sys.h" #include "math.h" #include "robot.h" /************************************************ 移动机器人运动控制程序 ************************************************/ union recieveData //接收到的数据 { float d;//左右轮速度 unsigned char data[4]; }leftdata, rightdata;//接收的左右轮速度 float odometry_data_right = 0, odometry_left = 0;//串口得到的左右轮速度 int pulse_per_rev = 25600;//电机每圈需要的脉冲数 int wheel_d = 125;//电机直径mm float pi = 3.1415;//圆周率 int arr_right = 999; int psc_right = 199; int arr_left = 999; int psc_left = 199; int main(void) { NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级 GpioInit();//初始化IO Usart_Init(115200);//初始化串口 while(1) { if(USART_RX_STA&0X8000) { int i; for(i=0;i<4;i++) { rightdata.data[i] = USART_RX_BUF[i]; leftdata.data[i] = USART_RX_BUF[i+4]; } USART_RX_STA = 0;//清除接收标志位 rightdata.d = (rightdata.d/((float)(pi*wheel_d)))*pulse_per_rev;//速度转换成pulse/s的形式 leftdata.d = (leftdata.d/((float)(pi*wheel_d)))*pulse_per_rev;//速度转换成pulse/s的形式 //右车轮控制 if(rightdata.d > 0)//右轮正转 { GPIO_ResetBits(GPIOA, GPIO_Pin_15);//方向控制端口，正转 arr_right = 72000000.0/rightdata.d/((float)(psc_right+1));//计算得到arr值 TIM3_rightwheel_Init(arr_right, psc_right);//初始化右轮定时器 } else if(rightdata.d < 0) { GPIO_SetBits(GPIOA, GPIO_Pin_15);//方向控制端口，反转 rightdata.d = -rightdata.d; arr_right = 72000000.0/rightdata.d/((float)(psc_right+1));//计算得到arr值 TIM3_rightwheel_Init(arr_right, psc_right);//初始化右轮定时器 } else TIM_Cmd(TIM3, DISABLE);//失能右轮定时器 //左车轮控制 if(leftdata.d > 0)//左轮正转 { GPIO_ResetBits(GPIOB, GPIO_Pin_4);//方向控制端口，正转 arr_left = 72000000.0/leftdata.d/((float)(psc_left+1));//计算得到arr值 TIM4_leftwheel_Init(arr_left, psc_left);//初始化左轮定时器 } else if(leftdata.d < 0) { GPIO_SetBits(GPIOB, GPIO_Pin_4);//方向控制端口，反转 leftdata.d = -leftdata.d; arr_left = 72000000.0/leftdata.d/((float)(psc_left+1));//计算得到arr值 TIM4_leftwheel_Init(arr_left, psc_left);//初始化左轮定时器 } else TIM_Cmd(TIM4, DISABLE);//失能左轮定时器 } } }
robot.h代码：
#include "robot.h" #include "sys.h" #include "math.h" //IO口初始化函数 void GpioInit(void) { GPIO_InitTypeDef GPIO_InitStructure;//创建io口初始化结构体 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|//开启A口时钟 RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC|//开启B&C口时钟 RCC_APB2Periph_GPIOD|RCC_APB2Periph_GPIOE|//开启D&E口时钟 RCC_APB2Periph_GPIOF|RCC_APB2Periph_GPIOG,ENABLE);//开启F&G口时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE); GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;//右轮方向控制端口 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); GPIO_ResetBits(GPIOA, GPIO_Pin_15); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;//左轮方向控制端口 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_ResetBits(GPIOB, GPIO_Pin_4); } //右侧车轮定时器初始化 void TIM3_rightwheel_Init(u16 arr, u16 psc) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);//使能定时器3时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);//使能GPIO外设和AFIO复用功能模块时钟 GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE);//Time3部分重映射 TIM3_CH2->PB5 //设置该引脚为复用输出功能，输出TIM3_CH2的pwm脉冲波形 GPIOB.Pin5 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_5;//TIM_CH2 GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;//复用推挽输出 GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; GPIO_Init(GPIOB,&GPIO_InitStructure);//初始化GPIO //初始化TIM3 TIM_TimeBaseStructure.TIM_Period = arr;//设置在下一个更新事件装入活动的自动重装载寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler = psc;//设置用来作为TIMx时钟频率除数的预分频值 TIM_TimeBaseStructure.TIM_ClockDivision = 0;//设置时钟分割：TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//TIM向上计数模式 TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);//根据TIM_TimeBaseStructure中指定的参数初始化TIMx的时间基数单位 //初始化TIM3.CH2的PWM模式 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;//选择定时器模式：TIM脉冲宽度调制模式2 TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;//比较输出使能 TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;//输出极性：TIM输出比较极性高 TIM_OC2Init(TIM3, &TIM_OCInitStructure);//根据T指定的参数初始化外设TIM3 OC2 TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); TIM_SetCompare2(TIM3, (int)(arr/2));//设置比较值，决定脉冲占空比 TIM_Cmd(TIM3, ENABLE);//默认状态失能TIM3 } //左侧车轮定时器初始化 void TIM4_leftwheel_Init(u16 arr, u16 psc) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);//使能定时器4时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE);//使能GPIO外设和AFIO复用功能模块时钟 //设置该引脚为复用输出功能，输出TIM4_CH2的pwm脉冲波形 GPIOB.Pin7 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_7;//TIM_CH2 GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;//复用推挽输出 GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; GPIO_Init(GPIOB,&GPIO_InitStructure);//初始化GPIO //初始化TIM4 TIM_TimeBaseStructure.TIM_Period = arr;//设置在下一个更新事件装入活动的自动重装载寄存器周期的值 TIM_TimeBaseStructure.TIM_Prescaler = psc;//设置用来作为TIMx时钟频率除数的预分频值 TIM_TimeBaseStructure.TIM_ClockDivision = 0;//设置时钟分割：TDTS = Tck_tim TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//TIM向上计数模式 TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);//根据TIM_TimeBaseStructure中指定的参数初始化TIMx的时间基数单位 //初始化TIM4.CH2的PWM模式 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;//选择定时器模式：TIM脉冲宽度调制模式2 TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;//比较输出使能 TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;//输出极性：TIM输出比较极性高 TIM_OC2Init(TIM4, &TIM_OCInitStructure);//根据T指定的参数初始化外设TIM4 OC2 TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); TIM_SetCompare2(TIM4, (int)(arr/2));//设置比较值，决定脉冲占空比 TIM_Cmd(TIM4, ENABLE);//默认状态失能TIM4 } void Usart_Init(u32 bound) { GPIO_InitTypeDef GPIO_InitStructure; USART_InitTypeDef USART_InitStructure; NVIC_InitTypeDef NVIC_InitStructure; RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1|RCC_APB2Periph_GPIOA,ENABLE); //USART1_TX GPIOA.9 GPIO_InitStructure.GPIO_Pin=GPIO_Pin_9;//PA.9 GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;//复用推挽输出 GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; 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_InitStructure.GPIO_Speed=GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.10 //USART1 NVIC配置 NVIC_InitStructure.NVIC_IRQChannel=USART1_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;//抢占优先级3 NVIC_InitStructure.NVIC_IRQChannelSubPriority=3;//子优先级3 NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;//IRQ通道使能 NVIC_Init(&NVIC_InitStructure);//根据指定的参数初始化NVIC寄存器 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中断处理函数 u8 USART_RX_BUF[200]; //接收缓冲,最大200个字节. //接收状态 //bit15， 接收完成标志 //bit14， 接收到0x0d //bit13~0， 接收到的有效字节数目 u16 USART_RX_STA=0;//接收状态标记 void USART1_IRQHandler(void) { u8 Res; if(USART_GetFlagStatus(USART1, USART_IT_RXNE) != RESET) { Res =USART_ReceiveData(USART1); //读取接收到的数据 if((USART_RX_STA&0x8000)==0)//接收未完成 { if(USART_RX_STA&0x4000)//接收到了0x0d { if(Res!=0x0a)USART_RX_STA=0;//接收错误,重新开始 else USART_RX_STA|=0x8000; //接收完成了 } else //还没收到0X0D { if(Res==0x0d)USART_RX_STA|=0x4000; else { USART_RX_BUF[USART_RX_STA&0X3FFF]=Res ; USART_RX_STA++; if(USART_RX_STA>(200-1))USART_RX_STA=0;//接收数据错误,重新开始接收 } } } } }
robot.h代码：
#ifndef __ROBOT_H #define __ROBOT_H #include "sys.h" void GpioInit(void);//IO口初始化函数 void TIM3_rightwheel_Init(u16 arr, u16 psc);//右车轮定时器初始化函数 void TIM4_leftwheel_Init(u16 arr, u16 psc);//左车轮定时器初始化函数 extern u8 USART_RX_BUF[200]; //接收缓冲,最大USART_REC_LEN个字节.末字节为换行符 extern u16 USART_RX_STA; //接收状态标记 void Usart_Init(u32 bound);//串口初始化函数 #endif
