STM32正交编码器驱动，引入（突变）带进位的位置环和速度环

STM32

`参考资料：
1.增量式编码器的A.B.Z - 轩辕赤血的日志 - 网易博客（本人没有权利发URL。。。）

2.STM32官方的stm32正交编码器驱动代码（在原来的基础上进行了修改）

由于时间关系，我先写一部分，然后在楼下回复续写，不便之处，敬请见谅！

1.正交编码器部分（大家可以参考上面的链接理解，我只讲解有用的部分）

1.1 何为正交编码器
该编器除了供电（3.3V-5V）外，有A，B两相的输出（直观看就是两条线）
编码器是由一个中心有轴的光电码盘，其上有环形通、暗的刻线，有光电发射和接收器件读取，比如通则为高电平，不同则为低电平，于是就有脉冲输出了
下面看下编码器的输出波形：

A，B相永远相差90度，即正交的意思
1.2 为何不使用一相AB
正转时，A相超前B相90°先进行相位输出，反转时，B相超前A相90°先进行相位输出，可以通过这个来判断电机正反转
1.3分辨率
参考资料里指出：
分辨率—编码器以每旋转360度提供多少的通或暗（这里是或的关系，注意）刻线称为分辨率，也称解析分度、或直接称多少线，一般在每转分度5~10000线
我采用的电机是ESCAP 16MM迷宫鼠专用电机(二手电机有许多问题) 512线的（也有说法说是128线然后4倍频输出的）

先写到这里吧，下午还需要上课

搜索

复制

搜索

复制

`

鬼谷左原 · 2015-5-22 13:32:13

增量式编码器的A.B.Z - 轩辕赤血的日志 - 网易博客

需要连接的可以留下邮箱，或者搜索一下上面的关键字

鬼谷左原 · 2015-5-25 20:51:51

正交编码器：

参考资料：
STM32中文参考手册V1.0 定时器章节，编码器接口一节

具体内容参考一下参考资料,这里只讲我认为有疑惑的几点：

1.附件中TI1，TI2所映射的管脚TIMx_CH1,TIM_CH2连接正交编码器AB相

2.如果电机是一直正转会发生什么情况，当然是会发生更新时间，可以利用这个来获取速度，特别是获取速度的表达式：这次的计数值-上一次的计数值，如果是在更新时件发生，那算出的速度一定会有突变，但现实的速度没有突变，具体处理请看程序的定时器中断。

3.同样的，位置环的计算，不能简单是使用进中断的次数，最好使用对速度的积分（做个累加即可）

鬼谷左原 · 2015-5-25 20:54:23

/******************** (C) COPYRIGHT 2007 STMicroelectronics ********************
* File Name       : stm32f10x_encoder.c
* Author          : IMS Systems Lab
* Date First Issued  : 21/11/07
* Description       : This file contains the software implementation for the
*                   encoder unit
********************************************************************************
* History:
* 21/11/07 v1.0
********************************************************************************
* THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "stm32f10x_encoder.h"
#include "sys.h"
#include "usart.h"
#include "led.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/

#define COUNTER_RESET (u16)0
#define ICx_FILTER    (u8) 0 // 6<-> 670nsec

#define TIMx_PRE_EMPTION_PRIORITY 1
#define TIMx_SUB_PRIORITY 0

//#define MAX_COUNT ENCODER_TIM_PERIOD/2

/* Private functions ---------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/

volatile s16 Right_hEncoder_Timer_Overflow; //编码器计数值溢出标志
volatile s16 Right_hEncoder_Timer_Overflow_High; //编码器计数值溢出标志高位
volatile s16 Right_hRot_Speed; //当前的速度
volatile s16 Right_hRot_Acceleration; //当前的加速度

s32 Right_CurrentCount = 0; //编码器当前的总计数值
s32 Right_CurrentCount_high = 0; //编码器当前的总计数值高位  计算公式，注意结果可能超过32位的表示范围Left_CurrentCount_high*S32_MAX+Left_CurrentCount

volatile s16 Left_hEncoder_Timer_Overflow;
volatile s16 Left_hEncoder_Timer_Overflow_High;
volatile s16 Left_hRot_Speed;
volatile s16 Left_hRot_Acceleration;

s32 Left_CurrentCount = 0;
s32 Left_CurrentCount_high = 0;
/*******************************************************************************
* Function Name  : ENC_Right_Init
* Description : General Purpose Timer x set-up for encoder speed/position
*                sensors
* Input       : None
* Output       : None
* Return       : None
*******************************************************************************/
void ENC_Right_Init(void)
{
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  TIM_ICInitTypeDef TIM_ICInitStructure;

/* Encoder unit connected to TIM3, 4X mode */
  GPIO_InitTypeDef GPIO_InitStructure;
  NVIC_InitTypeDef NVIC_InitStructure;

  /* TIM3 clock source enable */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
  /* Enable GPIOA, clock */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

  GPIO_StructInit(&GPIO_InitStructure);
  /* Configure PA.06,07 as encoder input */
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; //A7接靠近红色的编码器输入一端
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(GPIOA, &GPIO_InitStructure);

  /* Enable the TIM3 Update Interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = TIMx_PRE_EMPTION_PRIORITY;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = TIMx_SUB_PRIORITY;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  /* Timer configuration in Encoder mode */
  TIM_DeInit(ENCODER_Right_TIMER);
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

  TIM_TimeBaseStructure.TIM_Prescaler = 0x0;  // No prescaling
  TIM_TimeBaseStructure.TIM_Period = (4*ENCODER_PPR)-1;
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit(ENCODER_Right_TIMER, &TIM_TimeBaseStructure);
//TIMx_SMCR 寄存器中的 SMS=011 = TIM_EncoderMode_TI12；
  TIM_EncoderInterfaceConfig(ENCODER_Right_TIMER, TIM_EncoderMode_TI12,
                           TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); //ICx 不反相输入捕获在上升沿
  TIM_ICStructInit(&TIM_ICInitStructure); //TIM_Channel_1 TIM_ICPSC_DIV1 TIM_ICPolarity_Rising
  TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER; //输入滤波 0110：采样频率fSAMPLING=fDTS/4， N=6
  TIM_ICInit(ENCODER_Right_TIMER, &TIM_ICInitStructure);

// Clear all pending interrupts
  TIM_ClearFlag(ENCODER_Right_TIMER, TIM_FLAG_Update);
  TIM_ITConfig(ENCODER_Right_TIMER, TIM_IT_Update, ENABLE);
  //Reset counter
  ENCODER_Right_TIMER->CNT = COUNTER_RESET;



  TIM_Cmd(ENCODER_Right_TIMER, ENABLE);
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
Right_hEncoder_Timer_Overflow = 0;
Right_hEncoder_Timer_Overflow_High = 0;
Right_hRot_Speed = 0;
Right_ENC_Get_Electrical_Angle(); //第一次不要这个数据！！lastCount
}

/*******************************************************************************
* Function Name  : ENC_Right_Init
* Description : General Purpose Timer x set-up for encoder speed/position
*                sensors
* Input       : None
* Output       : None
* Return       : None
*******************************************************************************/
void ENC_Left_Init(void)
{
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  TIM_ICInitTypeDef TIM_ICInitStructure;

/* Encoder unit connected to TIM2 mode */
  GPIO_InitTypeDef GPIO_InitStructure;
  NVIC_InitTypeDef NVIC_InitStructure;

  /* TIM2 clock source enable */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
  /* Enable GPIOA, clock */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

  GPIO_StructInit(&GPIO_InitStructure);
  /* Configure PA.00,01 as encoder input */
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1; //A7接靠近红色的编码器输入一端
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GPIO_Init(GPIOA, &GPIO_InitStructure);

  /* Enable the TIM2 Update Interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = TIMx_PRE_EMPTION_PRIORITY;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = TIMx_SUB_PRIORITY+1;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  /* Timer configuration in Encoder mode */
  TIM_DeInit(ENCODER_Left_TIMER);
  TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);

  TIM_TimeBaseStructure.TIM_Prescaler = 0x0;  // No prescaling
  TIM_TimeBaseStructure.TIM_Period = (4*ENCODER_PPR)-1;
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit(ENCODER_Left_TIMER, &TIM_TimeBaseStructure);
//TIMx_SMCR 寄存器中的 SMS=011 = TIM_EncoderMode_TI12；
  TIM_EncoderInterfaceConfig(ENCODER_Left_TIMER, TIM_EncoderMode_TI12,
                           TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); //ICx 不反相输入捕获在上升沿
  TIM_ICStructInit(&TIM_ICInitStructure); //TIM_Channel_1 TIM_ICPSC_DIV1 TIM_ICPolarity_Rising
  TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER; //输入滤波 0110：采样频率fSAMPLING=fDTS/4， N=0
  TIM_ICInit(ENCODER_Left_TIMER, &TIM_ICInitStructure);

// Clear all pending interrupts
  TIM_ClearFlag(ENCODER_Left_TIMER, TIM_FLAG_Update);
  TIM_ITConfig(ENCODER_Left_TIMER, TIM_IT_Update, ENABLE);
  //Reset counter
  ENCODER_Left_TIMER->CNT = COUNTER_RESET;



  TIM_Cmd(ENCODER_Left_TIMER, ENABLE);
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
__nop();
Left_hEncoder_Timer_Overflow = 0;
Left_hEncoder_Timer_Overflow_High = 0;
Left_hRot_Speed = 0;
Left_ENC_Get_Electrical_Angle(); //第一次不要这个数据！！lastCount
}

/*******************************************************************************
* Function Name  : Right_ENC_Get_Electrical_Angle
* Description : Returns the absolute electrical Rotor angle
* Input       : None
* Output       : None
* Return       : dAngle
//第一次不要这个数据！！lastCount
*******************************************************************************/
s16 Right_ENC_Get_Electrical_Angle(void)
{
static u16 lastCount = 0;
u16 curCount ;
s16 dAngle ;
static s16 Last_Overflow = 0;

curCount = ENCODER_Right_TIMER->CNT;
dAngle = curCount - lastCount;

//ensure    |Right_hEncoder_Timer_Overflow - Last_Overflow|<=1
if(Right_hEncoder_Timer_Overflow != Last_Overflow)
{

if ( (ENCODER_Right_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down) //反转
{// encoder timer down-counting
dAngle -= ENCODER_TIM_PERIOD;

}
else
{//encoder timer up-counting

dAngle += ENCODER_TIM_PERIOD;

}

}

lastCount = curCount;
Last_Overflow = Right_hEncoder_Timer_Overflow;
return (s16)dAngle;

}

/*******************************************************************************
* Function Name  : Left_ENC_Get_Electrical_Angle
* Description : Returns the absolute electrical Rotor angle
* Input       : None
* Output       : None
* Return       : dAngle
//第一次不要这个数据！！lastCount
*******************************************************************************/
s16 Left_ENC_Get_Electrical_Angle(void)
{
static u16 lastCount = 0;
u16 curCount ;
s16 dAngle ;
static s16 Last_Overflow = 0;

curCount = ENCODER_Left_TIMER->CNT;
dAngle = curCount - lastCount;

//ensure    |Left_hEncoder_Timer_Overflow - Last_Overflow|<=1
if(Left_hEncoder_Timer_Overflow != Last_Overflow)
{

if ( (ENCODER_Left_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down) //反转
{// encoder timer down-counting
dAngle -= ENCODER_TIM_PERIOD;

}
else
{//encoder timer up-counting

dAngle += ENCODER_TIM_PERIOD;

}

}

lastCount = curCount;
Last_Overflow = Left_hEncoder_Timer_Overflow;
return (s16)dAngle;

}

void TIM2_IRQHandler(void)
{
  /* Clear the interrupt pending flag */
  TIM_ClearFlag(ENCODER_Left_TIMER, TIM_FLAG_Update);

  if ((Left_hEncoder_Timer_Overflow != S16_MAX )&&(Left_hEncoder_Timer_Overflow != S16_MIN ))
  {
if ( (ENCODER_Left_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down) //反转
{// encoder timer down-counting
Left_hEncoder_Timer_Overflow--;
}
else
{//encoder timer up-counting
Left_hEncoder_Timer_Overflow++;
}

  }
if(Left_hEncoder_Timer_Overflow == S16_MAX)
{
Left_hEncoder_Timer_Overflow = 0;
      Left_hEncoder_Timer_Overflow_High++;
}
if(Left_hEncoder_Timer_Overflow == S16_MIN )
{
Left_hEncoder_Timer_Overflow = 0;
Left_hEncoder_Timer_Overflow_High--;

}

}

void TIM3_IRQHandler(void)
{
  /* Clear the interrupt pending flag */
  TIM_ClearFlag(ENCODER_Right_TIMER, TIM_FLAG_Update);

  if ((Right_hEncoder_Timer_Overflow != S16_MAX )&&(Right_hEncoder_Timer_Overflow != S16_MIN ))
  {
if ( (ENCODER_Right_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down) //反转
{// encoder timer down-counting
Right_hEncoder_Timer_Overflow--;

}
else
{//encoder timer up-counting
Right_hEncoder_Timer_Overflow++;
}

  }
if(Right_hEncoder_Timer_Overflow == S16_MAX)
{
Right_hEncoder_Timer_Overflow = 0;
      Right_hEncoder_Timer_Overflow_High++;
}
if(Right_hEncoder_Timer_Overflow == S16_MIN )
{
Right_hEncoder_Timer_Overflow = 0;
Right_hEncoder_Timer_Overflow_High--;

}

}

鬼谷左原 · 2015-5-25 20:58:22

以上使用了两个定时器来做编码，具体的一些宏我在这里补齐：
#define U32_MAX 4294967295U
#define U16_MAX 65535
#define S16_MAX 32767
#define S16_MIN -32768
#define S32_MAX 2147483647U
#define S32_MIN -2147483647U

#define ENCODER_Left_TIMER TIM2  // Encoder Left unit connected to TIM2
#define ENCODER_Right_TIMER TIM3  // Encoder Rigth unit connected to TIM3

#define ENCODER_PPR          (u16)(512*8) //(4*ENCODER_PPR)number of pulses per revolution

#define ENCODER_TIM_PERIOD    (u16)((4*ENCODER_PPR))

鬼谷左原 · 2015-5-25 21:17:57

通过以上的程序，（在主函数里初始化，就可以得到使用deltaCount = (s32)Right_ENC_Get_Electrical_Angle();得到每次的增量）

接下来就是位置信息的处理了，和速度的处理（我这些都是在定时器里处理的，每10ms处理一次，为何使用10MS 是更电机的速度有关的，我的电机是540r/min 所以一转不会小于10ms，同时响应太快了也不好，计算的速度小了，精度就低了）

1.1速度（速度不是已经得到了吗？为何要处理？？速度最好做一下滤波，滤波的方法有很多）

这里使用的移动均值滤波，思想比较简单，这里不加以阐述

要注意的是，移动均值滤波是有滞后的（10ms），不过比简单的平均值滤波滞后小（同样的响应速度下）

1.2位置环的算法就是每次对速度进行累加，但是32位的有符号的数据处理起来表麻烦，比如说：以8位有符号为例，最大是127，如果我当前的位置是120（是不确定的哦！！

0，算出的当前速度为20，那当然就超出8位的表示范围了，所以需要一个进位标志
具体怎么处理：
//Right calculate
deltaCount = (s32)Right_ENC_Get_Electrical_Angle();

if(((Right_CurrentCount +deltaCount)>0)&&(Right_CurrentCount +deltaCount)>= S32_MAX)
{

Right_CurrentCount = -(S32_MAX-Right_CurrentCount);
Right_CurrentCount_high++;

}
else if(((Right_CurrentCount +deltaCount)<0)&&(0-(Right_CurrentCount +deltaCount))>= S32_MAX )
{

Right_CurrentCount = S32_MAX+Right_CurrentCount;
Right_CurrentCount_high--;

}

Right_CurrentCount += deltaCount;

这里高能，如果大家细心的话会发现负数的处理比较特别，这个举例说明：如果我现在计算的位置即将超过-128 那么最高位（1--符号位），会被清零，就会变成正数来比较，同个这个算法，就相当了与求了绝对值表示的范围又多了

鬼谷左原 · 2015-5-25 21:19:54

这个是是速度处理的程序：

Right_hSpeed_Buffer[Right_Speed_Buffer_Index & 1] = deltaCount;
Right_Speed_Buffer_Index = (Right_Speed_Buffer_Index)%0xff;
Right_Speed_Buffer_Index++;

acc = 0;
for (i = 0; i < SPEED_BUFFER_SIZE ; i++)
{
acc += Right_hSpeed_Buffer[i];
}

Right_hRot_Acceleration = Right_hSpeed_Buffer[1]-Right_hSpeed_Buffer[0];

Right_hRot_Speed = (s16)(acc/SPEED_BUFFER_SIZE);

鬼谷左原 · 2015-5-25 21:21:43

这里先给出位置环和速度环的控制代码：

/* Includes ------------------------------------------------------------------*/
#include "stm32f10x.h"
#include "sys.h"
#include "stm32f10x_encoder.h"
#include "led.h"
#include "pid.h"
#include "pwm.h"
#include "usart.h"
// #define Rot_Speed_RPM    (540+100 ) //电机每分钟多少转

#define Cycle_Time    10000//us --10ms10000
#define SysTick_Reloadvalue  (72*Cycle_Time-1)
#define HCLK_DIV 0

void right_deal(void);
void left_deal(void);
//#define IS_SYSTick_Fit(Cy_time) (Cy_time > 0 &&  ( Cy_time <  (72*60000000U)/(Rot_Speed_RPM*2) ))
void SysTick_Configuration(void)
{
//assert_param(IS_SYSTick_Fit(SysTick_Reloadvalue));

if(HCLK_DIV)
/*select systick clock = sysclk8*/
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8);
else
// /*select systick clock = sysclk*/
SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK);

SysTick_Config(SysTick_Reloadvalue);

}

void SysTick_Handler(void)
{

right_deal();

left_deal();

}
//右轮子速度，位置，加速度计算，以及位置环控制，速度环控制
void right_deal(void)
{
static u8 Right_Speed_Buffer_Index = 0;
static s32 Right_hSpeed_Buffer[SPEED_BUFFER_SIZE] = {0,0};

u16 Right_PWM_OC2_ReloadValue = 0;

u8 i = 0;

s32 acc = 0;
s32 deltaCount = 0; //定时器脉冲计数值的增量
s32 speed_error = 0;
s32 pos_error = 0;
s16 speed_deltaout = 0;
s16 pos_detaout = 0;

//Right calculate
deltaCount = (s32)Right_ENC_Get_Electrical_Angle();

//printf("right x is %drn",Right_CurrentCount_high*S32_MAX+Right_CurrentCount);

if(((Right_CurrentCount +deltaCount)>0)&&(Right_CurrentCount +deltaCount)>= S32_MAX)
{

Right_CurrentCount = -(S32_MAX-Right_CurrentCount);
Right_CurrentCount_high++;

}
else if(((Right_CurrentCount +deltaCount)<0)&&(0-(Right_CurrentCount +deltaCount))>= S32_MAX )
{

Right_CurrentCount = S32_MAX+Right_CurrentCount;
Right_CurrentCount_high--;

}

Right_CurrentCount += deltaCount;

Right_hSpeed_Buffer[Right_Speed_Buffer_Index & 1] = deltaCount;
Right_Speed_Buffer_Index = (Right_Speed_Buffer_Index)%0xff;
Right_Speed_Buffer_Index++;

acc = 0;
for (i = 0; i < SPEED_BUFFER_SIZE ; i++)
{
acc += Right_hSpeed_Buffer[i];
}

Right_hRot_Acceleration = Right_hSpeed_Buffer[1]-Right_hSpeed_Buffer[0];

Right_hRot_Speed = (s16)(acc/SPEED_BUFFER_SIZE);

speed_error = Right_Speed_PID.targetValue - Right_hRot_Speed; //(speed_error = -(Right_hRot_Speed - Right_Speed_PID.targetValue ))
speed_error = speed_error/4;//为什么除4
speed_deltaout = calcPID(&Right_Speed_PID,speed_error);
Right_Speed_AutoReload += speed_deltaout;

if (  (Right_Speed_PID.targetValue==0 )&& (Right_Position_Target_Flag==1))
{
if(Right_Position_Target_First == 1) //第一次为零
{
Right_Positon_PID.targetValue = Right_CurrentCount;
Right_Position_Target_First = 0;
//记录下当前的信息
Right_Position_targert_high = Right_CurrentCount_high;
}

// pos_error = (s32)(Right_Positon_PID.targetValue) - (s32)Right_CurrentCount+(s32)((s32) ((First_hEncoder_Timer_Overflow-Right_CurrentCount_high)*ENCODER_TIM_PERIOD)); //(speed_error = -(Right_CurrentCount - Right_Positon_PID.targetValue ))
// pos_error = pos_error/4;

pos_error = Right_Positon_PID.targetValue - Right_CurrentCount+(Right_Position_targert_high-Right_CurrentCount_high)*S32_MAX;
pos_error = pos_error/4;
//printf("pos_error is %d rn",pos_error);
//printf("Right_Position_targert_high-Right_CurrentCount_high is %d rn" ,Right_Position_targert_high-Right_CurrentCount_high);

pos_detaout = calcPID(&Right_Positon_PID,pos_error);
// printf("pos_detaout is %drn",pos_detaout);
if(pos_detaout >= 0)
{
if(pos_detaout >= position_width)
pos_detaout = position_width;
TIM_OC2PolarityConfig(TIM4,TIM_OCPolarity_Low);
GPIO_ResetBits(PWM_OC_GPIO,PWM_OC2_Polarity);
TIM_SetCompare2(TIM4,(u16)pos_detaout);
//printf("pos_detaout >= 0 rn");
}
else
{
if(pos_detaout <= -position_width)
pos_detaout = -position_width;
TIM_OC2PolarityConfig(TIM4,TIM_OCPolarity_High);
GPIO_SetBits(PWM_OC_GPIO,PWM_OC2_Polarity);

TIM_SetCompare2(TIM4,(u16)-pos_detaout);

//printf("pos_detaout < 0 rn");
}

}

else if (Right_Speed_PID.targetValue>0)
{
if(Right_Speed_AutoReload >= 0 ) //正转
{
TIM_OC2PolarityConfig(TIM4,TIM_OCPolarity_Low);
GPIO_ResetBits(PWM_OC_GPIO,PWM_OC2_Polarity);

Right_PWM_OC2_ReloadValue = Right_Speed_AutoReload;
if(Right_Speed_AutoReload > PWM_us-1)
{
Right_PWM_OC2_ReloadValue = PWM_us-1;
Right_Speed_AutoReload = PWM_us-1;
}
}
else if(Right_Speed_PID.targetValue<0)//速度为0
{
Right_PWM_OC2_ReloadValue = 0;
Right_Speed_AutoReload = 0;
}
TIM_SetCompare2(TIM4,Right_PWM_OC2_ReloadValue);
}
else
{
   if(Right_Speed_AutoReload < 0) //反转
{

      TIM_OC2PolarityConfig(TIM4,TIM_OCPolarity_High);
GPIO_SetBits(PWM_OC_GPIO,PWM_OC2_Polarity); //在PWM中断中做！！

if(Right_Speed_AutoReload <( -PWM_us+1))
{
Right_PWM_OC2_ReloadValue = PWM_us-1;
Right_Speed_AutoReload = -PWM_us+1;
}

else Right_PWM_OC2_ReloadValue = -Right_Speed_AutoReload;

}
else
{
Right_PWM_OC2_ReloadValue = 0;
Right_Speed_AutoReload = 0;

}

TIM_SetCompare2(TIM4,Right_PWM_OC2_ReloadValue);
}

}

鬼谷左原 · 2015-5-25 21:23:02

有些注释没有时间改好点请见谅
以后有时间在补齐，
本人要补作业

鬼谷左原 · 2015-5-25 21:31:05

补齐一个带符号的 PID计算算法：

s16 calcPID( PID *pid, s16 error)
{
s32 output;

if (pid->Ki != 0)
{
pid->integrationError += error;
// Limit the maximum integration error
if (pid->integrationError > MAX_INTEGRATION_ERROR)
{
pid->integrationError = MAX_INTEGRATION_ERROR;
}
else if (pid->integrationError < -MAX_INTEGRATION_ERROR)
{
pid->integrationError = -MAX_INTEGRATION_ERROR;
}
}

output = (s32)( pid->Kp * error + pid->Ki * pid->integrationError + pid->Kd * (error - pid->prevError) );

// Limit the maximum output
if (output > MAX_PID_OUTPUT)
{
output = MAX_PID_OUTPUT;
}
else if ((output<0) && (-output) > MAX_PID_OUTPUT)
{
output = -MAX_PID_OUTPUT;
}

pid->prevError = error;
  return (s16)output; //（为何要使用32位的，然后还有转化一下？同样的如果负数超出表示范围的话，会变成正数，正数也会变成是、负数，（符号位被改变）转化出 16位是为了自己的方便）
}
/* Exported Various Declare    --------------------------------------------------*/

/* Exported Macro Declare       --------------------------------------------------*/
#define MAX_PID_OUTPUT 500
#define MAX_INTEGRATION_ERROR 20

#define MAX_Speed_PID_targetValue 800
/* Exported types 结构体定义区 --------------------------------------------------*/

typedef struct {

s32 targetValue;

u16 Kp;
u16 Ki;
u16 Kd;

s16 integrationError;
s16 prevError;
}  PID;