File content as of revision 1:cbd6a3232d5b:

#include "motor.h"


//////////////////////////////////////////////////////////////////////////////////   
//功能：电机结构体初始化
//参数：Motor：电机控制结构体
//返回值：无                           
//////////////////////////////////////////////////////////////////////////////////
void StepMotorInit(MotorType *Motor, DigitalOut *MotorDirPin, DigitalOut *EncoderCsPin, SPI *SPI_EncoderObj)
{
    /*RCC->APB1ENR|=1<<0;     //TIM2时钟使能   
    RCC->APB2ENR|=1<<2;     //GPIOA时钟使能 
    GPIOA->CRL&=0XFFFFFF0F; //PA1清除之前的设置
    GPIOA->CRL|=0X000000B0; //复用功能输出 
    
    TIM2->ARR=899;          //设定计数器自动重装值  定时器时钟频率 72K 固定的
    TIM2->PSC=999;          //预分频器设置
    
    TIM2->CCMR1|=7<<12;     //CH2 PWM2模式         
    TIM2->CCMR1|=1<<11;         //CH2预装载使能   
    TIM2->CCER|=1<<4;       //OC2 输出使能  

    TIM2->CR1=0x0080;       //ARPE使能
    TIM2->CR1|=0x01;        //使能定时器2*/
    
    RCC->APB2ENR|=1<<11;    //TIM1时钟使能    
    RCC->APB2ENR|=1<<2;     //GPIOA时钟使能 
    GPIOA->CRH&=0XFFFFFFF0; //PA8清除之前的设置
    GPIOA->CRH|=0X0000000B; //复用功能输出 
    
    TIM1->ARR=899;          //设定计数器自动重装值 
    TIM1->PSC=999;          //预分频器设置
  
    TIM1->CCMR1|=7<<4;      //CH1 PWM2模式         
    TIM1->CCMR1|=1<<3;      //CH1预装载使能   
    TIM1->CCER|=1<<0;       //OC1 输出使能    

    TIM1->BDTR|=1<<15;      //MOE 主输出使能    

    TIM1->CR1=0x0080;       //ARPE使能 
    TIM1->CR1|=0x01;        //使能定时器1                                       
    
    //初始胡电机控制结构体
    Motor->Encoder = 0;
    Motor->Difference = 0;
    Motor->Goal = 0;
    Motor->Integral = 0;
    Motor->PIDControl = 0;
    Motor->Kp = (float)MotorKp;
    Motor->Ki = (float)MotorKi;
    Motor->INTCounter = 0;
    Motor->EncoderPre = 0;
    Motor->ID = ID_Motor1;
    Motor->Status = MotorStatus_Running;
    Motor->Dir = 0; //默认使编码器读数减小的方向
    Motor->ChangeRatePIDControl = 0;
    Motor->PIDControl_pre = 0;
    Motor->Velocity = 0;                   
    Motor->V_PIDControl = 0;               
    Motor->V_PIDControl_pre = 0;       
    Motor->V_Difference = 0;             
    Motor->V_Integral = 0;                   
    Motor->V_Kp = MotorVKp;                            
    Motor->V_Ki = MotorVKi;                            
    Motor->V_Goal = 0;           
    Motor->GoalUpdated = 0;
    Motor->RevolveTimes = 0;
    Motor->ZeroPosition = 0;
    Motor->PresentPosition = 0;
    Motor->ZeroPosFound = 0;
    Motor->ZeroPosFoundAll = 0;
    Motor->Init_State = MotorInitState_Out;
    Motor->Init_ErrorTimes = 0;
    Motor->LagFiliterValue = 0;
    Motor->LagFiliterValuePre = 0;
    Motor->ChangeRateLagFiliterValue = 0;
    Motor->ChangeRateLagFiliterValueDigital = 0;
    Motor->ChangeRateLagFiliterValueDigitalPre = 0;
    Motor->InZeroZone = 0;
    Motor->ZeroZoneCounter = 0;
    Motor->BigStepCounter = 0;
    Motor->BigStepEncoderValue = 0;
    Motor->BigStepEncoderValuePre = 0;
    Motor->BigStepRevolveTimes = 0;
    Motor->HandUp1 = 0;
    Motor->HandUp2 = 0;
    Motor->EncoderTemp = 0;
    Motor->ChangeRatePIDControlPre = 0;
    Motor->MotorDir = MotorDirPin;
    Motor->EncoderCs = EncoderCsPin;
    Motor->SPI_Encoder = SPI_EncoderObj;
}

//////////////////////////////////////////////////////////////////////////////////   
//功能：电机控制的PI调节器
//参数：Motor：要计算PID控制量的电机对应的电机控制结构体
//返回值：无                           
//////////////////////////////////////////////////////////////////////////////////
void MotorPIDController(MotorType *Motor)
{
    char ChangeRateDifferenceLimit = 50;
    
    //位置偏差滤波 限制上升率 和 限制幅值 
    Motor->Difference_pre = Motor->Difference;
    Motor->Difference = -(Motor->Encoder- Motor->Goal);  //计算偏差
    Motor->ChangeRateDifference = Motor->Difference - Motor->Difference_pre;
    
    //切换参数
    /*if(Math_abs(Motor->Difference) <= ApproachDistance)  //很接近目标距离了
    {
        Motor->Ki = MotorKi_End;
        Motor->Kp = MotorKp_End;
        Motor->V_Ki = MotorVKi_End;
        Motor->V_Kp = MotorVKp_End;
    }
    else if(Motor->Velocity <= 15)
    {
        Motor->Ki = MotorKi_Start;
        Motor->Kp = MotorKp_Start;
        Motor->V_Ki = MotorVKi_Start;
        Motor->V_Kp = MotorVKp_Start;
    }
    else
    {
        Motor->Ki = MotorKi;
        Motor->Kp = MotorKp;
        Motor->V_Ki = MotorVKi;
        Motor->V_Kp = MotorVKp;
    }*/
        
        Motor->Integral += Motor->Difference;  //计算积分量
        if(Motor->Integral>15000)   Motor->Integral=15000; //积分限幅
        if(Motor->Integral<-15000)  Motor->Integral=-15000;  
    
        //速度目标值
        Motor->V_Goal = (Motor->Kp * Motor->Difference + Motor->Ki * Motor->Integral)/10; //速度环目标值
        Motor->GoalUpdated = 0;
    
    
        Motor->Velocity = Motor->Encoder - Motor->EncoderPre;
        Motor->V_Difference = -(Motor->Velocity - Motor->V_Goal);   //速度偏差
        Motor->V_Integral += Motor->V_Difference;
        if(Motor->V_Integral>6000)      Motor->V_Integral=6000; //积分限幅
        if(Motor->V_Integral<-6000) Motor->V_Integral=-6000; 
        Motor->PIDControl = (Motor->V_Kp * Motor->V_Difference + Motor->V_Ki * Motor->V_Integral); //计算控制量以及相应的数据处理
        
        
        if(Motor->PIDControl < 0)
        {
            Motor->PIDControl = -Motor->PIDControl;
            Motor->Dir = 0;
            Motor->MotorDir->write(0);
        }
        else
        {
            Motor->Dir = 1;
            Motor->MotorDir->write(1);
        }
        
        //判断是否进入误差容限
        if(Math_abs(Motor->Difference) <= ErrorTolerance)
        {
            if(Motor->INTCounter <= INT_TimesMin)
                Motor->INTCounter++;
            else
            {
                Motor->PIDControl = 0;
                Motor->Status = MotorStatus_OK;
            }
        }
        else
        {
            Motor->INTCounter = 0;
            Motor->Status = MotorStatus_Running;
        }
        
        //限幅
        if(Motor->PIDControl > PulseFreqLimit)
            Motor->PIDControl = PulseFreqLimit;
        
}

//////////////////////////////////////////////////////////////////////////////////   
//功能：求绝对值
//参数：num 带求绝对值的函数
//返回值：num的绝对值                             
//////////////////////////////////////////////////////////////////////////////////
int Math_abs(int num)
{
    if(num < 0)
        return -num;
    else
        return num;
}

//编码器初始化
void Encoder_Init(MotorType *Motor)
{
    Motor->EncoderCs->write(1);
    Motor->SPI_Encoder->format(16,1);
    Motor->SPI_Encoder->frequency(100000);
}

//读取编码器值
unsigned int Encoder_ReadData(MotorType *Motor)
{
    Motor->EncoderCs->write(0);
    int response = Motor->SPI_Encoder->write(0xFFFF);
    response &= 0x3fff;
    Motor->EncoderCs->write(1);
    return response;
}

//////////////////////////////////////////////////////////////////////////////////   
//功能：一阶低通滤波
//参数：Value_now 本次样值 Value_pre 上次滤波输出值
//返回值：本次滤波输出值                         
//////////////////////////////////////////////////////////////////////////////////
int Fliter_1(int Value_now, int Value_pre)
{
    return (int)(FlierCoefficient*(float)Value_now+(1-FlierCoefficient)*(float)Value_pre);
}

//////////////////////////////////////////////////////////////////////////////////   
//功能：设置步进电机脉冲信号的PWM频率
//
//返回值：无                           
//////////////////////////////////////////////////////////////////////////////////
void StepMotorSetFreq(MotorType *Motor)
{
    unsigned int Motor_ARR;
    //为了使频率步长1Hz平滑可调需要分段设置定时器的时钟频率，即满足(Tclk/freq-Tclk/freq)>1
    //并且考虑到余量与ARR寄存器只有16位，设置分段如下
    //8k-5k 72M
    //5k-1k 36M
    //1k-500 2M
    //500-100 720k
    //100-1 72k
    //Motor->PIDControl = 5000;
    /*if(Motor->PIDControl > PulseFreqLimit)
        Motor->PIDControl = PulseFreqLimit;
    else if((Motor->PIDControl <= PulseFreqLimit) && (Motor->PIDControl > 5000))
        TIM2->PSC=0;            //预分频器设置
    else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 1000))
        TIM2->PSC=1;            //预分频器设置
    else if((Motor->PIDControl <= 1000) && (Motor->PIDControl > 500))
        TIM2->PSC=35;           //预分频器设置
    else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 100))
        TIM2->PSC=99;           //预分频器设置
    else if((Motor->PIDControl <= 100) && (Motor->PIDControl > 0))
        TIM2->PSC=999;          //预分频器设置
    else
    {
        Motor->PIDControl = 1;
        TIM2->PSC=999;          //预分频器设置
    }
    
    Motor_ARR = APB2Freq / (TIM2->PSC+1) / Motor->PIDControl;
    TIM2->ARR = Motor_ARR;
    TIM2->CCR2 = Motor_ARR/2;*/
    if(Motor->PIDControl > PulseFreqLimit)
        Motor->PIDControl = PulseFreqLimit;
    else if((Motor->PIDControl <= PulseFreqLimit) && (Motor->PIDControl > 5000))
        TIM1->PSC=0;            //预分频器设置
    else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 1000))
        TIM1->PSC=1;            //预分频器设置
    else if((Motor->PIDControl <= 1000) && (Motor->PIDControl > 500))
        TIM1->PSC=35;           //预分频器设置
    else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 100))
        TIM1->PSC=99;           //预分频器设置
    else if((Motor->PIDControl <= 100) && (Motor->PIDControl > 0))
        TIM1->PSC=999;          //预分频器设置
    else
    {
        Motor->PIDControl = 1;
        TIM1->PSC=999;          //预分频器设置
    }
    
    Motor_ARR = APB2Freq / (TIM1->PSC+1) / Motor->PIDControl;
    //printf("ll%dll \r\n", Motor_ARR);
    TIM1->ARR = Motor_ARR;
    TIM1->CCR1 = Motor_ARR/2;
}

//控制接口
void Communication(Serial *Uart, MotorType *Motor)
{
    char UartTemp = 0;      //串口缓存
    if(Uart->readable())
    {
        
    }
}