모터 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_ */