Chen Huan
/
MotorPID
闭环步进电机
motor.cpp@1:cbd6a3232d5b, 2018-04-05 (annotated)
- Committer:
- heroistired
- Date:
- Thu Apr 05 01:37:17 2018 +0000
- Revision:
- 1:cbd6a3232d5b
- Parent:
- 0:5b4f19f8cd85
New
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
heroistired | 0:5b4f19f8cd85 | 1 | #include "motor.h" |
heroistired | 0:5b4f19f8cd85 | 2 | |
heroistired | 0:5b4f19f8cd85 | 3 | |
heroistired | 0:5b4f19f8cd85 | 4 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 5 | //功能:电机结构体初始化 |
heroistired | 0:5b4f19f8cd85 | 6 | //参数:Motor:电机控制结构体 |
heroistired | 0:5b4f19f8cd85 | 7 | //返回值:无 |
heroistired | 0:5b4f19f8cd85 | 8 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 9 | void StepMotorInit(MotorType *Motor, DigitalOut *MotorDirPin, DigitalOut *EncoderCsPin, SPI *SPI_EncoderObj) |
heroistired | 0:5b4f19f8cd85 | 10 | { |
heroistired | 1:cbd6a3232d5b | 11 | /*RCC->APB1ENR|=1<<0; //TIM2时钟使能 |
heroistired | 0:5b4f19f8cd85 | 12 | RCC->APB2ENR|=1<<2; //GPIOA时钟使能 |
heroistired | 0:5b4f19f8cd85 | 13 | GPIOA->CRL&=0XFFFFFF0F; //PA1清除之前的设置 |
heroistired | 0:5b4f19f8cd85 | 14 | GPIOA->CRL|=0X000000B0; //复用功能输出 |
heroistired | 0:5b4f19f8cd85 | 15 | |
heroistired | 0:5b4f19f8cd85 | 16 | TIM2->ARR=899; //设定计数器自动重装值 定时器时钟频率 72K 固定的 |
heroistired | 0:5b4f19f8cd85 | 17 | TIM2->PSC=999; //预分频器设置 |
heroistired | 0:5b4f19f8cd85 | 18 | |
heroistired | 0:5b4f19f8cd85 | 19 | TIM2->CCMR1|=7<<12; //CH2 PWM2模式 |
heroistired | 0:5b4f19f8cd85 | 20 | TIM2->CCMR1|=1<<11; //CH2预装载使能 |
heroistired | 0:5b4f19f8cd85 | 21 | TIM2->CCER|=1<<4; //OC2 输出使能 |
heroistired | 0:5b4f19f8cd85 | 22 | |
heroistired | 0:5b4f19f8cd85 | 23 | TIM2->CR1=0x0080; //ARPE使能 |
heroistired | 1:cbd6a3232d5b | 24 | TIM2->CR1|=0x01; //使能定时器2*/ |
heroistired | 1:cbd6a3232d5b | 25 | |
heroistired | 1:cbd6a3232d5b | 26 | RCC->APB2ENR|=1<<11; //TIM1时钟使能 |
heroistired | 1:cbd6a3232d5b | 27 | RCC->APB2ENR|=1<<2; //GPIOA时钟使能 |
heroistired | 1:cbd6a3232d5b | 28 | GPIOA->CRH&=0XFFFFFFF0; //PA8清除之前的设置 |
heroistired | 1:cbd6a3232d5b | 29 | GPIOA->CRH|=0X0000000B; //复用功能输出 |
heroistired | 1:cbd6a3232d5b | 30 | |
heroistired | 1:cbd6a3232d5b | 31 | TIM1->ARR=899; //设定计数器自动重装值 |
heroistired | 1:cbd6a3232d5b | 32 | TIM1->PSC=999; //预分频器设置 |
heroistired | 1:cbd6a3232d5b | 33 | |
heroistired | 1:cbd6a3232d5b | 34 | TIM1->CCMR1|=7<<4; //CH1 PWM2模式 |
heroistired | 1:cbd6a3232d5b | 35 | TIM1->CCMR1|=1<<3; //CH1预装载使能 |
heroistired | 1:cbd6a3232d5b | 36 | TIM1->CCER|=1<<0; //OC1 输出使能 |
heroistired | 1:cbd6a3232d5b | 37 | |
heroistired | 1:cbd6a3232d5b | 38 | TIM1->BDTR|=1<<15; //MOE 主输出使能 |
heroistired | 1:cbd6a3232d5b | 39 | |
heroistired | 1:cbd6a3232d5b | 40 | TIM1->CR1=0x0080; //ARPE使能 |
heroistired | 1:cbd6a3232d5b | 41 | TIM1->CR1|=0x01; //使能定时器1 |
heroistired | 0:5b4f19f8cd85 | 42 | |
heroistired | 0:5b4f19f8cd85 | 43 | //初始胡电机控制结构体 |
heroistired | 0:5b4f19f8cd85 | 44 | Motor->Encoder = 0; |
heroistired | 0:5b4f19f8cd85 | 45 | Motor->Difference = 0; |
heroistired | 0:5b4f19f8cd85 | 46 | Motor->Goal = 0; |
heroistired | 0:5b4f19f8cd85 | 47 | Motor->Integral = 0; |
heroistired | 0:5b4f19f8cd85 | 48 | Motor->PIDControl = 0; |
heroistired | 0:5b4f19f8cd85 | 49 | Motor->Kp = (float)MotorKp; |
heroistired | 0:5b4f19f8cd85 | 50 | Motor->Ki = (float)MotorKi; |
heroistired | 0:5b4f19f8cd85 | 51 | Motor->INTCounter = 0; |
heroistired | 0:5b4f19f8cd85 | 52 | Motor->EncoderPre = 0; |
heroistired | 0:5b4f19f8cd85 | 53 | Motor->ID = ID_Motor1; |
heroistired | 0:5b4f19f8cd85 | 54 | Motor->Status = MotorStatus_Running; |
heroistired | 0:5b4f19f8cd85 | 55 | Motor->Dir = 0; //默认使编码器读数减小的方向 |
heroistired | 0:5b4f19f8cd85 | 56 | Motor->ChangeRatePIDControl = 0; |
heroistired | 0:5b4f19f8cd85 | 57 | Motor->PIDControl_pre = 0; |
heroistired | 0:5b4f19f8cd85 | 58 | Motor->Velocity = 0; |
heroistired | 0:5b4f19f8cd85 | 59 | Motor->V_PIDControl = 0; |
heroistired | 0:5b4f19f8cd85 | 60 | Motor->V_PIDControl_pre = 0; |
heroistired | 0:5b4f19f8cd85 | 61 | Motor->V_Difference = 0; |
heroistired | 0:5b4f19f8cd85 | 62 | Motor->V_Integral = 0; |
heroistired | 0:5b4f19f8cd85 | 63 | Motor->V_Kp = MotorVKp; |
heroistired | 0:5b4f19f8cd85 | 64 | Motor->V_Ki = MotorVKi; |
heroistired | 0:5b4f19f8cd85 | 65 | Motor->V_Goal = 0; |
heroistired | 0:5b4f19f8cd85 | 66 | Motor->GoalUpdated = 0; |
heroistired | 0:5b4f19f8cd85 | 67 | Motor->RevolveTimes = 0; |
heroistired | 0:5b4f19f8cd85 | 68 | Motor->ZeroPosition = 0; |
heroistired | 0:5b4f19f8cd85 | 69 | Motor->PresentPosition = 0; |
heroistired | 0:5b4f19f8cd85 | 70 | Motor->ZeroPosFound = 0; |
heroistired | 0:5b4f19f8cd85 | 71 | Motor->ZeroPosFoundAll = 0; |
heroistired | 0:5b4f19f8cd85 | 72 | Motor->Init_State = MotorInitState_Out; |
heroistired | 0:5b4f19f8cd85 | 73 | Motor->Init_ErrorTimes = 0; |
heroistired | 0:5b4f19f8cd85 | 74 | Motor->LagFiliterValue = 0; |
heroistired | 0:5b4f19f8cd85 | 75 | Motor->LagFiliterValuePre = 0; |
heroistired | 0:5b4f19f8cd85 | 76 | Motor->ChangeRateLagFiliterValue = 0; |
heroistired | 0:5b4f19f8cd85 | 77 | Motor->ChangeRateLagFiliterValueDigital = 0; |
heroistired | 0:5b4f19f8cd85 | 78 | Motor->ChangeRateLagFiliterValueDigitalPre = 0; |
heroistired | 0:5b4f19f8cd85 | 79 | Motor->InZeroZone = 0; |
heroistired | 0:5b4f19f8cd85 | 80 | Motor->ZeroZoneCounter = 0; |
heroistired | 0:5b4f19f8cd85 | 81 | Motor->BigStepCounter = 0; |
heroistired | 0:5b4f19f8cd85 | 82 | Motor->BigStepEncoderValue = 0; |
heroistired | 0:5b4f19f8cd85 | 83 | Motor->BigStepEncoderValuePre = 0; |
heroistired | 0:5b4f19f8cd85 | 84 | Motor->BigStepRevolveTimes = 0; |
heroistired | 0:5b4f19f8cd85 | 85 | Motor->HandUp1 = 0; |
heroistired | 0:5b4f19f8cd85 | 86 | Motor->HandUp2 = 0; |
heroistired | 0:5b4f19f8cd85 | 87 | Motor->EncoderTemp = 0; |
heroistired | 0:5b4f19f8cd85 | 88 | Motor->ChangeRatePIDControlPre = 0; |
heroistired | 0:5b4f19f8cd85 | 89 | Motor->MotorDir = MotorDirPin; |
heroistired | 0:5b4f19f8cd85 | 90 | Motor->EncoderCs = EncoderCsPin; |
heroistired | 0:5b4f19f8cd85 | 91 | Motor->SPI_Encoder = SPI_EncoderObj; |
heroistired | 0:5b4f19f8cd85 | 92 | } |
heroistired | 0:5b4f19f8cd85 | 93 | |
heroistired | 0:5b4f19f8cd85 | 94 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 95 | //功能:电机控制的PI调节器 |
heroistired | 0:5b4f19f8cd85 | 96 | //参数:Motor:要计算PID控制量的电机对应的电机控制结构体 |
heroistired | 0:5b4f19f8cd85 | 97 | //返回值:无 |
heroistired | 0:5b4f19f8cd85 | 98 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 99 | void MotorPIDController(MotorType *Motor) |
heroistired | 0:5b4f19f8cd85 | 100 | { |
heroistired | 0:5b4f19f8cd85 | 101 | char ChangeRateDifferenceLimit = 50; |
heroistired | 0:5b4f19f8cd85 | 102 | |
heroistired | 0:5b4f19f8cd85 | 103 | //位置偏差滤波 限制上升率 和 限制幅值 |
heroistired | 0:5b4f19f8cd85 | 104 | Motor->Difference_pre = Motor->Difference; |
heroistired | 0:5b4f19f8cd85 | 105 | Motor->Difference = -(Motor->Encoder- Motor->Goal); //计算偏差 |
heroistired | 0:5b4f19f8cd85 | 106 | Motor->ChangeRateDifference = Motor->Difference - Motor->Difference_pre; |
heroistired | 0:5b4f19f8cd85 | 107 | |
heroistired | 0:5b4f19f8cd85 | 108 | //切换参数 |
heroistired | 1:cbd6a3232d5b | 109 | /*if(Math_abs(Motor->Difference) <= ApproachDistance) //很接近目标距离了 |
heroistired | 0:5b4f19f8cd85 | 110 | { |
heroistired | 0:5b4f19f8cd85 | 111 | Motor->Ki = MotorKi_End; |
heroistired | 0:5b4f19f8cd85 | 112 | Motor->Kp = MotorKp_End; |
heroistired | 0:5b4f19f8cd85 | 113 | Motor->V_Ki = MotorVKi_End; |
heroistired | 0:5b4f19f8cd85 | 114 | Motor->V_Kp = MotorVKp_End; |
heroistired | 0:5b4f19f8cd85 | 115 | } |
heroistired | 0:5b4f19f8cd85 | 116 | else if(Motor->Velocity <= 15) |
heroistired | 0:5b4f19f8cd85 | 117 | { |
heroistired | 0:5b4f19f8cd85 | 118 | Motor->Ki = MotorKi_Start; |
heroistired | 0:5b4f19f8cd85 | 119 | Motor->Kp = MotorKp_Start; |
heroistired | 0:5b4f19f8cd85 | 120 | Motor->V_Ki = MotorVKi_Start; |
heroistired | 0:5b4f19f8cd85 | 121 | Motor->V_Kp = MotorVKp_Start; |
heroistired | 0:5b4f19f8cd85 | 122 | } |
heroistired | 0:5b4f19f8cd85 | 123 | else |
heroistired | 0:5b4f19f8cd85 | 124 | { |
heroistired | 0:5b4f19f8cd85 | 125 | Motor->Ki = MotorKi; |
heroistired | 0:5b4f19f8cd85 | 126 | Motor->Kp = MotorKp; |
heroistired | 0:5b4f19f8cd85 | 127 | Motor->V_Ki = MotorVKi; |
heroistired | 0:5b4f19f8cd85 | 128 | Motor->V_Kp = MotorVKp; |
heroistired | 1:cbd6a3232d5b | 129 | }*/ |
heroistired | 0:5b4f19f8cd85 | 130 | |
heroistired | 0:5b4f19f8cd85 | 131 | Motor->Integral += Motor->Difference; //计算积分量 |
heroistired | 0:5b4f19f8cd85 | 132 | if(Motor->Integral>15000) Motor->Integral=15000; //积分限幅 |
heroistired | 0:5b4f19f8cd85 | 133 | if(Motor->Integral<-15000) Motor->Integral=-15000; |
heroistired | 0:5b4f19f8cd85 | 134 | |
heroistired | 0:5b4f19f8cd85 | 135 | //速度目标值 |
heroistired | 0:5b4f19f8cd85 | 136 | Motor->V_Goal = (Motor->Kp * Motor->Difference + Motor->Ki * Motor->Integral)/10; //速度环目标值 |
heroistired | 0:5b4f19f8cd85 | 137 | Motor->GoalUpdated = 0; |
heroistired | 0:5b4f19f8cd85 | 138 | |
heroistired | 0:5b4f19f8cd85 | 139 | |
heroistired | 0:5b4f19f8cd85 | 140 | Motor->Velocity = Motor->Encoder - Motor->EncoderPre; |
heroistired | 0:5b4f19f8cd85 | 141 | Motor->V_Difference = -(Motor->Velocity - Motor->V_Goal); //速度偏差 |
heroistired | 0:5b4f19f8cd85 | 142 | Motor->V_Integral += Motor->V_Difference; |
heroistired | 0:5b4f19f8cd85 | 143 | if(Motor->V_Integral>6000) Motor->V_Integral=6000; //积分限幅 |
heroistired | 0:5b4f19f8cd85 | 144 | if(Motor->V_Integral<-6000) Motor->V_Integral=-6000; |
heroistired | 0:5b4f19f8cd85 | 145 | Motor->PIDControl = (Motor->V_Kp * Motor->V_Difference + Motor->V_Ki * Motor->V_Integral); //计算控制量以及相应的数据处理 |
heroistired | 0:5b4f19f8cd85 | 146 | |
heroistired | 0:5b4f19f8cd85 | 147 | |
heroistired | 0:5b4f19f8cd85 | 148 | if(Motor->PIDControl < 0) |
heroistired | 0:5b4f19f8cd85 | 149 | { |
heroistired | 0:5b4f19f8cd85 | 150 | Motor->PIDControl = -Motor->PIDControl; |
heroistired | 0:5b4f19f8cd85 | 151 | Motor->Dir = 0; |
heroistired | 0:5b4f19f8cd85 | 152 | Motor->MotorDir->write(0); |
heroistired | 0:5b4f19f8cd85 | 153 | } |
heroistired | 0:5b4f19f8cd85 | 154 | else |
heroistired | 0:5b4f19f8cd85 | 155 | { |
heroistired | 0:5b4f19f8cd85 | 156 | Motor->Dir = 1; |
heroistired | 0:5b4f19f8cd85 | 157 | Motor->MotorDir->write(1); |
heroistired | 0:5b4f19f8cd85 | 158 | } |
heroistired | 0:5b4f19f8cd85 | 159 | |
heroistired | 0:5b4f19f8cd85 | 160 | //判断是否进入误差容限 |
heroistired | 0:5b4f19f8cd85 | 161 | if(Math_abs(Motor->Difference) <= ErrorTolerance) |
heroistired | 0:5b4f19f8cd85 | 162 | { |
heroistired | 0:5b4f19f8cd85 | 163 | if(Motor->INTCounter <= INT_TimesMin) |
heroistired | 0:5b4f19f8cd85 | 164 | Motor->INTCounter++; |
heroistired | 0:5b4f19f8cd85 | 165 | else |
heroistired | 0:5b4f19f8cd85 | 166 | { |
heroistired | 0:5b4f19f8cd85 | 167 | Motor->PIDControl = 0; |
heroistired | 0:5b4f19f8cd85 | 168 | Motor->Status = MotorStatus_OK; |
heroistired | 0:5b4f19f8cd85 | 169 | } |
heroistired | 0:5b4f19f8cd85 | 170 | } |
heroistired | 0:5b4f19f8cd85 | 171 | else |
heroistired | 0:5b4f19f8cd85 | 172 | { |
heroistired | 0:5b4f19f8cd85 | 173 | Motor->INTCounter = 0; |
heroistired | 0:5b4f19f8cd85 | 174 | Motor->Status = MotorStatus_Running; |
heroistired | 0:5b4f19f8cd85 | 175 | } |
heroistired | 0:5b4f19f8cd85 | 176 | |
heroistired | 0:5b4f19f8cd85 | 177 | //限幅 |
heroistired | 0:5b4f19f8cd85 | 178 | if(Motor->PIDControl > PulseFreqLimit) |
heroistired | 0:5b4f19f8cd85 | 179 | Motor->PIDControl = PulseFreqLimit; |
heroistired | 0:5b4f19f8cd85 | 180 | |
heroistired | 0:5b4f19f8cd85 | 181 | } |
heroistired | 0:5b4f19f8cd85 | 182 | |
heroistired | 0:5b4f19f8cd85 | 183 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 184 | //功能:求绝对值 |
heroistired | 0:5b4f19f8cd85 | 185 | //参数:num 带求绝对值的函数 |
heroistired | 0:5b4f19f8cd85 | 186 | //返回值:num的绝对值 |
heroistired | 0:5b4f19f8cd85 | 187 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 188 | int Math_abs(int num) |
heroistired | 0:5b4f19f8cd85 | 189 | { |
heroistired | 0:5b4f19f8cd85 | 190 | if(num < 0) |
heroistired | 0:5b4f19f8cd85 | 191 | return -num; |
heroistired | 0:5b4f19f8cd85 | 192 | else |
heroistired | 0:5b4f19f8cd85 | 193 | return num; |
heroistired | 0:5b4f19f8cd85 | 194 | } |
heroistired | 0:5b4f19f8cd85 | 195 | |
heroistired | 0:5b4f19f8cd85 | 196 | //编码器初始化 |
heroistired | 0:5b4f19f8cd85 | 197 | void Encoder_Init(MotorType *Motor) |
heroistired | 0:5b4f19f8cd85 | 198 | { |
heroistired | 0:5b4f19f8cd85 | 199 | Motor->EncoderCs->write(1); |
heroistired | 0:5b4f19f8cd85 | 200 | Motor->SPI_Encoder->format(16,1); |
heroistired | 0:5b4f19f8cd85 | 201 | Motor->SPI_Encoder->frequency(100000); |
heroistired | 0:5b4f19f8cd85 | 202 | } |
heroistired | 0:5b4f19f8cd85 | 203 | |
heroistired | 0:5b4f19f8cd85 | 204 | //读取编码器值 |
heroistired | 0:5b4f19f8cd85 | 205 | unsigned int Encoder_ReadData(MotorType *Motor) |
heroistired | 0:5b4f19f8cd85 | 206 | { |
heroistired | 0:5b4f19f8cd85 | 207 | Motor->EncoderCs->write(0); |
heroistired | 0:5b4f19f8cd85 | 208 | int response = Motor->SPI_Encoder->write(0xFFFF); |
heroistired | 0:5b4f19f8cd85 | 209 | response &= 0x3fff; |
heroistired | 0:5b4f19f8cd85 | 210 | Motor->EncoderCs->write(1); |
heroistired | 0:5b4f19f8cd85 | 211 | return response; |
heroistired | 0:5b4f19f8cd85 | 212 | } |
heroistired | 0:5b4f19f8cd85 | 213 | |
heroistired | 0:5b4f19f8cd85 | 214 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 215 | //功能:一阶低通滤波 |
heroistired | 0:5b4f19f8cd85 | 216 | //参数:Value_now 本次样值 Value_pre 上次滤波输出值 |
heroistired | 0:5b4f19f8cd85 | 217 | //返回值:本次滤波输出值 |
heroistired | 0:5b4f19f8cd85 | 218 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 219 | int Fliter_1(int Value_now, int Value_pre) |
heroistired | 0:5b4f19f8cd85 | 220 | { |
heroistired | 0:5b4f19f8cd85 | 221 | return (int)(FlierCoefficient*(float)Value_now+(1-FlierCoefficient)*(float)Value_pre); |
heroistired | 0:5b4f19f8cd85 | 222 | } |
heroistired | 0:5b4f19f8cd85 | 223 | |
heroistired | 0:5b4f19f8cd85 | 224 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 225 | //功能:设置步进电机脉冲信号的PWM频率 |
heroistired | 0:5b4f19f8cd85 | 226 | // |
heroistired | 0:5b4f19f8cd85 | 227 | //返回值:无 |
heroistired | 0:5b4f19f8cd85 | 228 | ////////////////////////////////////////////////////////////////////////////////// |
heroistired | 0:5b4f19f8cd85 | 229 | void StepMotorSetFreq(MotorType *Motor) |
heroistired | 0:5b4f19f8cd85 | 230 | { |
heroistired | 0:5b4f19f8cd85 | 231 | unsigned int Motor_ARR; |
heroistired | 0:5b4f19f8cd85 | 232 | //为了使频率步长1Hz平滑可调需要分段设置定时器的时钟频率,即满足(Tclk/freq-Tclk/freq)>1 |
heroistired | 0:5b4f19f8cd85 | 233 | //并且考虑到余量与ARR寄存器只有16位,设置分段如下 |
heroistired | 0:5b4f19f8cd85 | 234 | //8k-5k 72M |
heroistired | 0:5b4f19f8cd85 | 235 | //5k-1k 36M |
heroistired | 0:5b4f19f8cd85 | 236 | //1k-500 2M |
heroistired | 0:5b4f19f8cd85 | 237 | //500-100 720k |
heroistired | 0:5b4f19f8cd85 | 238 | //100-1 72k |
heroistired | 0:5b4f19f8cd85 | 239 | //Motor->PIDControl = 5000; |
heroistired | 1:cbd6a3232d5b | 240 | /*if(Motor->PIDControl > PulseFreqLimit) |
heroistired | 0:5b4f19f8cd85 | 241 | Motor->PIDControl = PulseFreqLimit; |
heroistired | 0:5b4f19f8cd85 | 242 | else if((Motor->PIDControl <= PulseFreqLimit) && (Motor->PIDControl > 5000)) |
heroistired | 0:5b4f19f8cd85 | 243 | TIM2->PSC=0; //预分频器设置 |
heroistired | 0:5b4f19f8cd85 | 244 | else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 1000)) |
heroistired | 0:5b4f19f8cd85 | 245 | TIM2->PSC=1; //预分频器设置 |
heroistired | 0:5b4f19f8cd85 | 246 | else if((Motor->PIDControl <= 1000) && (Motor->PIDControl > 500)) |
heroistired | 0:5b4f19f8cd85 | 247 | TIM2->PSC=35; //预分频器设置 |
heroistired | 0:5b4f19f8cd85 | 248 | else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 100)) |
heroistired | 0:5b4f19f8cd85 | 249 | TIM2->PSC=99; //预分频器设置 |
heroistired | 0:5b4f19f8cd85 | 250 | else if((Motor->PIDControl <= 100) && (Motor->PIDControl > 0)) |
heroistired | 0:5b4f19f8cd85 | 251 | TIM2->PSC=999; //预分频器设置 |
heroistired | 0:5b4f19f8cd85 | 252 | else |
heroistired | 0:5b4f19f8cd85 | 253 | { |
heroistired | 0:5b4f19f8cd85 | 254 | Motor->PIDControl = 1; |
heroistired | 0:5b4f19f8cd85 | 255 | TIM2->PSC=999; //预分频器设置 |
heroistired | 0:5b4f19f8cd85 | 256 | } |
heroistired | 0:5b4f19f8cd85 | 257 | |
heroistired | 0:5b4f19f8cd85 | 258 | Motor_ARR = APB2Freq / (TIM2->PSC+1) / Motor->PIDControl; |
heroistired | 1:cbd6a3232d5b | 259 | TIM2->ARR = Motor_ARR; |
heroistired | 1:cbd6a3232d5b | 260 | TIM2->CCR2 = Motor_ARR/2;*/ |
heroistired | 1:cbd6a3232d5b | 261 | if(Motor->PIDControl > PulseFreqLimit) |
heroistired | 1:cbd6a3232d5b | 262 | Motor->PIDControl = PulseFreqLimit; |
heroistired | 1:cbd6a3232d5b | 263 | else if((Motor->PIDControl <= PulseFreqLimit) && (Motor->PIDControl > 5000)) |
heroistired | 1:cbd6a3232d5b | 264 | TIM1->PSC=0; //预分频器设置 |
heroistired | 1:cbd6a3232d5b | 265 | else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 1000)) |
heroistired | 1:cbd6a3232d5b | 266 | TIM1->PSC=1; //预分频器设置 |
heroistired | 1:cbd6a3232d5b | 267 | else if((Motor->PIDControl <= 1000) && (Motor->PIDControl > 500)) |
heroistired | 1:cbd6a3232d5b | 268 | TIM1->PSC=35; //预分频器设置 |
heroistired | 1:cbd6a3232d5b | 269 | else if((Motor->PIDControl <= 5000) && (Motor->PIDControl > 100)) |
heroistired | 1:cbd6a3232d5b | 270 | TIM1->PSC=99; //预分频器设置 |
heroistired | 1:cbd6a3232d5b | 271 | else if((Motor->PIDControl <= 100) && (Motor->PIDControl > 0)) |
heroistired | 1:cbd6a3232d5b | 272 | TIM1->PSC=999; //预分频器设置 |
heroistired | 1:cbd6a3232d5b | 273 | else |
heroistired | 1:cbd6a3232d5b | 274 | { |
heroistired | 1:cbd6a3232d5b | 275 | Motor->PIDControl = 1; |
heroistired | 1:cbd6a3232d5b | 276 | TIM1->PSC=999; //预分频器设置 |
heroistired | 1:cbd6a3232d5b | 277 | } |
heroistired | 1:cbd6a3232d5b | 278 | |
heroistired | 1:cbd6a3232d5b | 279 | Motor_ARR = APB2Freq / (TIM1->PSC+1) / Motor->PIDControl; |
heroistired | 0:5b4f19f8cd85 | 280 | //printf("ll%dll \r\n", Motor_ARR); |
heroistired | 1:cbd6a3232d5b | 281 | TIM1->ARR = Motor_ARR; |
heroistired | 1:cbd6a3232d5b | 282 | TIM1->CCR1 = Motor_ARR/2; |
heroistired | 0:5b4f19f8cd85 | 283 | } |
heroistired | 0:5b4f19f8cd85 | 284 | |
heroistired | 0:5b4f19f8cd85 | 285 | //控制接口 |
heroistired | 0:5b4f19f8cd85 | 286 | void Communication(Serial *Uart, MotorType *Motor) |
heroistired | 0:5b4f19f8cd85 | 287 | { |
heroistired | 0:5b4f19f8cd85 | 288 | char UartTemp = 0; //串口缓存 |
heroistired | 0:5b4f19f8cd85 | 289 | if(Uart->readable()) |
heroistired | 0:5b4f19f8cd85 | 290 | { |
heroistired | 0:5b4f19f8cd85 | 291 | |
heroistired | 0:5b4f19f8cd85 | 292 | } |
heroistired | 0:5b4f19f8cd85 | 293 | } |