五、STM32ZET6控制程序设计

2024-04-01 12:40:40

单片机作为下位机，功能是：

（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

码农公寓

相关文章