모터 PI 제어 주기 변경 1s to 50ms

모터 동작속도가 느려서 모터 제어 주기를 기존 1초에서 50ms로 변경하였다. 제어주기는 TIM4의 HAL_TIM_PeriodElapsedCallback 함수를 사용하여 구현한다.. 50ms 인터럽트를 구현하기 위한 변수 설정은 아래와 같다. 

 

• APB1 timer clock = 60 MHz
• Prescaler = 60000 - 1
• Counter Period (ARR) = 50 - 1

계산식은 아래와 같다. 

 

 

PI 제어의 기본 데이터인 RPM 값이 기존 1초로 설정되어 있어서 50ms로 변경시 기본 값에 200배를 더해 주어야 한다. 

모터 제어 코드는 아래와 같다. 

​

#include "motor_control.h"

#include <stdio.h>

 

 

// Initialize motors

void motor_control_init(void)

{

// Example: Initialize GPIOs or peripherals for motor control

printf("Motor control initialized. (10 ms loop).\n");

 

// BTN7960 enable for traction and mower

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0,GPIO_PIN_SET);

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_1,GPIO_PIN_SET);

}

 

void motor_control_process(uint8_t cmd, uint8_t len, uint8_t *data)

{

static int8_t old_cmd = -1;

 

pi_left.target_rpm = data[0]; // Set target RPM for motor left

pi_right.target_rpm = data[1]; // Set target RPM for motor left

uart1_mower_duty = data[2];

 

 

// PI initialization if cmd is changed

if (cmd != old_cmd)

{

reset_pi(&pi_left);

reset_pi(&pi_right);

}

old_cmd = cmd;

 

// PWM control for motors based on the computed duty cycle

switch (cmd) {

case CMD_TRACTION_GO:

 

printf ("CMD_TRACTION_GO \r\n ");

// BTN7960 Forward enable for traction motor

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13,GPIO_PIN_SET); // Left

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14,GPIO_PIN_SET); // Right

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15,GPIO_PIN_RESET); // Mower

 

// BTN7960 Forward speed for traction motor with RPM

TIM1->CCR1 = 1000 - pwm_left;

TIM1->CCR2 = 1000 - pwm_right;

TIM1->CCR3 = (uart1_mower_duty * 10);

 

break;

 

case CMD_TRACTION_BACK:

 

printf ("CMD_TRACTION_BACK \r\n ");

// BTN7960 Reverse enable for traction motor

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13,GPIO_PIN_RESET); // Left

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14,GPIO_PIN_RESET); // Right

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15,GPIO_PIN_RESET); // Mower

 

// BTN7960 Reverse speed for traction motor

TIM1->CCR1 = pwm_left;

TIM1->CCR2 = pwm_right;

TIM1->CCR3 = (uart1_mower_duty * 10);

 

break;

 

case CMD_TRACTION_LEFT:

 

printf ("CMD_TRACTION_LEFT \r\n ");

// BTN7960 Forward enable for traction motor

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13,GPIO_PIN_SET); // Left

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14,GPIO_PIN_RESET); // Right

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15,GPIO_PIN_RESET); // Mower

 

// BTN7960 Forward speed for traction motor with RPM

TIM1->CCR1 = 1000 - pwm_left;

TIM1->CCR2 = pwm_right;

TIM1->CCR3 = (uart1_mower_duty * 10);

 

break;

 

case CMD_TRACTION_RIGHT:

 

printf ("CMD_TRACTION_RIGHT \r\n ");

// BTN7960 Reverse enable for traction motor

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13,GPIO_PIN_RESET); // Left

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14,GPIO_PIN_SET); // Right

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15,GPIO_PIN_RESET); // Mower

 

// BTN7960 Reverse speed for traction motor

TIM1->CCR1 = pwm_left;

TIM1->CCR2 = 1000 - pwm_right;

TIM1->CCR3 = (uart1_mower_duty * 10);

 

break;

 

case CMD_TRACTION_STOP:

 

printf ("CMD_TRACTION_STOP \r\n ");

 

// BTN7960 Reverse enable for traction motor

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); // Left

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); // Right

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); // Mower

 

TIM1->CCR1 = 1000;

TIM1->CCR2 = 1000;

TIM1->CCR3 = 1000;

 

break;

 

default:

 

printf ("default \r\n ");

 

#if 1

// BTN7960 Reverse enable for traction motor

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_13, GPIO_PIN_SET); // Left

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_14, GPIO_PIN_SET); // Right

HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); // Mower

 

TIM1->CCR1 = 1000;

TIM1->CCR2 = 1000;

TIM1->CCR3 = 1000;

#endif

 

break;

}

}

 

// using TIM4 50msec timer period elapsed call back for left, right traction motor PI control

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)

{

static encoder_instance_t encoder_left;

static encoder_instance_t encoder_right;

 

static counter;

 

if (htim->Instance == TIM4) // Check if TIM4 triggered the interrupt

{

update_encoder(&encoder_left, &htim3); // Process encoder from TIM3

update_encoder(&encoder_right, &htim2); // Process encoder from TIM2,

 

encoder_right.rpm = -encoder_right.rpm; // timer2 has a negative value when moving forward

 

if (encoder_left.rpm < 0) encoder_left.rpm = -encoder_left.rpm;

if (encoder_right.rpm < 0) encoder_right.rpm = -encoder_right.rpm;

 

pwm_left = run_pi_controller(&pi_left, encoder_left.rpm);

pwm_right = run_pi_controller(&pi_right, encoder_right.rpm);

 

counter++;

}

 

}

 

// Using TIM2 encoder for right encoder, TIM3 for left encoder

void update_encoder(encoder_instance_t *encoder, TIM_HandleTypeDef *htim)

{

encoder->counter = __HAL_TIM_GET_COUNTER(htim);

 

static uint8_t first_time = 0;

 

if (!first_time)

{

encoder->velocity = 0;

first_time = 1;

}

else

{

// Calculate signed difference to handle wraparound correctly

int16_t delta = (int16_t)(encoder->counter - encoder->old_counter);

encoder->velocity = delta;

}

 

encoder->old_counter = encoder->counter;

encoder->position += encoder->velocity;

 

// Update RPM: update interval is 100 ms (0.1s)

encoder->rpm = calculate_rpm_int(encoder->velocity);

 

// printf("Update encoder, counter = %d, old_counter = %d, velocity = %d, rpm = %d\r\n",

// encoder->counter, encoder->old_counter, encoder->velocity, encoder->rpm);

}

 

// motor rpm calculation for 100ms update rate

int calculate_rpm_int(int velocity_counts_per_period)

{

// For 100ms: RPM = (counts / CPR) / 0.1s * 60s = counts * 600 / CPR

#define ENCODER_CPR 3960 // 11 * 90 * 4

 

return (velocity_counts_per_period * 1200) / ENCODER_CPR; // 1s = 60, 100ms = 600, 50ms = 1200

}

 

// reset velocity, position, old_counter

void reset_encoder(encoder_instance_t *encoder_value)

{

encoder_value->velocity = 0;

encoder_value->position = 0;

encoder_value->old_counter = 0;

}

 

// reset integral and output_pwm of PI controller

void reset_pi(PIController *pi_controller)

{

pi_controller->integral =0;

// pi_controller->target_rpm =0;

pi_controller->output_pwm =0;

}

 

// PI controller

int run_pi_controller(PIController* controller, int measured_rpm)

{

int error = controller->target_rpm - measured_rpm;

 

controller->integral += error;

 

int output = (int)(controller->kp * error + controller->ki * controller->integral);

 

// Clamp to PWM range

if (output > MAX_PWM) output = MAX_PWM;

if (output < 0) output = 0;

 

controller->output_pwm = output;

return output;

}

 

 

헤더파일

#ifndef INC_MOTOR_CONTROL_H_

#define INC_MOTOR_CONTROL_H_

 

#include "stm32f4xx_hal.h"

 

// Constants

#define CMD_TRACTION_GO 0x11

#define CMD_TRACTION_BACK 0x12

#define CMD_TRACTION_LEFT 0x13

#define CMD_TRACTION_RIGHT 0x14

#define CMD_TRACTION_STOP 0x15

 

#define ENCODER_CPR 3960 // counts per shaft revolution, 11 CPR x 90 gear ratio x 4 encoder A/B

#define MAX_PWM 1000 // max PWM for TIM1->CCR1/2/3

 

#define TIMER_PERIOD_MS 1000 // Timer callback every 1s

 

// Timer Handle

extern TIM_HandleTypeDef htim2;

extern TIM_HandleTypeDef htim3;

extern TIM_HandleTypeDef htim4;

 

uint8_t uart1_traction_left_duty;

uint8_t uart1_traction_right_duty;

uint8_t uart1_mower_duty;

 

 

// Encoder instance definition

typedef struct {

int16_t velocity;

int64_t position;

uint16_t counter, old_counter;

int rpm;

} encoder_instance_t;

 

// Global encoder instances

encoder_instance_t encoder_left = {0, 0, 0, 0, 0};

encoder_instance_t encoder_right = {0, 0, 0, 0, 0};

 

// PI controller

typedef struct {

float kp;

float ki;

float integral;

int target_rpm;

int output_pwm;

} PIController;

 

PIController pi_left = { .kp = .5, .ki = .5, .integral = 0, .target_rpm = 0, .output_pwm = 0 };

PIController pi_right = { .kp = .5, .ki = .5, .integral = 0, .target_rpm = 0, .output_pwm = 0 };

 

// encoder

uint16_t counter_left = 0, counter_right = 0;

uint16_t encoder_velocity_left = 0;

uint32_t encoder_position_left = 0;

uint16_t left_turn = 0, right_turn = 0;

 

uint16_t timer_counter_left = 0;

 

int pwm_left, pwm_right;

 

 

// Function Prototypes

void motor_control_init(void);

void motor_control_process(uint8_t cmd, uint8_t len, uint8_t *data);

 

// PI controller

int run_pi_controller(PIController* controller, int measured_rpm);

 

// Function prototypes

//void update_encoder(encoder_instance_t *encoder_value, TIM_HandleTypeDef *htim);

//void reset_encoder(encoder_instance_t *encoder_value);

 

 

 

#endif /* INC_MOTOR_CONTROL_H_ */