#include "encoder.h"
#include "sys.h"
#include <math.h>
int32_t encoder_cnt[4]={0,0,0,0};
int32_t last_cnt[4]={0,0,0,0};
int32_t delta_cnt[4]={0,0,0,0};
int8_t encoder_direct[4]={0,0,0,0};
float encoder_circles[4]={0,0,0,0};
s32 EncoderPul[4]={0};//脉冲总数
s32 EncoderPulSpeed[4]={0};//脉冲每秒 ,编码器速度
float last_Wheelcircles[4]={0,0,0,0};
float WheelSpeed[4]={0,0,0,0};
float WheelCircles[4]={0,0,0,0};//电机的圈数
u8 setdirecflag[4]={1,1,1,1};//设置的电机方向,假如设置目标是负数,那么方向等于0
u8 Car_movingFlag=0;//0停止,1正转,-1翻转
/**
* @brief TIM4 通道1通道2 正交编码器(编码器接口)-B6B7--T4:
* @param none
*/
void TIM4_MOTOR1_encoder_init(void)
{
GPIO_InitTypeDef GPIO_InitStruct; /*GPIO*/
TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct; /*时基*/
TIM_ICInitTypeDef TIM_ICInitStruct; /*输入通道*/
NVIC_InitTypeDef NVIC_InitStructure; /*中断*/
/*GPIO初始化*/
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); /*使能GPIO时钟 AHB1*/
GPIO_StructInit(&GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; /*复用功能*/
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; /*速度100MHz*/
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);
/*时基初始化*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); /*使能定时器时钟 APB1*/
TIM_DeInit(TIM4);
TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
TIM_TimeBaseStruct.TIM_Prescaler = ENCODER_TIM_PSC; /*预分频 */
TIM_TimeBaseStruct.TIM_Period = ENCODER_TIM_PERIOD; /*周期(重装载值)*/
TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up; /*连续向上计数模式*/
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStruct);
/*编码器模式配置:同时捕获通道1与通道2(即4倍频),极性均为Rising*/
TIM_EncoderInterfaceConfig(TIM4, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter = 0; /*输入通道的滤波参数*/
TIM_ICInit(TIM4, &TIM_ICInitStruct); /*输入通道初始化*/
TIM_SetCounter(TIM4, CNT_INIT); /*CNT设初值*/
TIM_ClearFlag(TIM4,TIM_IT_Update); /*中断标志清0*/
TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE); /*中断使能*/
TIM_Cmd(TIM4,ENABLE); /*使能CR寄存器*/
/*中断配置*/
NVIC_InitStructure.NVIC_IRQChannel=TIM4_IRQn; //定时器4中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; //子优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
/**
* @brief TIM5 通道1通道2 正交编码器(编码器接口)A0A1--T5
* @param none
*/
void TIM5_MOTOR2_encoder_init(void)
{
GPIO_InitTypeDef GPIO_InitStruct; /*GPIO*/
TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct; /*时基*/
TIM_ICInitTypeDef TIM_ICInitStruct; /*输入通道*/
NVIC_InitTypeDef NVIC_InitStructure; /*中断*/
/*GPIO初始化*/
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); /*使能GPIO时钟 AHB1*/
GPIO_StructInit(&GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; /*复用功能*/
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; /*速度100MHz*/
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource0,GPIO_AF_TIM5);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource1,GPIO_AF_TIM5);
/*时基初始化*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); /*使能定时器时钟 APB1*/
TIM_DeInit(TIM5);
TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
TIM_TimeBaseStruct.TIM_Prescaler = ENCODER_TIM_PSC; /*预分频 */
TIM_TimeBaseStruct.TIM_Period = ENCODER_TIM_PERIOD; /*周期(重装载值)*/
TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up; /*连续向上计数模式*/
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStruct);
/*编码器模式配置:同时捕获通道1与通道2(即4倍频),极性均为Rising......TIM_ICPolarity_Rising TIM_ICPolarity_Falling */
//由于编码器线翻了,所以,将rising改为falling,其他不改。
TIM_EncoderInterfaceConfig(TIM5, TIM_EncoderMode_TI12,TIM_ICPolarity_Falling, TIM_ICPolarity_Falling);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter = 0; /*输入通道的滤波参数*/
TIM_ICInit(TIM5, &TIM_ICInitStruct); /*输入通道初始化*/
TIM_SetCounter(TIM5, CNT_INIT); /*CNT设初值*/
TIM_ClearFlag(TIM5,TIM_IT_Update); /*中断标志清0*/
TIM_ITConfig(TIM5, TIM_IT_Update, ENABLE); /*中断使能*/
TIM_Cmd(TIM5,ENABLE); /*使能CR寄存器*/
/*中断配置*/
NVIC_InitStructure.NVIC_IRQChannel=TIM5_IRQn; //定时器5中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; //子优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
/**
* @brief TIM2 通道1通道2 正交编码器-(编码器接口)A15B3--T2:
* @param none
*/
void TIM2_MOTOR3_encoder_init(void)
{
GPIO_InitTypeDef GPIO_InitStruct; /*GPIO*/
TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct; /*时基*/
TIM_ICInitTypeDef TIM_ICInitStruct; /*输入通道*/
NVIC_InitTypeDef NVIC_InitStructure; /*中断*/
/*GPIO初始化*/
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); /*使能GPIO时钟 AHB1*/
GPIO_StructInit(&GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; /*复用功能*/
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; /*速度100MHz*/
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStruct);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); /*使能GPIO时钟 AHB1*/
GPIO_StructInit(&GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; /*复用功能*/
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; /*速度100MHz*/
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource15,GPIO_AF_TIM2);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource3,GPIO_AF_TIM2);
/*时基初始化*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); /*使能定时器时钟 APB1*/
TIM_DeInit(TIM2);
TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
TIM_TimeBaseStruct.TIM_Prescaler = ENCODER_TIM_PSC; /*预分频 */
TIM_TimeBaseStruct.TIM_Period = ENCODER_TIM_PERIOD; /*周期(重装载值)*/
TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up; /*连续向上计数模式*/
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStruct);
/*编码器模式配置:同时捕获通道1与通道2(即4倍频),极性均为Rising*/
TIM_EncoderInterfaceConfig(TIM2, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter = 0; /*输入通道的滤波参数*/
TIM_ICInit(TIM2, &TIM_ICInitStruct); /*输入通道初始化*/
TIM_SetCounter(TIM2, CNT_INIT); /*CNT设初值*/
TIM_ClearFlag(TIM2,TIM_IT_Update); /*中断标志清0*/
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); /*中断使能*/
TIM_Cmd(TIM2,ENABLE); /*使能CR寄存器*/
/*中断配置*/
NVIC_InitStructure.NVIC_IRQChannel=TIM2_IRQn; //定时器5中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; //子优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
/**
* @brief TIM3 通道1通道2 正交编码器(编码器接口)A0A1--T5
* @param none
*/
void TIM3_MOTOR4_encoder_init(void)
{
GPIO_InitTypeDef GPIO_InitStruct; /*GPIO*/
TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct; /*时基*/
TIM_ICInitTypeDef TIM_ICInitStruct; /*输入通道*/
NVIC_InitTypeDef NVIC_InitStructure; /*中断*/
/*GPIO初始化*/
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); /*使能GPIO时钟 AHB1*/
GPIO_StructInit(&GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_4| GPIO_Pin_5;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; /*复用功能*/
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; /*速度100MHz*/
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource4,GPIO_AF_TIM3);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource5,GPIO_AF_TIM3);
/*时基初始化*/
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); /*使能定时器时钟 APB1*/
TIM_DeInit(TIM3);
TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
TIM_TimeBaseStruct.TIM_Prescaler = ENCODER_TIM_PSC; /*预分频 */
TIM_TimeBaseStruct.TIM_Period = ENCODER_TIM_PERIOD; /*周期(重装载值)*/
TIM_TimeBaseStruct.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up; /*连续向上计数模式*/
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStruct);
/*编码器模式配置:同时捕获通道1与通道2(即4倍频),极性均为Rising*/
TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12,TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
TIM_ICStructInit(&TIM_ICInitStruct);
TIM_ICInitStruct.TIM_ICFilter = 0; /*输入通道的滤波参数*/
TIM_ICInit(TIM3, &TIM_ICInitStruct); /*输入通道初始化*/
TIM_SetCounter(TIM3, CNT_INIT); /*CNT设初值*/
TIM_ClearFlag(TIM3,TIM_IT_Update); /*中断标志清0*/
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); /*中断使能*/
TIM_Cmd(TIM3,ENABLE); /*使能CR寄存器*/
/*中断配置*/
NVIC_InitStructure.NVIC_IRQChannel=TIM3_IRQn; //定时器5中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; //子优先级1
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
/* 定时器溢出次数 */
__IO int16_t Encoder1_OverflowCnt = 0;
__IO int16_t Encoder2_OverflowCnt = 0;
__IO int16_t Encoder3_OverflowCnt = 0;
__IO int16_t Encoder4_OverflowCnt = 0;
//定时器4中断服务函数
void TIM4_IRQHandler(void)
{
if(TIM_GetITStatus(TIM4,TIM_IT_Update)==SET) //溢出中断
{
if((TIM4->CR1 & TIM_CounterMode_Down) != TIM_CounterMode_Down)
{
Encoder1_OverflowCnt++;/*编码器计数值[向上]溢出*/
}
else
{
Encoder1_OverflowCnt--;/*编码器计数值[向下]溢出*/
}
}
TIM_ClearITPendingBit(TIM4,TIM_IT_Update); //清除中断标志位
}
//定时器5中断服务函数
void TIM5_IRQHandler(void)
{
if(TIM_GetITStatus(TIM5,TIM_IT_Update)==SET) //溢出中断
{
if((TIM5->CR1 & TIM_CounterMode_Down) != TIM_CounterMode_Down)
{
Encoder2_OverflowCnt++;/*编码器计数值[向上]溢出*/
}
else
{
Encoder2_OverflowCnt--;/*编码器计数值[向下]溢出*/
}
}
TIM_ClearITPendingBit(TIM5,TIM_IT_Update); //清除中断标志位
}
//定时器2中断服务函数
void TIM2_IRQHandler(void)
{
if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET) //溢出中断
{
if((TIM2->CR1 & TIM_CounterMode_Down) != TIM_CounterMode_Down)
{
Encoder3_OverflowCnt++;/*编码器计数值[向上]溢出*/
}
else
{
Encoder3_OverflowCnt--;/*编码器计数值[向下]溢出*/
}
}
TIM_ClearITPendingBit(TIM2,TIM_IT_Update); //清除中断标志位
}
//定时器3中断服务函数
void TIM3_IRQHandler(void)
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) //溢出中断
{
if((TIM3->CR1 & TIM_CounterMode_Down) != TIM_CounterMode_Down)
{
Encoder4_OverflowCnt++;/*编码器计数值[向上]溢出*/
}
else
{
Encoder4_OverflowCnt--;/*编码器计数值[向下]溢出*/
}
}
TIM_ClearITPendingBit(TIM3,TIM_IT_Update); //清除中断标志位
}
// 50Hz update
//获取编码器的编码数量
void update_All_encoder(void)
{
encoder_cnt[0] = (uint32_t)(TIM4->CNT);
encoder_cnt[1] = (uint32_t)(TIM5->CNT);
encoder_cnt[2] = (uint32_t)(TIM2->CNT);
encoder_cnt[3] = (uint32_t)(TIM3->CNT);
update_All_Encoder_delta();
}
void update_Encoder1_delta(void)
{
delta_cnt[0] = encoder_cnt[0] - last_cnt[0];
//处理边界问题,人为定了个最大边界的差值MAX_DELTA_50HZ=200
if(fabs(delta_cnt[0]) >= MAX_DELTA_ENCODER_Freq)
{
int32_t temp;
if(delta_cnt[0] > 0)
{
temp = ENCODER_TIM_PERIOD - encoder_cnt[0];//65535-n2
delta_cnt[0] = -(last_cnt[0] + temp);//de=-(100+35)=-135是负数
}
else
{
temp = ENCODER_TIM_PERIOD - last_cnt[0];//65535-n1=65535-65500=35
delta_cnt[0] = encoder_cnt[0] + temp;//n2+35=100+35=135
}
}
last_cnt[0] = encoder_cnt[0];//更新数据
if(delta_cnt[0] == 0) {//停止
encoder_direct[0] = 0;
} else if(delta_cnt[0] > 0) { //如果差值大于0,那么是正转,
encoder_direct[0] = 1;
} else {
encoder_direct[0] = -1;//如果差值小于0那么是翻转
}
EncoderPulSpeed[0] = delta_cnt[0]*MAX_ReadEncoderFreq;//除以5ms,等于乘以200
EncoderPul[0]=EncoderPul[0]+delta_cnt[0];
//四分频,因此乘以0.25
encoder_circles[0] += delta_cnt[0]*ENCODER1_MULTIPLE/ENCODER1_RESOLUTION;
WheelCircles[0]=encoder_circles[0]/MOTOR1_REDUCTION_RATIO;
// encoder_circles[0] += delta_cnt[0]*ENCODER1_MULTIPLE/ENCODER1_RESOLUTION;
// WheelCircles[0] +=delta_cnt[0]*ENCODER1_MULTIPLE/ENCODER1_RESOLUTION/MOTOR1_REDUCTION_RATIO;
}
void update_Encoder2_delta(void)
{
delta_cnt[1] = encoder_cnt[1] - last_cnt[1];
//处理边界问题,人为定了个最大边界的差值MAX_DELTA_50HZ=200
if(fabs(delta_cnt[1]) >= MAX_DELTA_ENCODER_Freq)
{
int32_t temp;
if(delta_cnt[1] > 0)
{
temp = ENCODER_TIM_PERIOD - encoder_cnt[1];//65535-n2
delta_cnt[1] = -(last_cnt[1] + temp);//de=-(100+35)=-135是负数
}
else
{
temp = ENCODER_TIM_PERIOD - last_cnt[1];//65535-n1=65535-65500=35
delta_cnt[1] = encoder_cnt[1] + temp;//n2+35=100+35=135
}
}
last_cnt[1] = encoder_cnt[1];//更新数据
if(delta_cnt[1] == 0) {//停止
encoder_direct[1] = 0;
} else if(delta_cnt[1] > 0) { //如果差值大于0,那么是正转,
encoder_direct[1] = 1;
} else {
encoder_direct[1] = -1;//如果差值小于0那么是翻转
}
EncoderPulSpeed[1] = delta_cnt[1]*MAX_ReadEncoderFreq;//除以20ms,等于乘以50
EncoderPul[1]=EncoderPul[1]+delta_cnt[1];
encoder_circles[1] += delta_cnt[1]*ENCODER2_MULTIPLE/ENCODER2_RESOLUTION;
WheelCircles[1]=encoder_circles[1]/MOTOR2_REDUCTION_RATIO;
}
void update_Encoder3_delta(void)
{
delta_cnt[2] = encoder_cnt[2] - last_cnt[2];
//处理边界问题,人为定了个最大边界的差值MAX_DELTA_50HZ=200
if(fabs(delta_cnt[2]) >= MAX_DELTA_ENCODER_Freq)
{
int32_t temp;
if(delta_cnt[2] > 0)
{
temp = ENCODER_TIM_PERIOD - encoder_cnt[2];//65535-n2
delta_cnt[2] = -(last_cnt[2] + temp);//de=-(100+35)=-135是负数
}
else
{
temp = ENCODER_TIM_PERIOD - last_cnt[2];//65535-n1=65535-65500=35
delta_cnt[2] = encoder_cnt[2] + temp;//n2+35=100+35=135
}
}
last_cnt[2] = encoder_cnt[2];//更新数据
if(delta_cnt[2] == 0) {//停止
encoder_direct[2] = 0;
} else if(delta_cnt[2] > 0) { //如果差值大于0,那么是正转,
encoder_direct[2] = 1;
} else {
encoder_direct[2] = -1;//如果差值小于0那么是翻转
}
EncoderPulSpeed[2] = delta_cnt[2]*MAX_ReadEncoderFreq;//除以20ms,等于乘以50
EncoderPul[2]=EncoderPul[2]+delta_cnt[2];
encoder_circles[2] += delta_cnt[2]*ENCODER3_MULTIPLE/ENCODER3_RESOLUTION;
WheelCircles[2]=encoder_circles[2]/MOTOR3_REDUCTION_RATIO;
}
void update_Encoder4_delta(void)
{
delta_cnt[3] = encoder_cnt[3] - last_cnt[3];
//处理边界问题,人为定了个最大边界的差值MAX_DELTA_50HZ=200
if(fabs(delta_cnt[3]) >= MAX_DELTA_ENCODER_Freq)
{
int32_t temp;
if(delta_cnt[3] > 0)
{
temp = ENCODER_TIM_PERIOD - encoder_cnt[3];//65535-n2
delta_cnt[3] = -(last_cnt[3] + temp);//de=-(100+35)=-135是负数
}
else
{
temp = ENCODER_TIM_PERIOD - last_cnt[3];//65535-n1=65535-65500=35
delta_cnt[3] = encoder_cnt[3] + temp;//n2+35=100+35=135
}
}
last_cnt[3] = encoder_cnt[3];//更新数据
if(delta_cnt[3] == 0) {//停止
encoder_direct[3] = 0;
} else if(delta_cnt[3] > 0) { //如果差值大于0,那么是正转,
encoder_direct[3] = 1;
} else {
encoder_direct[3] = -1;//如果差值小于0那么是翻转
}
EncoderPulSpeed[3] = delta_cnt[3]*MAX_ReadEncoderFreq;//除以20ms,等于乘以50
EncoderPul[3]=EncoderPul[3]+delta_cnt[3];
encoder_circles[3] += delta_cnt[3]*ENCODER4_MULTIPLE/ENCODER4_RESOLUTION;
WheelCircles[3]=encoder_circles[3]/MOTOR4_REDUCTION_RATIO;
}
void update_All_Encoder_delta(void)
{
update_Encoder1_delta();
update_Encoder2_delta();
update_Encoder3_delta();
update_Encoder4_delta();
}
******************计算电机速度,返回的速度单位是Np=(脉冲个数/s)
//int32_t calc_Motor_PulseSpeed(u8 whichmotor)
//{
// int32_t speed;
// switch(whichmotor)
// {
// case 1:
// speed = delta_cnt[0]*MAX_ReadEncoderFreq;
// break;
// case 2:
// speed = delta_cnt[1]*MAX_ReadEncoderFreq;
// break;
// case 3:
// speed = delta_cnt[2]*MAX_ReadEncoderFreq;
// break;
// case 4:
// speed = delta_cnt[3]*MAX_ReadEncoderFreq;
// break;
// default:break;
// }
// return speed;//处理float类型的取绝对值
//}
//计算速度思路
//速度等于当前圈数-上次圈数的差值,再除以deltaT,这里的deltaT=20ms,因为50HZ
//如果速度大于200*1/48约等于4圈每20ms,20圈每秒
//https://item.taobao.com/item.htm?spm=a230r.1.14.136.e2024fadEuXhHU&id=618948199559&ns=1&abbucket=14#detail
//
float calc_Wheel1_speed(void)
{
float speed;
speed = (WheelCircles[0] - last_Wheelcircles[0])*MAX_ReadEncoderFreq;
last_Wheelcircles[0]= WheelCircles[0];
return speed;//处理float类型的取绝对值
//return fabsf(speed);//处理float类型的取绝对值
}
float calc_Wheel2_speed(void)
{
float speed;
speed = (WheelCircles[1] - last_Wheelcircles[1])*MAX_ReadEncoderFreq;
last_Wheelcircles[1]= WheelCircles[1];
return (speed);//处理float类型的取绝对值
}
float calc_Wheel3_speed(void)
{
float speed;
speed = (WheelCircles[2] - last_Wheelcircles[2])*MAX_ReadEncoderFreq;
last_Wheelcircles[2]= WheelCircles[2];
return (speed);//处理float类型的取绝对值
}
float calc_Wheel4_speed(void)
{
float speed;
speed = (WheelCircles[3] - last_Wheelcircles[3])*MAX_ReadEncoderFreq;
last_Wheelcircles[3]= WheelCircles[3];
return (speed);//处理float类型的取绝对值
}
void ClearEncoder(void)
{
TIM_SetCounter(TIM2, 0);/*CNT设初值*/
TIM_SetCounter(TIM3, 0);/*CNT设初值*/
TIM_SetCounter(TIM4, 0);/*CNT设初值*/
TIM_SetCounter(TIM5, 0);/*CNT设初值*/
WheelCircles[0]=0;
WheelCircles[1]=0;
WheelCircles[2]=0;
WheelCircles[3]=0;
}
//*****************************定时器13,定时20ms。
//通用定时器13初始化
//arr:自动重装值。
//psc:时钟预分频数
//定时器溢出时间计算方法:Tout=((arr+1)*(psc+1))/Ft us.
//Ft=定时器工作频率,单位:Mhz
// //定时器13,定时监控初始化,
//定时器13时钟挂载在84Mhz,这里定时20ms,分频84,1us计算一次。
//那么20ms/1us=20 000/1us=20000
//这里要是0.1ms时基,那么是f=1/100us=0.01Mhz;
//TIM13_updateEncoder_init(MAX_DELTA_50HZ-1,8400-1); //20ms监控一次
void TIM13_updateEncoder_init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM13,ENABLE); ///使能TIM7时钟
TIM_TimeBaseInitStructure.TIM_Period = arr; //自动重装载值
TIM_TimeBaseInitStructure.TIM_Prescaler=psc; //定时器分频
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM13,&TIM_TimeBaseInitStructure);//初始化TIM7
TIM_ITConfig(TIM13,TIM_IT_Update,ENABLE); //允许定时器6更新中断
TIM_Cmd(TIM13,DISABLE); //初始化时先不开启定时器7
NVIC_InitStructure.NVIC_IRQChannel=TIM8_UP_TIM13_IRQn; //定时器6中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
//定时器13中20ms,读取一次编码器的速度等信息
void TIM8_UP_TIM13_IRQHandler(void)
{
if(TIM_GetITStatus(TIM13,TIM_IT_Update)==SET) //溢出中断
{
update_All_encoder();
WheelSpeed[0]=calc_Wheel1_speed();
WheelSpeed[1]=calc_Wheel2_speed();
WheelSpeed[2]=calc_Wheel3_speed();
WheelSpeed[3]=calc_Wheel4_speed();
TIM_ClearITPendingBit(TIM13,TIM_IT_Update); //清除中断标志位
}
}
encoder.c--20220327
最新推荐文章于 2022-05-03 20:22:44 发布