144 lines
4.3 KiB
C
144 lines
4.3 KiB
C
#include "stm32f1xx_hal.h"
|
||
|
||
// 定义步进电机的步序
|
||
const uint8_t stepSequence[4] = {0x01, 0x02, 0x04, 0x08};
|
||
uint8_t currentStep = 0;
|
||
|
||
// 定义 PWM 和定时器句柄
|
||
TIM_HandleTypeDef htim1;
|
||
|
||
// 初始化 PWM
|
||
void PWM_Init(void) {
|
||
__HAL_RCC_TIM1_CLK_ENABLE();
|
||
|
||
TIM_OC_InitTypeDef sConfigOC = {0};
|
||
|
||
htim1.Instance = TIM1;
|
||
htim1.Init.Prescaler = 72 - 1; // 1 MHz 时钟
|
||
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||
htim1.Init.Period = 1000 - 1; // 1 kHz PWM
|
||
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
||
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
|
||
|
||
HAL_TIM_PWM_Init(&htim1);
|
||
|
||
sConfigOC.OCMode = TIM_OCMODE_PWM1;
|
||
sConfigOC.Pulse = 500; // 50% 占空比
|
||
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
|
||
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
|
||
|
||
HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1);
|
||
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
|
||
}
|
||
|
||
// 初始化 GPIO，用于控制栅极驱动器
|
||
void GPIO_Init(void) {
|
||
__HAL_RCC_GPIOA_CLK_ENABLE();
|
||
|
||
GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||
|
||
GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3;
|
||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
|
||
|
||
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||
}
|
||
|
||
// 更新步进电机的步序
|
||
void StepMotor_Update(void) {
|
||
// 设置 GPIO 输出
|
||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, (stepSequence[currentStep] & 0x01) ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, (stepSequence[currentStep] & 0x02) ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_2, (stepSequence[currentStep] & 0x04) ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_3, (stepSequence[currentStep] & 0x08) ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||
|
||
// 更新步序
|
||
currentStep = (currentStep + 1) % 4;
|
||
}
|
||
|
||
// 主函数
|
||
int main(void) {
|
||
HAL_Init();
|
||
SystemClock_Config(); // 配置系统时钟
|
||
|
||
GPIO_Init();
|
||
PWM_Init();
|
||
|
||
while (1) {
|
||
StepMotor_Update();
|
||
HAL_Delay(1); // 控制转速，调整延迟时间以改变转速
|
||
}
|
||
}
|
||
|
||
|
||
% 定义系统参数
|
||
motor_inertia = 0.01; % 电机惯量
|
||
motor_damping = 0.1; % 电机阻尼系数
|
||
|
||
% 定义 PID 参数
|
||
pid_position = struct('kp', 0.5, 'ki', 0.1, 'kd', 0.01, 'out_limit', 100);
|
||
pid_speed = struct('kp', 0.1, 'ki', 0.01, 'kd', 0.05, 'out_limit', 100);
|
||
|
||
% 初始化变量
|
||
current_position = 0;
|
||
current_speed = 0;
|
||
target_position = 3600; % 目标位置
|
||
dt = 0.01; % 仿真时间步长
|
||
time = 0:dt:10; % 仿真时间
|
||
|
||
% 记录数据
|
||
position_data = zeros(size(time));
|
||
speed_data = zeros(size(time));
|
||
|
||
% PID 控制器状态
|
||
position_integral = 0;
|
||
position_prev_error = 0;
|
||
speed_integral = 0;
|
||
speed_prev_error = 0;
|
||
|
||
% 仿真主循环
|
||
for i = 1:length(time)
|
||
% 位置环 PID
|
||
position_error = target_position - current_position;
|
||
position_integral = position_integral + position_error * dt;
|
||
position_derivative = (position_error - position_prev_error) / dt;
|
||
target_speed = pid_position.kp * position_error + ...
|
||
pid_position.ki * position_integral + ...
|
||
pid_position.kd * position_derivative;
|
||
target_speed = max(min(target_speed, pid_position.out_limit), -pid_position.out_limit);
|
||
position_prev_error = position_error;
|
||
|
||
% 速度环 PID
|
||
speed_error = target_speed - current_speed;
|
||
speed_integral = speed_integral + speed_error * dt;
|
||
speed_derivative = (speed_error - speed_prev_error) / dt;
|
||
motor_pwm = pid_speed.kp * speed_error + ...
|
||
pid_speed.ki * speed_integral + ...
|
||
pid_speed.kd * speed_derivative;
|
||
motor_pwm = max(min(motor_pwm, pid_speed.out_limit), -pid_speed.out_limit);
|
||
speed_prev_error = speed_error;
|
||
|
||
% 电机模型更新
|
||
motor_acceleration = motor_pwm / motor_inertia - motor_damping * current_speed;
|
||
current_speed = current_speed + motor_acceleration * dt;
|
||
current_position = current_position + current_speed * dt;
|
||
|
||
% 记录数据
|
||
position_data(i) = current_position;
|
||
speed_data(i) = current_speed;
|
||
end
|
||
|
||
% 绘制结果
|
||
figure;
|
||
subplot(2, 1, 1);
|
||
plot(time, position_data);
|
||
title('位置响应');
|
||
xlabel('时间 (s)');
|
||
ylabel('位置');
|
||
|
||
subplot(2, 1, 2);
|
||
plot(time, speed_data);
|
||
title('速度响应');
|
||
xlabel('时间 (s)');
|
||
ylabel('速度'); |