My microcontroller code has some issues, and it makes the robot’s navigation not smooth. Could anyone help me fix it?
#include <PID_v1.h>
#include <math.h>
// ---------------- Encoder pins ----------------
#define ENC_IN_LEFT_A 2 // interrupt
#define ENC_IN_LEFT_B 4
#define ENC_IN_RIGHT_A 3 // interrupt
#define ENC_IN_RIGHT_B 5
// ---------------- LEDs / buttons ----------------
const int buttonPin = 26;
const int red = 22;
const int yellow = 24;
const int pos1 = 34;
const int pos2 = 44;
// ---------------- BTS7960 pins ----------------
const int RPWM_L = 11; // forward left
const int LPWM_L = 7; // reverse left
const int RPWM_R = 10; // forward right
const int LPWM_R = 12; // reverse right
// ---------------- Robot geometry ----------------
const double BASE_WIDTH_M = 0.345; // distance between wheels (m)
const double WHEEL_DIAM_M = 0.085; // diameter (m)
const double WHEEL_CIRC_M = M_PI * WHEEL_DIAM_M;
const long TICKS_PER_REV = 300;
const double DT_LOOP_MS = 30.0; // control loop period (ms)
// ---------------- PID gains ----------------
double left_kp = 15, left_ki = 0.5, left_kd = 0.05;
double right_kp = 15, right_ki = 0.5, right_kd = 0.05;
const double K_V2PWM = 160.0;
// Convert keyboard pulse to setpoints
const double K_PULSE2V = 0.00013;
const double K_PULSE2W = 0.0035;
// ---------------- Runtime state ----------------
volatile long left_ticks_cum = 0;
volatile long right_ticks_cum = 0;
volatile long enc_left_dt = 0;
volatile long enc_right_dt = 0;
boolean Direction_left = true;
boolean Direction_right = true;
double v_left_meas = 0, v_right_meas = 0;
double v_left_sp = 0, v_right_sp = 0;
double left_input = 0, left_output = 0, left_setpoint = 0;
double right_input = 0, right_output = 0, right_setpoint = 0;
PID leftPID (&left_input, &left_output, &left_setpoint, left_kp, left_ki, left_kd, DIRECT);
PID rightPID(&right_input, &right_output, &right_setpoint, right_kp, right_ki, right_kd, DIRECT);
// Bổ sung: Biến lưu trữ Odometry
double x_pos = 0.0;
double y_pos = 0.0;
double theta_angle = 0.0; // Góc hiện tại của robot (radian)
// Smooth PWM for soft start/stop
int smooth_pwmL = 0, smooth_pwmR = 0;
const int PWM_STEP_UP = 5; // tăng dần
const int PWM_STEP_DOWN = 4; // giảm dần
const int PWM_LIMIT = 80; // giới hạn PWM
// UART buffer & watchdog
String inbuf;
unsigned long last_cmd_ms = 0;
const unsigned long CMD_TIMEOUT_MS = 100;
// PID state enable flag
bool pidEnabled = false;
// UART xuất tín hiệu encoder cho UNO tay rung
#define ENCODER_TX Serial1
// ---------------- Encoder ISRs ----------------
void right_wheel_tick() {
int val = digitalRead(ENC_IN_RIGHT_B);
Direction_right = (val == LOW);
if (Direction_right) {
right_ticks_cum++;
enc_right_dt++;
} else {
right_ticks_cum–;
enc_right_dt–;
}
}
void left_wheel_tick() {
int val = digitalRead(ENC_IN_LEFT_B);
Direction_left = (val != LOW);
if (Direction_left) {
left_ticks_cum++;
enc_left_dt++;
} else {
left_ticks_cum–;
enc_left_dt–;
}
}
// ---------------- BTS7960 helper ----------------
static inline void driveBTS(int pwmCmd, int pinRPWM, int pinLPWM, int &smoothRef) {
pwmCmd = constrain(pwmCmd, -PWM_LIMIT, PWM_LIMIT);
// ramp PWM để phanh mượt
if (smoothRef < pwmCmd) smoothRef += PWM_STEP_UP;
else if (smoothRef > pwmCmd) smoothRef -= PWM_STEP_DOWN;
// giới hạn sau ramp
smoothRef = constrain(smoothRef, -PWM_LIMIT, PWM_LIMIT);
int mag = abs(smoothRef);
if (smoothRef >= 0) {
analogWrite(pinRPWM, mag);
analogWrite(pinLPWM, 0);
} else {
analogWrite(pinRPWM, 0);
analogWrite(pinLPWM, mag);
}
}
// ---------------- Emergency STOP ----------------
static inline void stopAll() {
v_left_sp = 0;
v_right_sp = 0;
pidEnabled = false;
leftPID.SetMode(MANUAL);
rightPID.SetMode(MANUAL);
left_output = 0;
right_output = 0;
smooth_pwmL = 0;
smooth_pwmR = 0;
driveBTS(0, RPWM_R, LPWM_R, smooth_pwmR);
driveBTS(0, RPWM_L, LPWM_L, smooth_pwmL);
analogWrite(RPWM_R, 0); analogWrite(LPWM_R, 0);
analogWrite(RPWM_L, 0); analogWrite(LPWM_L, 0);
// Đặt lại Odometry khi dừng hoàn toàn
x_pos = 0.0;
y_pos = 0.0;
theta_angle = 0.0;
Serial.println(“[STOP_CMD_RX]”);
digitalWrite(red, HIGH);
digitalWrite(yellow, LOW);
}
// ---------------- UART command parsing ----------------
void handleUartCmd(const String &s) {
if (s.length() == 0) return;
if (s.equalsIgnoreCase(“stop”)) { stopAll(); return; }
char c = s[0];
if (c == ‘x’ || c == ‘X’) { stopAll(); return; }
long val = (s.length() > 1) ? s.substring(1).toInt() : 0;
double linear = 0.0, angular = 0.0;
if (c == ‘w’) linear = K_PULSE2V * val;
if (c == ‘s’) linear = -K_PULSE2V * val;
if (c == ‘a’) angular = K_PULSE2W * val;
if (c == ‘d’) angular = -K_PULSE2W * val;
double v_r = linear + angular * (BASE_WIDTH_M / 2.0);
double v_l = linear - angular * (BASE_WIDTH_M / 2.0);
if (fabs(linear) > 1e-9 || fabs(angular) > 1e-9) {
if (!pidEnabled) {
pidEnabled = true;
leftPID.SetMode(AUTOMATIC);
rightPID.SetMode(AUTOMATIC);
Serial.println(“[PID ON]”);
}
v_left_sp = v_l;
v_right_sp = v_r;
} else {
stopAll();
}
}
// ---------------- Setup ----------------
void setup() {
Serial.begin(115200);
ENCODER_TX.begin(9600);
pinMode(ENC_IN_LEFT_A , INPUT_PULLUP);
pinMode(ENC_IN_LEFT_B , INPUT);
pinMode(ENC_IN_RIGHT_A, INPUT_PULLUP);
pinMode(ENC_IN_RIGHT_B, INPUT);
attachInterrupt(digitalPinToInterrupt(ENC_IN_LEFT_A), left_wheel_tick, RISING);
attachInterrupt(digitalPinToInterrupt(ENC_IN_RIGHT_A), right_wheel_tick, RISING);
pinMode(RPWM_R, OUTPUT); pinMode(LPWM_R, OUTPUT);
pinMode(RPWM_L, OUTPUT); pinMode(LPWM_L, OUTPUT);
pinMode(yellow, OUTPUT); pinMode(red, OUTPUT);
pinMode(pos1, OUTPUT); pinMode(pos2, OUTPUT);
analogWrite(RPWM_R, 0);
analogWrite(LPWM_R, 0);
analogWrite(RPWM_L, 0);
analogWrite(LPWM_L, 0);
digitalWrite(red, HIGH);
digitalWrite(yellow, LOW);
digitalWrite(pos1, LOW);
digitalWrite(pos2, LOW);
leftPID.SetMode(MANUAL);
rightPID.SetMode(MANUAL);
leftPID.SetSampleTime((int)DT_LOOP_MS);
rightPID.SetSampleTime((int)DT_LOOP_MS);
leftPID.SetOutputLimits(-1.0, 1.0);
rightPID.SetOutputLimits(-1.0, 1.0);
stopAll();
last_cmd_ms = millis();
Serial.println(“System ready. Motors stopped.”);
}
// ---------------- Main loop ----------------
void loop() {
// UART đọc lệnh
while (Serial.available()) {
char c = (char)Serial.read();
Serial.print("[RX] "); Serial.println(c);
if (c == 'x' || c == 'X' || c == ' ') {
stopAll();
inbuf = "";
unsigned long start_clean = millis();
while (Serial.available() && (millis() - start_clean < 5)) Serial.read();
last_cmd_ms = millis();
continue;
}
if (c == '\r' || c == '\n') {
if (inbuf.length() > 0) {
handleUartCmd(inbuf);
inbuf = "";
last_cmd_ms = millis();
}
} else {
if (isAlphaNumeric(c)) {
inbuf += c;
if (inbuf.length() > 32) inbuf = "";
}
}
}
// Watchdog timeout
if (millis() - last_cmd_ms > CMD_TIMEOUT_MS) {
v_left_sp = 0;
v_right_sp = 0;
}
static unsigned long prev_ms = 0, prev_print = 0;
unsigned long now = millis();
// Control loop
if (now - prev_ms >= (unsigned long)DT_LOOP_MS) {
prev_ms = now;
// 1. Lấy dữ liệu Encoder
noInterrupts();
long dL = enc_left_dt;
long dR = enc_right_dt;
enc_left_dt = 0;
enc_right_dt = 0;
long total_left_ticks = left_ticks_cum;
long total_right_ticks = right_ticks_cum;
interrupts();
// 2. Tính vận tốc và quãng đường (Cho PID và Odometry)
double ticks_per_sec = 1000.0 / DT_LOOP_MS;
v_left_meas = (dL * ticks_per_sec / (double)TICKS_PER_REV) * WHEEL_CIRC_M;
v_right_meas = (dR * ticks_per_sec / (double)TICKS_PER_REV) * WHEEL_CIRC_M;
// Tính quãng đường di chuyển của từng bánh xe (m)
double dist_L = ((double)dL / TICKS_PER_REV) * WHEEL_CIRC_M;
double dist_R = ((double)dR / TICKS_PER_REV) * WHEEL_CIRC_M;
// 3. Tính Kinematics chuyển tiếp (Odometry)
double d_theta = (dist_R - dist_L) / BASE_WIDTH_M; // Thay đổi góc
double d_center = (dist_R + dist_L) / 2.0; // Quãng đường trung tâm
// Tích hợp Odometry (sử dụng xấp xỉ cung tròn nhỏ)
double avg_theta = theta_angle + (d_theta / 2.0); // Góc trung bình cho đoạn di chuyển
double d_x = d_center * cos(avg_theta);
double d_y = d_center * sin(avg_theta);
x_pos += d_x;
y_pos += d_y;
theta_angle += d_theta;
// Chuẩn hóa góc theta (-PI đến PI) để tránh tràn số (optional)
while (theta_angle > M_PI) theta_angle -= 2 * M_PI;
while (theta_angle < -M_PI) theta_angle += 2 * M_PI;
// 4. PID Control
left_setpoint = v_left_sp;
right_setpoint = v_right_sp;
left_input = v_left_meas;
right_input = v_right_meas;
if (pidEnabled) {
leftPID.Compute();
rightPID.Compute();
} else {
left_output = 0;
right_output = 0;
}
int pwmL = (int)round(left_output * K_V2PWM);
int pwmR = (int)round(right_output * K_V2PWM);
// 5. Điều khiển động cơ
driveBTS(pwmR, RPWM_R, LPWM_R, smooth_pwmR);
driveBTS(pwmL, RPWM_L, LPWM_L, smooth_pwmL);
if (fabs(v_left_sp) < 1e-3 && fabs(v_right_sp) < 1e-3) {
digitalWrite(red, HIGH);
digitalWrite(yellow, LOW);
} else {
digitalWrite(red, LOW);
digitalWrite(yellow, HIGH);
}
}
// Xuất encoder và Odometry định kỳ 100ms
if (now - prev_print >= 100) {
prev_print = now;
// Xuất Odometry và ticks qua Serial1 để đọc bằng ROS
// Format: x,y,theta,ticksL,ticksR
ENCODER_TX.print(x_pos);
ENCODER_TX.print(",");
ENCODER_TX.print(y_pos);
ENCODER_TX.print(",");
ENCODER_TX.print(theta_angle);
ENCODER_TX.print(",");
ENCODER_TX.print(left_ticks_cum);
ENCODER_TX.print(",");
ENCODER_TX.println(right_ticks_cum);
// In ra Serial (Debug)
Serial.print("X="); Serial.print(x_pos, 4);
Serial.print(" Y="); Serial.print(y_pos, 4);
Serial.print(" Theta="); Serial.println(theta_angle, 4);
}
}
