#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('速度');