概述
/*
M法测速，A1 A2都设置为中断，相当于四倍频，取单位时间内1s内中断个数除以每转的霍尔脉冲数=转数每秒。
此法收到结果A电机是50转，BCD电机是100转，一个电机显示不正常，未找到原因，因此选用的T法测速。
T法测速是A1设置为中断，A2设置为方向，单次中断的时间乘以每圈霍尔个数就是一圈需要的时间，倒数就是转数每秒
*/
//M法测速
//#include "encoder.h"
//s32 C_HALL_IT_Update[4]={0};//脉冲个数
//s8 EncoderSpeed[4]={0};//每S转速
//void EncoderInit(void)
//{
// GPIO_InitTypeDef GPIO_InitStructure;
// EXTI_InitTypeDef EXTI_InitStructure;
// NVIC_InitTypeDef NVIC_InitStructure;
//
//
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC,ENABLE);//使能PORTA,PORTE时钟
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE); //使能复用功能时钟
// GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE);
//
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;//KEY0-KEY2
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //设置成上拉输入
// GPIO_Init(GPIOC, &GPIO_InitStructure);//初始化GPIOE2,3,4
//
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5;//KEY0-KEY2
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //设置成上拉输入
// GPIO_Init(GPIOB, &GPIO_InitStructure);//初始化GPIOE2,3,4
// //初始化 WK_UP-->GPIOA.0 下拉输入
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
// GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA.0
// //GPIOE.2 中断线以及中断初始化配置 下降沿触发
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource3);
// EXTI_InitStructure.EXTI_Line=EXTI_Line3; //KEY2
// EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
// EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
// EXTI_InitStructure.EXTI_LineCmd = ENABLE;
// EXTI_Init(&EXTI_InitStructure); //根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
//
// //GPIOE.3 中断线以及中断初始化配置 下降沿触发 //KEY1
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource4);
//
// EXTI_InitStructure.EXTI_Line=EXTI_Line4;
// EXTI_Init(&EXTI_InitStructure); //根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
// //GPIOE.4 中断线以及中断初始化配置 下降沿触发 //KEY0
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);
//
// EXTI_InitStructure.EXTI_Line=EXTI_Line5;
// EXTI_Init(&EXTI_InitStructure); //根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
// //GPIOA.0 中断线以及中断初始化配置 上升沿触发 PA0 WK_UP
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource11);
//
// EXTI_InitStructure.EXTI_Line=EXTI_Line11;
// EXTI_Init(&EXTI_InitStructure); //根据EXTI_InitStruct中指定的参数初始化外设EXTI寄存器
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOC,GPIO_PinSource6);
//
// EXTI_InitStructure.EXTI_Line=EXTI_Line6;
// EXTI_Init(&EXTI_InitStructure);
//
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOC,GPIO_PinSource7);
//
// EXTI_InitStructure.EXTI_Line=EXTI_Line7;
// EXTI_Init(&EXTI_InitStructure);
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOC,GPIO_PinSource8);
//
// EXTI_InitStructure.EXTI_Line=EXTI_Line8;
// EXTI_Init(&EXTI_InitStructure);
// GPIO_EXTILineConfig(GPIO_PortSourceGPIOC,GPIO_PinSource8);
//
// EXTI_InitStructure.EXTI_Line=EXTI_Line9;
// EXTI_Init(&EXTI_InitStructure);
// NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn; //使能按键WK_UP所在的外部中断通道
// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级2，
// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00; //子优先级3
// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
// NVIC_Init(&NVIC_InitStructure);
// NVIC_InitStructure.NVIC_IRQChannel = EXTI4_IRQn; //使能按键KEY2所在的外部中断通道
// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级2，
// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01; //子优先级2
// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
// NVIC_Init(&NVIC_InitStructure);
// NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn ; //使能按键KEY1所在的外部中断通道
// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级2
// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x02; //子优先级1
// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
// NVIC_Init(&NVIC_InitStructure); //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器
// NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn ; //使能按键KEY0所在的外部中断通道
// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级2
// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x03; //子优先级0
// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //使能外部中断通道
// NVIC_Init(&NVIC_InitStructure); //根据NVIC_InitStruct中指定的参数初始化外设NVIC寄存器
//
//}
外部中断0服务程序
// void EXTI3_IRQHandler(void)
//{
//if(EXTI_GetITStatus(EXTI_Line3) != RESET)//右轮B相 PB15
// {
// EXTI_ClearITPendingBit(EXTI_Line3); //清除LINE上的中断标志位
// if(HALLA1==1) //这里判断检测到的是否是下降沿
// {
// if(HALLA2==0) C_HALL_IT_Update[0]++;//A相的电平如果是高，电机就是正转加1
// else C_HALL_IT_Update[0]--;//否则就是反转减1
// }
// else //上升沿
// {
// if(HALLA2==1) C_HALL_IT_Update[0]++; //A相电平如果为低，电机就是正转加1
// else C_HALL_IT_Update[0]--;//否则就是反转减1
// }
//
// }
//}
外部中断2服务程序
//void EXTI4_IRQHandler(void)
//{
// if(EXTI_GetITStatus(EXTI_Line4) != RESET)//右轮B相 PB15
// {
// EXTI_ClearITPendingBit(EXTI_Line4); //清除LINE上的中断标志位
// if(HALLB1==1) //这里判断检测到的是否是下降沿
// {
// if(HALLB2==0) C_HALL_IT_Update[1]++;//A相的电平如果是高，电机就是正转加1
// else C_HALL_IT_Update[1]--;//否则就是反转减1
// }
// else //上升沿
// {
// if(HALLB2==1) C_HALL_IT_Update[1]++; //A相电平如果为低，电机就是正转加1
// else C_HALL_IT_Update[1]--;//否则就是反转减1
// }
//
// }
//}
//void EXTI9_5_IRQHandler(void)
//{
// if(EXTI_GetITStatus(EXTI_Line5) != RESET)
// {
// EXTI_ClearITPendingBit(EXTI_Line5); //清除LINE上的中断标志位
// if(HALLC1==1) //这里判断检测到的是否是下降沿
// {
// if(HALLC2==0) C_HALL_IT_Update[2]++;//B相的电平如果是低，电机就是正转加1
// else C_HALL_IT_Update[2]--;//否则就是反转减1
// }
// else //上升沿
// {
// if(HALLC2==1) C_HALL_IT_Update[2]++; //B相电平如果为高，电机就是正转加1
// else C_HALL_IT_Update[2]--;//否则就是反转减1
// }
// }
//
// if(EXTI_GetITStatus(EXTI_Line7) != RESET)//左轮B相 PB13
// {
// EXTI_ClearITPendingBit(EXTI_Line7); //清除LINE上的中断标志位
// if(HALLC2==1) //这里判断检测到的是否是下降沿
// {
// if(HALLC1==1) C_HALL_IT_Update[2]++; //B相的电平如果是高，电机就是正转加1
// else C_HALL_IT_Update[2]--;//否则就是反转减1
// }
// else //上升沿
// {
// if(HALLC1==0) C_HALL_IT_Update[2]++; //B相电平如果为高，电机就是正转加1
// else C_HALL_IT_Update[2]--;//否则就是反转减1
// }
// }
//
// if(EXTI_GetITStatus(EXTI_Line9) != RESET)//右轮A相 PB14
// {
// EXTI_ClearITPendingBit(EXTI_Line9); //清除LINE上的中断标志位
// if(HALLA2==1) //这里判断检测到的是否是下降沿
// {
// if(HALLA1==1) C_HALL_IT_Update[0]++;//B相的电平如果是低，电机就是正转加1
// else C_HALL_IT_Update[0]--;//否则就是反转减1
// }
// else //上升沿
// {
// if(HALLA1==0) C_HALL_IT_Update[0]++; //B相电平如果为高，电机就是正转加1
// else C_HALL_IT_Update[0]--;//否则就是反转减1
// }
// }
//
// if(EXTI_GetITStatus(EXTI_Line8) != RESET)//右轮B相 PB15
// {
// EXTI_ClearITPendingBit(EXTI_Line8); //清除LINE上的中断标志位
// if(HALLB2==1) //这里判断检测到的是否是下降沿
// {
// if(HALLB1==1) C_HALL_IT_Update[1]++;//A相的电平如果是高，电机就是正转加1
// else C_HALL_IT_Update[1]--;//否则就是反转减1
// }
// else //上升沿
// {
// if(HALLB1==0) C_HALL_IT_Update[1]++; //A相电平如果为低，电机就是正转加1
// else C_HALL_IT_Update[1]--;//否则就是反转减1
// }
// }
// if(EXTI_GetITStatus(EXTI_Line6) != RESET)//右轮B相 PB15
// {
// EXTI_ClearITPendingBit(EXTI_Line6); //清除LINE上的中断标志位
// if(HALLD2==1) //这里判断检测到的是否是下降沿
// {
// if(HALLD1==1) C_HALL_IT_Update[3]++;//A相的电平如果是高，电机就是正转加1
// else C_HALL_IT_Update[3]--;//否则就是反转减1
// }
// else //上升沿
// {
// if(HALLD1==0) C_HALL_IT_Update[3]++; //A相电平如果为低，电机就是正转加1
// else C_HALL_IT_Update[3]--;//否则就是反转减1
// }
//
// }
//
//}
//void EXTI15_10_IRQHandler(void)
//{
// if(EXTI_GetITStatus(EXTI_Line11) != RESET)//右轮B相 PB15
// {
// EXTI_ClearITPendingBit(EXTI_Line11); //清除LINE上的中断标志位
// if(HALLD1==1) //这里判断检测到的是否是下降沿
// {
// if(HALLD2==0) C_HALL_IT_Update[3]++;//A相的电平如果是高，电机就是正转加1
// else C_HALL_IT_Update[3]--;//否则就是反转减1
// }
// else //上升沿
// {
// if(HALLD2==1) C_HALL_IT_Update[3]++; //A相电平如果为低，电机就是正转加1
// else C_HALL_IT_Update[3]--;//否则就是反转减1
// }
//
// }
//}
//
初始化TIM6：
根据"timer.h"文件中的宏定义，设置TIM2的溢出时间
--------------------------------------------------------------------------------------------------------------
//void TIM6_Time_Init(void)
//{
// TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
// NVIC_InitTypeDef NVIC_InitStructure;
// RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); // 使能TIM3的时钟
//
// //定时器TIM2初始化
// TIM_TimeBaseStructure.TIM_Period = TIM6_Frequency_Divide ; // 设置下一个更新事件后，装入自动重装载寄存器的值
// TIM_TimeBaseStructure.TIM_Prescaler = TIM6_Reload_Num;// 设置TIM6时钟预分频值
// TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; // 设置时钟分割:TDTS = Tck_tim
// TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; // TIM向上计数模式
// TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure); // 根据参数初始化TIM3的计数方式
//
// TIM_ITConfig(TIM6,TIM_IT_Update,ENABLE ); // 允许TIM3更新中断
//
// //TIM6的中断NVIC设置
// NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn; // TIM3中断
// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; // 抢占优先级1级
// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; // 子优先级3级
// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; // 使能TIM3_IRQn通道
// NVIC_Init(&NVIC_InitStructure); // 初始化NVIC寄存器
// TIM_Cmd(TIM6, ENABLE); //
//}
--------------------------------------------------------------------------------------------------------------
电机参数
//#define MotorRedRatio 20 //电机减速比
//#define MotorWheelPerimeter 308 //轮子周长，单位mm，直径98mm
//#define MotMagNumber 12 //霍尔极对数（转动一圈输出的脉冲数）
减速电机带负载转速240RMP,每圈输出脉冲数240个，减速比20，电机本身一圈输出脉冲12个为一圈，本身转速80转/s；
//void TIM6_IRQHandler(void)
//{
// if( TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET ) // 判断是否为TIM3的更新中断
// {
// u8 i=0;
//
// TIM_ClearITPendingBit(TIM6, TIM_IT_Update ); // 清除TIM3更新中断标志
//
// for(i=0;i<4;i++)
// {
// EncoderSpeed[i] = C_HALL_IT_Update[i]/4*20/MotMagNumber; //每s多少转
//
// C_HALL_IT_Update[i]=0;
// }
//
// }
//}
//s8 MotorRPM_A(void)
//{
// return EncoderSpeed[0];
//}
//s8 MotorRPM_B(void)
//{
// return EncoderSpeed[1];
//}
//s8 MotorRPM_C(void)
//{
// return EncoderSpeed[2];
//}
//s8 MotorRPM_D(void)
//{
// return EncoderSpeed[3];
//}
///*
//获取线速度，单位mm/s
//*/
//int GetLinearVelocity_A(void)
//{//电机本身每秒转数*电机本身每转毫米=每秒毫米速度
// s8 i=MotorRPM_A();
// i=i*(MotorWheelPerimeter/MotorRedRatio);
// return i;
//}
///*
//获取线速度，单位mm/min
//*/
//int GetLinearVelocity_B(void)
//{
// s8 i=MotorRPM_B();
// i=i*(MotorWheelPerimeter/MotorRedRatio);
// return i;
//}
///*
//获取线速度，单位mm/s
//*/
//int GetLinearVelocity_C(void)
//{
// s8 i=MotorRPM_C();
// i=i*(MotorWheelPerimeter/MotorRedRatio);
// return i;
//}
///*
//获取线速度，单位mm/s
//*/
//int GetLinearVelocity_D(void)
//{
// s8 i=MotorRPM_D();
// i=i*(MotorWheelPerimeter/MotorRedRatio);
// return i;
//}
//T法测速
#include "encoder.h"
void EncoderInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC,ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8|GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOC, &GPIO_InitStructure);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource3);
EXTI_InitStructure.EXTI_Line=EXTI_Line3;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource4);
EXTI_InitStructure.EXTI_Line=EXTI_Line4;
EXTI_Init(&EXTI_InitStructure);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);
EXTI_InitStructure.EXTI_Line=EXTI_Line5;
EXTI_Init(&EXTI_InitStructure);
GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource11);
EXTI_InitStructure.EXTI_Line=EXTI_Line11;
EXTI_Init(&EXTI_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = EXTI4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn ;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x02;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn ;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x03;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
int Odometer[4]={0,0,0,0}; //里程值（编码器累加）
int Odometer_Keep[4]={0,0,0,0}; //里程值（编码器累加）
int HallSpeed[4][5]={{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0}}; //霍尔转速
int MotSpeed[4]={0,0,0,0};
int HallDir[4]={0,0,0,0};
void EXTI3_IRQHandler(void)
{
static u8 trun=0;
int i=0;
EXTI_ClearITPendingBit(EXTI_Line3);
Odometer[0]++; //里程
trun=!trun;
if(trun)
{TimerStart(0);i=TimerStop(1);}
else
{TimerStart(1);i=TimerStop(0);}
FIFO_long(i,&HallSpeed[0][0],5);
HallDir[0]=(HallA_InD)?(1):(0); //方向保存
}
void EXTI4_IRQHandler(void)
{
static u8 trun=0;
int i=0;
EXTI_ClearITPendingBit(EXTI_Line4);
Odometer[1]++; //里程
trun=!trun;
if(trun)
{TimerStart(2);i=TimerStop(3);}
else
{TimerStart(3);i=TimerStop(2);}
FIFO_long(i,&HallSpeed[1][0],5);
HallDir[1]=(HallB_InD)?(1):(0); //方向保存
}
void EXTI9_5_IRQHandler(void)
{
static u8 trun=0;
int i=0;
EXTI_ClearITPendingBit(EXTI_Line5);
Odometer[2]++;
trun=!trun;
if(trun)
{TimerStart(4);i=TimerStop(5);}
else
{TimerStart(5);i=TimerStop(4);}
FIFO_long(i,&HallSpeed[2][0],5);
HallDir[2]=(HallC_InD)?(1):(0); //方向保存
}
void EXTI15_10_IRQHandler(void)
{
static u8 trun=0;
int i=0;
EXTI_ClearITPendingBit(EXTI_Line11);
Odometer[3]++;
trun=!trun;
if(trun)
{TimerStart(6);i=TimerStop(7);}
else
{TimerStart(7);i=TimerStop(6);}
FIFO_long(i,&HallSpeed[3][0],5);
HallDir[3]=(HallD_InD)?(1):(0); //方向保存
}
void TIM6_Time_Init(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);
TIM_TimeBaseStructure.TIM_Prescaler = 72-1;
TIM_TimeBaseStructure.TIM_Period = 25000-1;
TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStructure);
TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
TIM_Cmd(TIM6, ENABLE);
}
int SysRunTimeCnt=0;
//定时器溢出25ms中断一次
void TIM6_IRQHandler (void)
{
SysRunTimeCnt++;
}
TypeTimerPar TimerParam[10];
/**************************************
开启计数
Channel：通道号码 支持0~9
**************************************/
void TimerStart(u8 Channel)
{
TimerParam[Channel].ms=SysRunTimeCnt;
TimerParam[Channel].us=TIM6->CNT;
}
/***************************************
关闭计数，返回计数时长，单位us
Channel：通道号码 支持0~9
返回值单位：微秒，且保存该值在TimerParam[Channel].cnt
***************************************/
int TimerStop(u8 Channel)
{
int gapTim_ms=SysRunTimeCnt;
int gapTim_us=TIM6->CNT;
if(gapTim_ms<TimerParam[Channel].ms) return 0;
TimerParam[Channel].cnt=(gapTim_ms-TimerParam[Channel].ms)*25000 + gapTim_us- TimerParam[Channel].us;
return TimerParam[Channel].cnt;
}
void FIFO_long(int FD,int *buf,u8 LEN)
{
u8 i;
for(i=0;i<(LEN-1);i++)
buf[i]=buf[i+1];
buf[i]=FD;
}
//TM3定时器5ms一次，用于电机测速计算
void BeatHandle_Encoder(void)
{
static u8 Cnt=0;
int temp=0;
u8 i=0,k=0;
Cnt++;
for(i=0;i<4;i++)
{
bubbling_long(&HallSpeed[i][0],5); //冒泡
temp=(HallSpeed[i][1]+HallSpeed[i][2]+HallSpeed[i][3])/3; //去最大最小，求平均
temp=temp*MotMagNumber;//每圈多少时间
temp=1000000/temp;//倒数为没秒多少圈
if(( i==0 ) ||(i==1)) //根据实际情况调整方向，两个轮子是反转
{temp=(HallDir[i])?(temp):(-temp);}
else
{temp=(HallDir[i])?(-temp):(temp);}
MotSpeed[i]=temp;
if(Cnt>=4) // 0.1秒钟之内没转到一0.1圈认为停转
{
if( (Odometer[i]-Odometer_Keep[i]) <= (u32)MotMagNumber/10)
{
MotSpeed[i]=0;
for(k=0;k<5;k++) HallSpeed[i][k]=0;
}
Odometer_Keep[i]=Odometer[i];
}
}
if(Cnt>=4) Cnt=0;
}
void bubbling_long(int *path,u8 LEN)
{
u8 i,k,j;
int temp=0;
k=LEN-1;
for(j=0;j<k;j++)
{
LEN--;
for(i=0;i<LEN;i++)
{
if(path[i]>path[i+1]) //往下则调换位置
{
temp=path[i];
path[i]=path[i+1];
path[i+1]=temp;
}
}
}
}
//获得电机转速
int MotorRPM_A(void)
{
return MotSpeed[0];
}
int MotorRPM_B(void)
{
return MotSpeed[1];
}
int MotorRPM_C(void)
{
return MotSpeed[2];
}
int MotorRPM_D(void)
{
return MotSpeed[3];
}
//获取线速度，单位mm/s,转每秒*电机本身每转距离=距离每秒即速度
int GetLinearVelocity_A(void)
{
long i=MotorRPM_A();
i=i*(MotorWheelPerimeter/MotorRedRatio);
return i;
}
int GetLinearVelocity_B(void)
{
long i=MotorRPM_B();
i=i*(MotorWheelPerimeter/MotorRedRatio);
return i;
}
int GetLinearVelocity_C(void)
{
long i=MotorRPM_C();
i=i*(MotorWheelPerimeter/MotorRedRatio);
return i;
}
int GetLinearVelocity_D(void)
{
long i=MotorRPM_D();
i=i*(MotorWheelPerimeter/MotorRedRatio);
return i;
}
//需要循环调用，比如5ms调用一次，根据轮子尺寸算里程
s32 MileageA_mm=0;
s32 MileageB_mm=0;
s32 MileageC_mm=0;
s32 MileageD_mm=0;
//主函数50ms调用一次，用下面函数显示里程，不计算；
void GetMileage(void)
{
//脉冲个数*（电机本身每转毫米/每转脉冲数）=每脉冲多少毫米*脉冲数=行驶距离
MileageA_mm=( Odometer[0]*MotorWheelPerimeter)/(MotorRedRatio*MotMagNumber);
MileageB_mm=( Odometer[1]*MotorWheelPerimeter)/(MotorRedRatio*MotMagNumber);
MileageC_mm=( Odometer[2]*MotorWheelPerimeter)/(MotorRedRatio*MotMagNumber);
MileageD_mm=( Odometer[3]*MotorWheelPerimeter)/(MotorRedRatio*MotMagNumber);
}
int MotA_distance_mm(void)
{
return MileageA_mm;
}
int MotB_distance_mm(void)
{
return MileageB_mm;
}
int MotC_distance_mm(void)
{
return MileageC_mm;
}
int MotD_distance_mm(void)
{
return MileageD_mm;
}
//M法测速
//#ifndef __ENCODER_H
//#define __ENCODER_H
//#include "stm32f10x.h"
//#include "bit_band.h"
按键宏定义
-----------------------------------------------------
//#define HALLA1 PB_in(3)
//#define HALLB1 PB_in(4)
//#define HALLC1 PB_in(5)
//#define HALLD1 PA_in(11)
//#define HALLA2 PC_in(9)
//#define HALLB2 PC_in(8)
//#define HALLC2 PC_in(7)
//#define HALLD2 PC_in(6)
//#define TIM6_Frequency_Divide (72-1) // TIM1时钟预分频值
//#define TIM6_Reload_Num (50000-1) // 自动重装载寄存器的值
//void EncoderInit(void);
//void TIM6_Time_Init(void);
//s8 MotorRPM_A(void);
//s8 MotorRPM_B(void);
//s8 MotorRPM_C(void);
//s8 MotorRPM_D(void);
//#endif /* __KEY_H */
//T法测速
#ifndef __ENCODER_H
#define __ENCODER_H
#include "stm32f10x.h"
#include "bit_band.h"
//空载300RMP,带负载240RMP,每圈输出脉冲数240个，减速比20，12个霍尔极对数，最高转速100转/s；
#define MotorRedRatio 20 //电机减速比
#define MotorWheelPerimeter 308 //轮子周长，单位mm，直径98mm
#define MotMagNumber 12 //霍尔极对数（转动一圈输出的脉冲数）
//根据霍尔方向调整测速MotSpeed[i]的正负值------------------------------------------
#define HallA_InD PC_in(9)
#define HallB_InD PC_in(8)
#define HallC_InD PC_in(7)
#define HallD_InD PC_in(6)
//---------------------------------------------------------------
//中断计时结构体
typedef struct
{
u32 ms;
u32 us;
u32 cnt;
}TypeTimerPar;
//---------------------------------------------------------------
void EncoderInit(void);
void TIM6_Time_Init(void);
void TimerStart(u8 Channel);
int TimerStop(u8 Channel);
void FIFO_long(int FD,int *buf,u8 LEN);
void BeatHandle_Encoder(void);
void bubbling_long(int *path,u8 LEN);
int MotorRPM_A(void);
int MotorRPM_B(void);
int MotorRPM_C(void);
int MotorRPM_D(void);
int GetLinearVelocity_A(void);
int GetLinearVelocity_B(void);
int GetLinearVelocity_C(void);
int GetLinearVelocity_D(void);
void GetMileage(void);
int MotA_distance_mm(void);
int MotB_distance_mm(void);
int MotC_distance_mm(void);
int MotD_distance_mm(void);
void Mot_speedset(int Speed1, int Speed2,int Speed3, int Speed4);
#endif
#include "oled.h"
#include "oledfont.h"
#include "delay.h"
#include "i2c.h"
#include "encoder.h"
#include "rf2G4.h"
/*OLED函数*/
void WriteCmd(unsigned char I2C_Command)//写命令
{
I2C_Start();
I2C_WriteByte(0x78); //Slave address,SA0=0
I2C_Wait_Ack();
I2C_WriteByte(0x00); //write command
I2C_Wait_Ack();
I2C_WriteByte(I2C_Command);
I2C_Wait_Ack();
I2C_Stop();
}
void WriteDat(unsigned char I2C_Data)//写数据
{
I2C_Start();
I2C_WriteByte(0x78); //D/C#=0; R/W#=0
I2C_Wait_Ack();
I2C_WriteByte(0x40); //write data//设置显示开始行 [5:0],行数.
I2C_Wait_Ack();
I2C_WriteByte(I2C_Data);
I2C_Wait_Ack();
I2C_Stop();
}
void OLED_Init(void)
{
I2C_Configuration();
delay_ms(100); //这里的延时很重要
WriteCmd(0xAE); //display off
WriteCmd(0x20); //Set Memory Addressing Mode
WriteCmd(0x10); //00,Horizontal Addressing Mode;01,Vertical Addressing Mode;10,Page Addressing Mode (RESET);11,Invalid
WriteCmd(0xb0); //Set Page Start Address for Page Addressing Mode,0-7
WriteCmd(0xc8); //Set COM Output Scan Direction
WriteCmd(0x00); //---set low column address
WriteCmd(0x10); //---set high column address
WriteCmd(0x40); //--set start line address
WriteCmd(0x81); //--set contrast control register
WriteCmd(0xff); //亮度调节 0x00~0xff
WriteCmd(0xa1); //--set segment re-map 0 to 127
WriteCmd(0xa6); //--set normal display
WriteCmd(0xa8); //--set multiplex ratio(1 to 64)
WriteCmd(0x3F); //
WriteCmd(0xa4); //0xa4,Output follows RAM content;0xa5,Output ignores RAM content
WriteCmd(0xd3); //-set display offset
WriteCmd(0x00); //-not offset
WriteCmd(0xd5); //--set display clock divide ratio/oscillator frequency
WriteCmd(0xf0); //--set divide ratio
WriteCmd(0xd9); //--set pre-charge period
WriteCmd(0x22); //
WriteCmd(0xda); //--set com pins hardware configuration
WriteCmd(0x12);
WriteCmd(0xdb); //--set vcomh
WriteCmd(0x20); //0x20,0.77xVcc
WriteCmd(0x8d); //--set DC-DC enable
WriteCmd(0x14); //
WriteCmd(0xaf); //--turn on oled panel
OLED_CLS();//不清屏幕，未用到的地方是花屏
}
void OLED_ON(void)
{
WriteCmd(0X8D); //设置电荷泵
WriteCmd(0X14); //开启电荷泵
WriteCmd(0XAF); //OLED唤醒
}
void OLED_OFF(void)
{
WriteCmd(0X8D); //设置电荷泵
WriteCmd(0X10); //关闭电荷泵
WriteCmd(0XAE); //OLED休眠
}
void OLED_SetPos(unsigned char x, unsigned char y) //设置起始点坐标。Y为行，X为列
{
WriteCmd(0xb0+y);//行地址是b0-b7
WriteCmd(((x&0xf0)>>4)|0x10);//列地址高四位是0x10-0x1F=10000-11111
WriteCmd(x&0x0f);//列地址低四位是0x00-0x0F=00000000-00001111
}
void OLED_Fill(unsigned char fill_Data)//全屏填充
{
unsigned char m,n;
for(m=0;m<8;m++)
{
WriteCmd(0xb0+m); //page0-page1
WriteCmd(0x00); //low column start address
WriteCmd(0x10); //high column start address
for(n=0;n<128;n++)
{
WriteDat(fill_Data);
}
}
}
void OLED_CLS(void)//清屏
{
OLED_Fill(0x00);
}
void OLED_ShowStr(unsigned char x, unsigned char y, unsigned char ch[], unsigned char TextSize)
{
unsigned char c = 0,i = 0,j = 0;
switch(TextSize)
{
case 6:
{
while(ch[j] != ' ')
{
c = ch[j] - 32;
if(x > 126)
{
x = 0;
y++;
}
OLED_SetPos(x,y);
for(i=0;i<6;i++)
WriteDat(F6x8[c][i]);
x += 6;
j++;
}
}break;
case 8:
{
while(ch[j] != ' ')
{
c = ch[j] - 32;
if(x > 120)
{
x = 0;
y++;
}
OLED_SetPos(x,y);
for(i=0;i<8;i++)
WriteDat(F8X16[c*16+i]);
OLED_SetPos(x,y+1);
for(i=0;i<8;i++)
WriteDat(F8X16[c*16+i+8]);
x += 8;
j++;
}
}break;
}
}
//显示1个汉字
void OLED_ShowCN(unsigned char x, unsigned char y, unsigned char N)
{
unsigned char wm=0;
unsigned int adder=32*N;
OLED_SetPos(x , y);
for(wm = 0;wm < 16;wm++)
{
WriteDat(F16x16[adder]);
adder += 1;
}
OLED_SetPos(x,y + 1);
for(wm = 0;wm < 16;wm++)
{
WriteDat(F16x16[adder]);
adder += 1;
}
}
//显示一串汉字
void OLED_ShowCNStr(unsigned char x, unsigned char y,unsigned char num)
{
unsigned char i;
for(i=0;i<num;i++)
OLED_ShowCN(x+i*16, y, i);
}
void OLED_DrawBMP(unsigned char x0,unsigned char y0,unsigned char x1,unsigned char y1,unsigned char BMP[])
{
unsigned int j=0;
unsigned char x,y;
if(y1%8==0)//在整页位置首地址
y = y1/8;
else
y = y1/8 + 1;
for(y=y0;y<y1;y++)
{
OLED_SetPos(x0,y);
for(x=x0;x<x1;x++)
{
WriteDat(BMP[j++]);
}
}
}
/*----------------------------------以下程序主要为显示负整数转速-----------------------------*/
//在指定位置显示一个字符,包括部分字符
//x:0~127
//y:0~63
//mode:0,反白显示;1,正常显示
//size:选择字体 8/6
void OLED_ShowChar(unsigned char x,unsigned char y,unsigned char chr,unsigned char TextSize)
{
unsigned char c = 0,i = 0;
switch(TextSize)
{
case 6:
{
c=chr-' ';//得到偏移后的值，ASCII码显示字符里第一个字符是空字符，十进制是32
if(x > 121)
{
x = 0;
y++;
}
OLED_SetPos(x,y);
for(i=0;i<6;i++)
WriteDat(F6x8[c][i]);//二维数组不用乘以每个字符需要的OLED显示个数
}break;
case 8:
{
c = chr - 32;
if(x > 120)
{
x = 0;
y++;
}
OLED_SetPos(x,y);
for(i=0;i<8;i++)
WriteDat(F8X16[c*16+i]);
OLED_SetPos(x,y+1);
for(i=0;i<8;i++)
WriteDat(F8X16[c*16+i+8]);//一维数组
}break;
}
}
//m^n次方函数
unsigned int oled_pow(unsigned char m,unsigned char n)
{
u32 result=1;
while(n--)
result*=m;
return result;
}
//显示无符号长整形
//x,y :起点坐标
//len :数字的位数
//size:字体大小
//mode:模式 0,填充模式;1,叠加模式
//num:数值(0~4294967295);
void OLED_ShowNum(unsigned char x,unsigned char y,unsigned int num,unsigned char len,unsigned char TextSize)
{
unsigned char t,temp;
unsigned char enshow=0;
for(t=0;t<len;t++)
{
temp=(num/oled_pow(10,len-t-1))%10;
//此部分程序有点疑问，后期看视频学习
if(enshow==0&&t<(len-1))
{
if(temp==0)
{
OLED_ShowChar(x+TextSize*t,y,' ',TextSize);
continue;
}
else
enshow=1;
}
OLED_ShowChar(x+TextSize*t,y,temp+'0',TextSize); //将temp单个整数0至9+字符'0'转换为ASCII码字符）
}
}
/*
描述：显示整数(可显示正负数)（-65535~65535）
输入：
x:显示的x起始位置
y:显示的y起始位置
num:要显示的数字
Char_Size:要显示数字的大小
*/
void OLED_ShowIntegerNumber(unsigned char x,unsigned char y,int num,unsigned char len,unsigned char TextSize)
{
if(num<0)
{
OLED_ShowChar(x,y,'-',TextSize);
num=-num;
OLED_ShowNum(x+TextSize,y,(unsigned int)num,len,TextSize);
}
else
{
OLED_ShowNum(x,y,(unsigned int)num,len,TextSize);
}
}
void OLED_display(void)
{
//OLED显示
OLED_ShowStr(0,1,"SetSpeed: R/S",6) ; OLED_ShowIntegerNumber(50,1,RF2G4_Receive_Data[11]/2.55,2,6);
OLED_ShowStr(0,3,"AR: ",6) ; OLED_ShowIntegerNumber(18,3,MotorRPM_A(),2,6);
OLED_ShowStr(64,3,"AL: ",6); OLED_ShowIntegerNumber(82,3,GetLinearVelocity_A(),2,6);
OLED_ShowStr(0,4,"BR: ",6) ; OLED_ShowIntegerNumber(18,4,MotorRPM_B(),2,6);
OLED_ShowStr(64,4,"BL: ",6); OLED_ShowIntegerNumber(82,4,GetLinearVelocity_B(),2,6);
OLED_ShowStr(0,5,"CR: ",6) ; OLED_ShowIntegerNumber(18,5,MotorRPM_C(),2,6);
OLED_ShowStr(64,5,"CL: ",6); OLED_ShowIntegerNumber(82,5,GetLinearVelocity_C(),2,6);
OLED_ShowStr(0,6,"DR: ",6) ; OLED_ShowIntegerNumber(18,6,MotorRPM_D(),2,6);
OLED_ShowStr(64,6,"DL: ",6); OLED_ShowIntegerNumber(82,6,GetLinearVelocity_D(),2,6);
}
#ifndef __OLED_H
#define __OLED_H
#include "stm32f10x.h"
void WriteCmd(unsigned char I2C_Command);
void WriteDat(unsigned char I2C_Data);
void OLED_Init(void);
void OLED_ON(void);
void OLED_OFF(void);
void OLED_SetPos(unsigned char x, unsigned char y);
void OLED_Fill(unsigned char fill_Data);
void OLED_CLS(void);
void OLED_ShowStr(unsigned char x, unsigned char y, unsigned char ch[], unsigned char TextSize);
void OLED_ShowCN(unsigned char x, unsigned char y, unsigned char N);
void OLED_DrawBMP(unsigned char x0,unsigned char y0,unsigned char x1,unsigned char y1,unsigned char BMP[]);
void OLED_ShowChar(unsigned char x,unsigned char y,unsigned char chr,unsigned char TextSize);
unsigned int oled_pow(unsigned char m,unsigned char n);
void OLED_ShowNum(unsigned char x,unsigned char y,unsigned int num,unsigned char len,unsigned char TextSize);
void OLED_ShowIntegerNumber(unsigned char x,unsigned char y,int num,unsigned char len,unsigned char TextSize);
void OLED_display(void);
#endif
暂未发完
最后
以上就是如意人生为你收集整理的麦克纳姆轮小车程序第二部分（标准库版本）的全部内容，希望文章能够帮你解决麦克纳姆轮小车程序第二部分（标准库版本）所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复