stm32c8t6作为主控芯片 ,直流光电编码器测速电机作为控制转速的电机,用32的标准库编程,写出模糊PID控制的完整代码
时间: 2023-06-13 13:06:34 浏览: 118
stm32c8t6电机控制PWM和光电传感器控制
很抱歉,我不是一个能直接编写代码的AI,但我可以给你一些思路和参考代码:
思路:
1. 通过读取直流光电编码器的脉冲数量,计算电机的当前转速。
2. 根据设定的目标转速和当前转速,计算出误差值。
3. 使用模糊PID控制算法,根据误差值计算出合适的控制量。
4. 将控制量输出到电机控制器中,通过PWM控制电机的转速。
参考代码:
以下是一个简单的基于标准库的模糊PID控制代码示例,仅供参考:
```c
#include "stm32f1xx.h"
#include "math.h"
#define ENCODER_PULSES_PER_REV 1024 // 每圈的脉冲数
#define PWM_FREQ 20000 // PWM 频率,单位 Hz
// PID控制器参数
#define KP 1.0f
#define KI 0.0f
#define KD 0.0f
// 模糊PID控制器参数
#define ERROR_THRESHOLD 0.1f
#define ERROR_RATE_THRESHOLD 0.05f
#define ERROR_LOW -1.0f
#define ERROR_HIGH 1.0f
#define ERROR_RATE_LOW -1.0f
#define ERROR_RATE_HIGH 1.0f
#define CONTROL_LOW -1.0f
#define CONTROL_HIGH 1.0f
// 电机控制器参数
#define PWM_MAX 65535 // PWM最大值
#define PWM_MIN 0 // PWM最小值
#define PWM_DEADBAND 10 // PWM死区,防止电机抖动
volatile uint32_t encoderCount = 0; // 直流光电编码器计数器
float targetSpeed = 100.0f; // 目标转速
float currentSpeed = 0.0f; // 当前转速
float error = 0.0f; // 误差
float lastError = 0.0f; // 上一次的误差
float errorRate = 0.0f; // 误差变化率
float control = 0.0f; // 控制量
float pwm = 0.0f; // PWM输出
void TIM2_IRQHandler(void)
{
if (TIM2->SR & TIM_SR_UIF)
{
TIM2->SR &= ~TIM_SR_UIF;
encoderCount++;
}
}
void initEncoder(void)
{
// 初始化编码器GPIO
RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
GPIOA->CRL &= ~(GPIO_CRL_CNF0 | GPIO_CRL_MODE0 | GPIO_CRL_CNF1 | GPIO_CRL_MODE1);
GPIOA->CRL |= GPIO_CRL_CNF0_1 | GPIO_CRL_MODE0_0 | GPIO_CRL_CNF1_1 | GPIO_CRL_MODE1_0;
// 初始化编码器计数器
RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
TIM2->PSC = 0;
TIM2->ARR = 0xFFFF;
TIM2->CNT = 0;
TIM2->CR1 |= TIM_CR1_CEN;
TIM2->DIER |= TIM_DIER_UIE;
NVIC_EnableIRQ(TIM2_IRQn);
}
void initPWM(void)
{
// 初始化PWM GPIO
RCC->APB2ENR |= RCC_APB2ENR_IOPBEN;
GPIOB->CRH &= ~(GPIO_CRH_CNF13 | GPIO_CRH_MODE13);
GPIOB->CRH |= GPIO_CRH_CNF13_1 | GPIO_CRH_MODE13_0;
// 初始化PWM定时器
RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
TIM4->PSC = SystemCoreClock / (PWM_FREQ * PWM_MAX);
TIM4->ARR = PWM_MAX;
TIM4->CCR2 = 0;
TIM4->CCER |= TIM_CCER_CC2E;
TIM4->CCMR1 |= TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1;
TIM4->CR1 |= TIM_CR1_CEN;
}
float fuzzyPID(float error, float errorRate)
{
float errorLow = -ERROR_THRESHOLD;
float errorHigh = ERROR_THRESHOLD;
float errorRateLow = -ERROR_RATE_THRESHOLD;
float errorRateHigh = ERROR_RATE_THRESHOLD;
float controlLow = -1.0f;
float controlHigh = 1.0f;
float errorNB = 0.0f;
float errorNS = 0.0f;
float errorZO = 0.0f;
float errorPS = 0.0f;
float errorPB = 0.0f;
float errorRateNB = 0.0f;
float errorRateNS = 0.0f;
float errorRateZO = 0.0f;
float errorRatePS = 0.0f;
float errorRatePB = 0.0f;
float controlNB = 0.0f;
float controlNS = 0.0f;
float controlZO = 0.0f;
float controlPS = 0.0f;
float controlPB = 0.0f;
float totalError = 0.0f;
float totalErrorRate = 0.0f;
float totalControl = 0.0f;
float totalWeight = 0.0f;
// 计算误差的隶属度函数
if (error <= errorLow)
{
errorNB = 1;
}
else if (error > errorLow && error < 0)
{
errorNB = (error - errorLow) / (0 - errorLow);
errorNS = 1 - errorNB;
}
else if (error == 0)
{
errorZO = 1;
}
else if (error > 0 && error < errorHigh)
{
errorPS = (errorHigh - error) / (errorHigh - 0);
errorZO = 1 - errorPS;
}
else if (error >= errorHigh)
{
errorPB = 1;
}
// 计算误差变化率的隶属度函数
if (errorRate <= errorRateLow)
{
errorRateNB = 1;
}
else if (errorRate > errorRateLow && errorRate < 0)
{
errorRateNB = (errorRate - errorRateLow) / (0 - errorRateLow);
errorRateNS = 1 - errorRateNB;
}
else if (errorRate == 0)
{
errorRateZO = 1;
}
else if (errorRate > 0 && errorRate < errorRateHigh)
{
errorRatePS = (errorRateHigh - errorRate) / (errorRateHigh - 0);
errorRateZO = 1 - errorRatePS;
}
else if (errorRate >= errorRateHigh)
{
errorRatePB = 1;
}
// 计算控制量的隶属度函数
controlNB = CONTROL_LOW;
controlNS = (CONTROL_LOW + CONTROL_HIGH) / 2;
controlZO = CONTROL_HIGH;
controlPS = (CONTROL_LOW + CONTROL_HIGH) / 2;
controlPB = CONTROL_HIGH;
// 计算模糊推理
for (int i = 0; i < 5; i++)
{
for (int j = 0; j < 5; j++)
{
if (i == 0 || j == 0)
{
continue;
}
totalWeight = fminf(errorNB, errorRateNB);
totalControl = controlNB;
totalError = i - 3;
totalErrorRate = j - 3;
if (errorNS > 0 && errorRateNB > 0)
{
totalWeight = fminf(totalWeight, fminf(errorNS, errorRateNB));
totalControl = fmaxf(totalControl, controlNS);
totalError = totalError + (i - 2);
totalErrorRate = totalErrorRate - (j - 2);
}
if (errorNS > 0 && errorRateZO > 0)
{
totalWeight = fminf(totalWeight, fminf(errorNS, errorRateZO));
totalControl = fmaxf(totalControl, controlNS);
totalError = totalError + (i - 2);
}
if (errorNS > 0 && errorRatePS > 0)
{
totalWeight = fminf(totalWeight, fminf(errorNS, errorRatePS));
totalControl = fmaxf(totalControl, controlZO);
totalError = totalError + (i - 2);
totalErrorRate = totalErrorRate + (j - 2);
}
if (errorZO > 0 && errorRateNB > 0)
{
totalWeight = fminf(totalWeight, fminf(errorZO, errorRateNB));
totalControl = fmaxf(totalControl, controlNS);
totalErrorRate = totalErrorRate - (j - 2);
}
if (errorZO > 0 && errorRateZO > 0)
{
totalWeight = fminf(totalWeight, fminf(errorZO, errorRateZO));
totalControl = fmaxf(totalControl, controlZO);
}
if (errorZO > 0 && errorRatePS > 0)
{
totalWeight = fminf(totalWeight, fminf(errorZO, errorRatePS));
totalControl = fmaxf(totalControl, controlZO);
totalErrorRate = totalErrorRate + (j - 2);
}
if (errorPS > 0 && errorRateNB > 0)
{
totalWeight = fminf(totalWeight, fminf(errorPS, errorRateNB));
totalControl = fmaxf(totalControl, controlZO);
totalError = totalError - (i - 2);
totalErrorRate = totalErrorRate - (j - 2);
}
if (errorPS > 0 && errorRateZO > 0)
{
totalWeight = fminf(totalWeight, fminf(errorPS, errorRateZO));
totalControl = fmaxf(totalControl, controlZO);
totalError = totalError - (i - 2);
}
if (errorPS > 0 && errorRatePS > 0)
{
totalWeight = fminf(totalWeight, fminf(errorPS, errorRatePS));
totalControl = fmaxf(totalControl, controlPS);
totalError = totalError - (i - 2);
totalErrorRate = totalErrorRate + (j - 2);
}
if (totalWeight > 0)
{
control = control + (totalControl * totalWeight);
}
}
}
return control;
}
int main(void)
{
// 初始化硬件
SystemInit();
initEncoder();
initPWM();
// 进入主循环
while (1)
{
// 计算当前转速
currentSpeed = encoderCount * 60.0f / (ENCODER_PULSES_PER_REV * TIM2->CNT);
encoderCount = 0;
// 计算误差和误差变化率
error = targetSpeed - currentSpeed;
errorRate = error - lastError;
lastError = error;
// 计算模糊PID控制量
control = fuzzyPID(error, errorRate);
// 计算PWM输出
pwm = PWM_MIN + (PWM_MAX - PWM_MIN) * (control + 1) / 2;
if (pwm < (PWM_MIN + PWM_DEADBAND))
{
pwm = PWM_MIN;
}
else if (pwm > (PWM_MAX - PWM_DEADBAND))
{
pwm = PWM_MAX;
}
TIM4->CCR2 = (uint16_t)pwm;
}
}
```
需要注意的是,以上代码仅是一个简单的示例,实际应用中需要根据具体情况进行调整和优化。如果您有任何问题或需要进一步的帮助,请随时联系我。
阅读全文