tôi làm robot bám đường atmega16A dùng pure persuit giao tiếp esp32 để nhận tọa độ đường:

#define F_CPU 8000000UL

#include <avr/io.h>
#include <util/delay.h>
#include <math.h>
#include <avr/interrupt.h>
// ================= CẤU HÌNH CHÂ TB6612 =================
#define AIN1 PB0
#define AIN2 PB1
#define STBY PB2
#define BIN1 PB3
#define BIN2 PB4
#define PWMA PD4 // OC1B
#define PWMB PD5 // OC1A
// ================= CẤU HÌNH LED 7 ĐOẠN & 74HC595 ==================
#define HC595_PORT PORTB
#define HC595_DDR DDRB
#define DS_PIN PB5 // Data Serial (Chân 14 của IC)
#define SH_CP_PIN PB6 // Shift Clock (Chân 11 của IC)
#define ST_CP_PIN PB7 // Latch Clock (Chân 12 của IC)

#define DIGIT_PORT PORTA
#define DIGIT_DDR DDRA
#define D1_PIN PA2 // Transistor số 1 (Trăm)
#define D2_PIN PA3 // Transistor số 2 (Chục)
#define D3_PIN PA4 // Transistor số 3 (Đơn vị)

// Mảng chứa mã Hex để hiển thị các số từ 0 -> 9 (Chuẩn âm chung - Common Cathode)
// Thứ tự nối dây chuẩn: Q0->A, Q1->B, Q2->C, Q3->D, Q4->E, Q5->F, Q6->G
const uint8_t LED_MAP[10] = {
0x3F, // 0
0x06, // 1
0x5B, // 2
0x4F, // 3
0x66, // 4
0x6D, // 5
0x7D, // 6
0x07, // 7
0x7F, // 8
0x6F // 9
};

volatile uint8_t led_buffer[3] = {0, 0, 0}; // Mảng lưu mã nút của 3 số đang hiển thị
volatile uint8_t current_digit = 0; // Biến định dấu số nào đang được quét

// ================= ODOMETRY & PATH TRACKING CONSTANTS =================
#define PI 3.14159265359f
#define WHEEL_BASE 0.15f // Khoảng cách giữa 2 bánh (meters)
#define TICK_TO_M 0.0001f // Chuyển ticks -> meters (tùy chỉnh theo encoder của bạn)
#define MAX_SPEED 0.3f // Tốc độ tối đa (m/s)
#define Kp 50.0f // PID Proportional
#define Ki 10.0f // PID Integral
#define Kd 5.0f // PID Derivative
#define LOOKAHEAD_DIST 0.14f // Khoảng cách lookahead (meters)

// ================= ODOMETRY & CONTROL VARIABLES =================
volatile float x = 0.0f, y = 0.0f, theta = 0.0f; // Vị trí và hướng
volatile float vL = 0.0f, vR = 0.0f; // Tốc độ lý thuyết
volatile long prevLeft = 0, prevRight = 0; // Giá trị encoder trước đó
volatile uint32_t lastTime = 0; // Thời gian lần cuối
volatile int currentTarget = 0; // Chỉ số điểm hiện tại trên đường
volatile uint8_t finished = 0; // Có đi đến đích không

// Biến trạng thái PID
volatile float prevErrL = 0.0f, prevErrR = 0.0f;
volatile float intL = 0.0f, intR = 0.0f;
volatile float pwml1 = 0.0f, pwmr1 = 0.0f;

// ================= NORMALIZE ANGLE =================
void chuan_hoa(float &angle) {
while (angle > PI) angle -= 2 * PI;
while (angle < -PI) angle += 2 * PI;
}

#include <avr/interrupt.h>
// Hàm khởi tạo các chân điều khiển LED
void Led7_Init(void) {
// Cấu hình chân 74HC595 làm Output
HC595_DDR |= (1 << DS_PIN) | (1 << SH_CP_PIN) | (1 << ST_CP_PIN);

text
// Cấu hình 3 chân điều khiển Transistor làm Output
DIGIT_DDR |= (1 << D1_PIN) | (1 << D2_PIN) | (1 << D3_PIN);

// Ban đầu tắt hết cả 3 số (đưa Base Transistor NPN xuống LOW)
DIGIT_PORT &= ~((1 << D1_PIN) | (1 << D2_PIN) | (1 << D3_PIN));

}

// Hàm dịch 8 bit dữ liệu vào IC 74HC595 (Bit-banging)
void HC595_ShiftOut(uint8_t data) {
for (int8_t i = 7; i >= 0; i--) {
// Đưa Data ra chân DS (Xuất từ bit cao xuống bit thấp)
if (data & (1 << i)) {
HC595_PORT |= (1 << DS_PIN);
} else {
HC595_PORT &= ~(1 << DS_PIN);
}

text
    // Tạo xung Shift Clock (Kéo lên HIGH rồi hạ xuống LOW)
    HC595_PORT |= (1 << SH_CP_PIN);
    HC595_PORT &= ~(1 << SH_CP_PIN);
}

// Tạo xung Latch Clock để xuất toàn bộ 8 bit ra các chân Q0-Q7
HC595_PORT |= (1 << ST_CP_PIN);
HC595_PORT &= ~(1 << ST_CP_PIN);

}

// Hàm chia tách một số nguyên (0-999) vào mảng buffer để tự động hiển thị
// Hàm chia tách một số nguyên và XÓA SỐ 0 VÔ NGHĨA Ở ĐẦU
void Led7_SetNumber(uint16_t number) {
if (number > 999) number = 999;

text
uint8_t tram = number / 100;
uint8_t chuc = (number / 10) % 10;
uint8_t donvi = number % 10;

// --- Xử lý hàng Trăm ---
if (tram == 0) {
	led_buffer[0] = 0x00; // 0x00 là tắt đèn xì (Không cấp điện cho 595)
	} else {
	led_buffer[0] = LED_MAP[tram];
}

// --- Xử lý hàng Chục ---
if (tram == 0 && chuc == 0) {
	led_buffer[1] = 0x00; // Trăm bằng 0 và Chục cũng bằng 0 -> Tắt luôn hàng chục
	} else {
	led_buffer[1] = LED_MAP[chuc];
}

// --- Xử lý hàng Đơn vị ---
led_buffer[2] = LED_MAP[donvi]; // Hàng đơn vị thì luôn hiển thị kể cả là số 0

}
// Thay 0x80 bằng mã Hex dấu chấm mà bạn vừa để được!
#define DP_BIT 0x80

// =================================================================
// Hàm in số thập phân (Ví dụ truyền vào 12.3 thì LED hiển 12.3)
// number: Số thực cần in (0.00 đến 99.9)
// decimal_places: Số chữ số sau dấu phẩy (1 hoặc 2)
// =================================================================
void Led7_SetFloat(float number, uint8_t decimal_places) {
if (number > 99.9) number = 99.9; // Giới hạn hiển thị
if (number < 0) number = 0; // Tạm thời bỏ qua số âm cho dễ nhận

text
if (decimal_places == 1) {
	// Ví dụ: number = 12.3
	// Nhân 10 để biến thành số nguyên: 123
	uint16_t int_val = (uint16_t)(number * 10.0 + 0.5);
	
	uint8_t chuc = (int_val / 100) % 10;
	uint8_t donvi = (int_val / 10) % 10;
	uint8_t thapphan = int_val % 10;

	// Xóa số 0 vô nghĩa ở hàng chục
	if (chuc == 0) led_buffer[0] = 0x00;
	else led_buffer[0] = LED_MAP[chuc];

	// Lấy mã số hàng đơn vị CỘNG THÊM dấu chấm!
	led_buffer[1] = LED_MAP[donvi] | DP_BIT;
	
	led_buffer[2] = LED_MAP[thapphan];
}
else if (decimal_places == 2) {
	// Ví dụ: number = 5.67
	// Nhân 100 để biến thành số nguyên: 567
	uint16_t int_val = (uint16_t)(number * 100.0 + 0.5);
	
	uint8_t donvi = (int_val / 100) % 10;
	uint8_t phan_muoi = (int_val / 10) % 10;
	uint8_t phan_tram = int_val % 10;

	// Lấy mã số hàng đơn vị CỘNG THÊM dấu chấm!
	led_buffer[0] = LED_MAP[donvi] | DP_BIT;
	
	led_buffer[1] = LED_MAP[phan_muoi];
	led_buffer[2] = LED_MAP[phan_tram];
}

}
// Biến lưu trữ số đếm Encoder (Dùng int32_t vì xung có thể lên số âm/dương rất lớn)
volatile int32_t encA_ticks = 0;
volatile int32_t encB_ticks = 0;

// Biến đếm thời gian thực (đơn vị mili-giây)
volatile uint32_t sys_time_ms = 0;
void Motor_Init(void) {
// 1. Cấu hình các chân điều hướng và STBY làm Output
DDRB |= (1 << AIN1) | (1 << AIN2) | (1 << STBY) | (1 << BIN1) | (1 << BIN2);

text
// 2. Cấu hình chân phát bpm xung (PWM) làm Output
DDRD |= (1 << PWMA) | (1 << PWMB);

// 3. Bật chân STBY lên mức cao (Mức 1) để đánh thức TB6612 khỏi chế độ ngủ
PORTB |= (1 << STBY);

// 4. Thiết lập Timer1 ở chế độ Fast PWM 8-bit (Giới hạn xung từ 0 - 255)
// COM1A1 = 1, COM1B1 = 1: Chế độ xuất xung thuận (Non-inverting) trên OC1A và OC1B
TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM10);

// WGM12 = 1 (Fast PWM 8-bit)
// CS11 = 1: Prescaler = 8 -> Xung nhập Timer = 16MHz / 8 = 2MHz.
// Tần số PWM = 2MHz / 256 = ~7.8 kHz (Rất là tương cho motor DC và TB6612)
TCCR1B = (1 << WGM12) | (1 << CS11);

// Khởi tạo tốc độ ban đầu = 0 (đứng im)
OCR1B = 0; // Tốc độ PWMA (Motor A)
OCR1A = 0; // Tốc độ PWMB (Motor B)

}
// Hàm điều khiển tốc độ: speed từ -255 đến 255
void Motor_SetSpeed(int16_t speedA, int16_t speedB) {
// --- Xử lý Motor A (Trái) ---
if (speedA > 0) {
PORTB |= (1 << AIN1);
PORTB &= ~(1 << AIN2);
if (speedA > 255) speedA = 255; // Giới hạn ngưỡng an toàn
OCR1B = speedA;
} else if (speedA < 0) {
PORTB &= ~(1 << AIN1);
PORTB |= (1 << AIN2);
if (speedA < -255) speedA = -255;
OCR1B = -speedA; // Lấy trị tuyệt đối đưa vào thanh ghi PWM
} else {
PORTB &= ~(1 << AIN1);
PORTB &= ~(1 << AIN2);
OCR1B = 0;
}

text
// --- Xử lý Motor B (Phải) ---
if (speedB > 0) {
	PORTB |= (1 << BIN1);
	PORTB &= ~(1 << BIN2);
	if (speedB > 255) speedB = 255;
	OCR1A = speedB;
	} else if (speedB < 0) {
	PORTB &= ~(1 << BIN1);
	PORTB |= (1 << BIN2);
	if (speedB < -255) speedB = -255;
	OCR1A = -speedB;
	} else {
	PORTB &= ~(1 << BIN1);
	PORTB &= ~(1 << BIN2);
	OCR1A = 0;
}

}

// Hàm chạy kịch bản Test phần cứng
void Motor_Test(void) {
UART_SendString("[TEST] 1. Motor Tiến chậm (Speed 80)...\r\n");
Motor_SetSpeed(80, 80);
_delay_ms(2000);

text
UART_SendString("[TEST] 2. Motor Tiến MAX (Speed 255)...\r\n");
Motor_SetSpeed(255, 255);
_delay_ms(2000);

UART_SendString("[TEST] 3. Dừng Motor...\r\n");
Motor_SetSpeed(0, 0);
_delay_ms(1000);

UART_SendString("[TEST] 4. Motor Lùi (Speed -150)...\r\n");
Motor_SetSpeed(-150, -150);
_delay_ms(2000);

UART_SendString("[TEST] 5. Quay xe tại chỗ (A lùi, B tiến)...\r\n");
Motor_SetSpeed(-100, 100);
_delay_ms(2000);

UART_SendString("[TEST] XONG! Dừng hãn để chờ lệnh tiếp theo.\r\n");
Motor_SetSpeed(0, 0);

}

// ================= ADVANCED PATH TRACKING & PID CONTROL =================

// Tính toán vận tốc 2 bánh từ encoder
void getWheelVelocities(float &vL, float &vR, float dt) {
long dl = encA_ticks - prevLeft;
long dr = -encB_ticks + prevRight;

text
prevLeft = encA_ticks;
prevRight = encB_ticks;

vL = (dl * TICK_TO_M) / dt;
vR = (dr * TICK_TO_M) / dt;

}

// Cập nhật Odometry (vị trí và hướng)
void updateOdometry(float dt) {
long dl = encA_ticks - prevLeft;
long dr = -encB_ticks + prevRight;

text
prevLeft = encA_ticks;
prevRight = encB_ticks;

float dL = dl * TICK_TO_M;
float dR = dr * TICK_TO_M;

float dCenter = (dL + dR) / 2.0f;
float dTheta = (dR - dL) / WHEEL_BASE;

x += dCenter * cos(theta + dTheta / 2.0f);
y += dCenter * sin(theta + dTheta / 2.0f);
theta += dTheta;

if (theta > PI) theta -= 2 * PI;
else if (theta < -PI) theta += 2 * PI;

}

// Tìm điểm lookahead trên đường đi
Point getLookahead(float lookaheadDist) {
for (int i = currentTarget; i < num_points; i++) {
float dx = path[i].x - x;
float dy = path[i].y - y;
float dist = sqrt(dx * dx + dy * dy);
if (dist >= lookaheadDist) {
currentTarget = i;
return path[i];
}
}
if (num_points > 0) return path[num_points - 1];
return {x, y};
}

// Thực hiện tính toán PID
float PID(float error, float &prevErr, float &integral, float antiwindup, float dt) {
if (antiwindup < 255 && antiwindup > -255)
integral += error * dt;

text
float derivative = (error - prevErr) / dt;
float out = Kp * error + Ki * integral + Kd * derivative;
prevErr = error;
return out;

}

// Reset trạng thái PID
void resetPID(float &prevErr, float &integral, float &pwm, int32_t &ticks) {
prevErr = 0.0f;
integral = 0.0f;
pwm = 0.0f;
ticks = 0;
}

// Dừng motor (dừng cả hai bánh)
void brake(void) {
Motor_SetSpeed(0, 0);
}

// Vòng điều khiển chính
void controlLoop(void) {
if (num_points == 0 || finished) {
resetPID(prevErrL, intL, pwml1, encA_ticks);
resetPID(prevErrR, intR, pwmr1, encB_ticks);
prevLeft = 0;
prevRight = 0;
return;
}

text
if (currentTarget >= num_points - 1) {
	Motor_SetSpeed(0, 0);
	intL = 0;
	intR = 0;
	prevErrL = 0;
	prevErrR = 0;
	finished = 1;
	return;
}

Point goal = getLookahead(LOOKAHEAD_DIST);
float dx = goal.x - x;
float dy = goal.y - y;
float dist = sqrt(dx * dx + dy * dy);

// Nếu gần điểm trước thì sang điểm tiếp theo
if (dist < 0.08f && currentTarget < num_points - 1) {
	currentTarget++;
	return;
}

float targetAngle = atan2(dy, dx);
float angleError = targetAngle - theta;
chuan_hoa(angleError);

// Chỉ hành động khi sai góc nhỏ hơn 90 độ
if (fabs(angleError) <= PI / 2) {
	uint32_t now = sys_time_ms;
	float dt = (now - lastTime) / 1000.0f;
	if (dt <= 0) dt = 0.01f;
	if (dt > 0.1f) dt = 0.1f;
	lastTime = now;

	// Tính vận tốc góc
	float w = MAX_SPEED * 2 * (-sin(theta) * dx + cos(theta) * dy) / (dist * dist);
	vL = MAX_SPEED - w * WHEEL_BASE / 2.0f;
	vR = MAX_SPEED + w * WHEEL_BASE / 2.0f;

	// Cập nhật odometry
	updateOdometry(dt);

	// Lấy vận tốc đo được từ encoder
	float vL_meas = 0.0f, vR_meas = 0.0f;
	getWheelVelocities(vL_meas, vR_meas, dt);

	// Tính sai số vận tốc
	float errL = vL - vL_meas;
	float errR = vR - vR_meas;

	// Tính toán PID
	float pwmL = PID(errL, prevErrL, intL, pwml1, dt);
	float pwmR = PID(errR, prevErrR, intR, pwmr1, dt);

	pwml1 = pwmL;
	pwmr1 = pwmR;

	// Giới hạn PWM
	if (pwmL > 255) pwmL = 255;
	if (pwmL < -255) pwmL = -255;
	if (pwmR > 255) pwmR = 255;
	if (pwmR < -255) pwmR = -255;

	// Điều khiển motor
	Motor_SetSpeed((int16_t)pwmL, (int16_t)pwmR);

	// Gửi thông tin debug qua UART
	UART_SendString("POS:");
	UART_PrintFloat(x);
	UART_SendString(",");
	UART_PrintFloat(y);
	UART_SendString(" ERR:");
	UART_PrintFloat(errL);
	UART_SendString(",");
	UART_PrintFloat(errR);
	UART_SendString("\r\n");
} 
else {
	// Sai góc quá lớn, dừng và reset PID
	brake();
	resetPID(prevErrL, intL, pwml1, encA_ticks);
	resetPID(prevErrR, intR, pwmr1, encB_ticks);
	prevLeft = 0;
	prevRight = 0;
	return;
}

}

// Cấu hình Baudrate 57600 (Sử dụng chế độ U2X = 1)
#define BAUD 57600
#define UBRR_VAL ((F_CPU / (8UL * BAUD)) - 1)

#define MAX_POINTS 20

typedef struct {
float x;
float y;
} Point;

Point path[MAX_POINTS];

typedef enum {
WAIT_HEADER_1, WAIT_HEADER_2,
READ_NUM_L, READ_NUM_H,
READ_PAYLOAD, READ_CHECKSUM
} ReceiveState;

ReceiveState state = WAIT_HEADER_1;
uint16_t num_points = 0;
uint16_t payload_bytes_to_read = 0;
uint16_t payload_index = 0;
uint8_t payload_buffer[MAX_POINTS * 8];
uint8_t calculated_checksum = 0;

// ================= CÁC HÀM GIAO TIẾP UART ==================

void UART_Init(void) {
UCSRA = (1 << U2X);
UBRRH = (uint8_t)(UBRR_VAL >> 8);
UBRRL = (uint8_t)UBRR_VAL;
UCSRB = (1 << RXEN) | (1 << TXEN);
UCSRC = (1 << URSEL) | (1 << UCSZ1) | (1 << UCSZ0);
}

void UART_Transmit(uint8_t data) {
while (!(UCSRA & (1 << UDRE)));
UDR = data;
}

void UART_SendString(const char* str) {
while (*str) {
UART_Transmit(*str++);
}
}

uint8_t UART_Available(void) {
return (UCSRA & (1 << RXC));
}

uint8_t UART_Receive(void) {
while (!(UCSRA & (1 << RXC)));
return UDR;
}

// ================= CÁC HÀM TỰ CODE (Thay thế thư viện chuẩn) =================

// Tự code hàm in số nguyên (Thay thế sprintf / itoa)
void UART_PrintInt(uint16_t num) {
if (num == 0) {
UART_Transmit('0');
return;
}

text
char buf[6]; // Để chứa số uint16_t (tối đa 65535)
int8_t i = 0;

while (num > 0) {
	buf[i++] = (num % 10) + '0';
	num /= 10;
}

// In ngược mảng ra UART
while (i > 0) {
	UART_Transmit(buf[--i]);
}

}
void UART_PrintInt32(int32_t num) {
if (num == 0) {
UART_Transmit('0');
return;
}
if (num < 0) {
UART_Transmit('-');
num = -num;
}

text
char buf[12]; // Để chứa số int32 (tối đa ~2 tỉ)
int8_t i = 0;

while (num > 0) {
	buf[i++] = (num % 10) + '0';
	num /= 10;
}

while (i > 0) {
	UART_Transmit(buf[--i]);
}

}
// Tự code hàm in số thập phân (Thay thế dtostrf)
void UART_PrintFloat(float num) {
// Xử lý số âm
if (num < 0) {
UART_Transmit('-');
num = -num;
}

text
// Tách phần nguyên
uint16_t int_part = (uint16_t)num;

// Tách phần thập phân (nhân 1000 để lấy 3 chữ số, cộng 0.5 để làm tròn)
float dec_part = num - int_part;
uint16_t dec_int = (uint16_t)(dec_part * 1000.0 + 0.5);

UART_PrintInt(int_part);
UART_Transmit('.');

// Xử lý in các số 0 ở đầu phần thập phân (VD: 0.005)

if (dec_int < 100) UART_Transmit('0');
if (dec_int < 10) UART_Transmit('0');

UART_PrintInt(dec_int);

}

// ================= XỬ LÝ DỮ LIỆU BẢN ĐỒ =================

void ParseAndPrintPath(void) {
UART_SendString("\r\n==================================\r\n");
UART_SendString("[ATmega16A] NHẬN THÀNH CÔNG PATH!\r\n");
UART_SendString("Số lượng điểm: ");
UART_PrintInt(num_points);
UART_SendString("\r\n");

text
for (uint16_t i = 0; i < num_points; i++) {
	float x, y;
	
	// Tự code thay thế hàm memcpy()
	// Ép kiểu địa chỉ của x và y thành con trỏ byte, sau đó gán từng byte một
	uint8_t* px = (uint8_t*)&x;
	uint8_t* py = (uint8_t*)&y;
	for(uint8_t j = 0; j < 4; j++) {
		px[j] = payload_buffer[i * 8 + j];
		py[j] = payload_buffer[i * 8 + 4 + j];
	}
	
	path[i].x = x;
	path[i].y = y;
	
	// In ra màn hình Debug
	UART_SendString("P[");
	UART_PrintInt(i);
	UART_SendString("]: ");
	UART_PrintFloat(x);
	UART_SendString(" , ");
	UART_PrintFloat(y);
	UART_SendString("\r\n");
}
UART_SendString("==================================\r\n");

}
void Encoder_Init(void) {
// 1. Cấu hình chân D2 (INT0) và D3 (INT1) làm Input, bật điện trở kéo lên (Pull-up)
DDRD &= ~((1 << PD2) | (1 << PD3));
PORTD |= (1 << PD2) | (1 << PD3);

text
// 2. Cấu hình chân C0 và C1 làm Input, bật Pull-up
DDRC &= ~((1 << PC0) | (1 << PC1));
PORTC |= (1 << PC0) | (1 << PC1);

// 3. Cài đặt Ngắt kích hoạt ở "Sườn lên" (Rising Edge) cho cả INT0 và INT1
MCUCR |= (1 << ISC01) | (1 << ISC00) | (1 << ISC11) | (1 << ISC10);

// 4. Cho phép INT0 và INT1 hoạt động trong thanh ghi GICR
GICR |= (1 << INT0) | (1 << INT1);

}

void Timer0_Millis_Init(void) {
// Thiết lập Timer0 ở chế độ Normal, Prescaler = 64
// Tần số Timer: 16MHz / 64 = 250 kHz -> 1 chu kỳ máy tính 4us.
// Tràn Timer (256 chu kỳ) tính: 256 * 4us = 1.024 mili-giây (~ 1ms)
TCCR0 = (1 << CS01) | (1 << CS00);

text
// Bật ngắt khi tràn Timer0
TIMSK |= (1 << TOIE0);

}

// ================= CÁC TRÌNH PHỤC VỤ NGẮT (ISR) =================

// Hàm ngắt đếm mili-giây chạy ngầm
// Hàm ngắt đếm mili-giây và kết hợp quét LED
ISR(TIMER0_OVF_vect) {
// 1. Việc cơ bản: đếm thời gian
sys_time_ms += 2; // (Do tràn ở 8MHz tần ~2ms nên ta cộng 2 cho chuẩn thực tế)

text
// 2. VIỆC MỚI: QUÉT LED 7 ĐOẠN
// Bước 1: Tắt toàn bộ 3 transistor trước để chống hiện tượng "bóng ma" (Ghosting)
DIGIT_PORT &= ~((1 << D1_PIN) | (1 << D2_PIN) | (1 << D3_PIN));

// Bước 2: Bơm dữ liệu mã nút của chỉ số hiện tại vào IC 74HC595
HC595_ShiftOut(led_buffer[current_digit]);

// Bước 3: Bật transistor tương ứng để con số đó phát sáng
if (current_digit == 0) {
    DIGIT_PORT |= (1 << D1_PIN);      // Bật số hàng trăm
} 
else if (current_digit == 1) {
    DIGIT_PORT |= (1 << D2_PIN);      // Bật số hàng chục
} 
else if (current_digit == 2) {
    DIGIT_PORT |= (1 << D3_PIN);      // Bật số hàng đơn vị
}

// Bước 4: Tăng biến đếm để lần ngắt tiếp theo quét sang vị trí số kế bên
current_digit++;
if (current_digit > 2) current_digit = 0; // Quay vòng lại số đầu tiên

}

// Hàm ngắt cho Encoder A (Chân D3 - INT1)
ISR(INT1_vect) {
// Đọc trạng thái chân C1 để biết chiều quay
if (PINC & (1 << PC1)) {
encA_ticks++; // Nếu quay thuận
} else {
encA_ticks--; // Nếu quay nghịch
}
}

// Hàm ngắt cho Encoder B (Chân D2 - INT0)
ISR(INT0_vect) {
// Đọc trạng thái chân C0 để biết chiều quay
if (PINC & (1 << PC0)) {
encB_ticks++;
} else {
encB_ticks--;
}
}
// ================= HÀM CHÍNH =================

int main(void) {
UART_Init();
Motor_Init(); // Khởi tạo chân Motor và Timer1
Encoder_Init();
Led7_Init();
Timer0_Millis_Init(); // Khởi tạo bộ đếm thời gian

text
sei(); // QUAN TRỌNG: Lệnh bật Ngắt toàn cực (Global Interrupt Enable)
_delay_ms(100);
UART_SendString("\r\n[ATmega16A] Đã khởi động.\r\n");

Led7_SetNumber(107);
_delay_ms(1000);

text
// Chạy chuỗi test phần cứng 1 lần lúc bật nguồn
Motor_Test();

UART_SendString("[ATmega16A] Chờ dữ liệu path từ ESP32...\r\n");
uint32_t last_print_time = 0; // Biến lưu mốc thời gian in cuối cùng
uint32_t last_control_time = 0; // Biến lưu thời gian điều khiển
while (1) {
	// Gọi controlLoop nếu có path và chưa hoàn thành
	if (sys_time_ms - last_control_time >= 50) {  // 50ms -> 20Hz control
		last_control_time = sys_time_ms;
		controlLoop();
	}
	
	// In thông tin debug
	if (sys_time_ms - last_print_time >= 1000) {
		last_print_time = sys_time_ms; // Cập nhật mốc thời gian
		
		UART_SendString("EncA: ");
		UART_PrintInt32(encA_ticks);
		UART_SendString(" | EncB: ");
		UART_PrintInt32(encB_ticks);
		UART_SendString(" | Pos(");
		UART_PrintFloat(x);
		UART_SendString(",");
		UART_PrintFloat(y);
		UART_SendString(")\r\n");
	}
	if (UART_Available()) {
		uint8_t b = UART_Receive();

		switch (state) {
			case WAIT_HEADER_1:
			if (b == 0xAA) state = WAIT_HEADER_2;
			break;
			
			case WAIT_HEADER_2:
			if (b == 0xAA) state = READ_NUM_L;
			else if (b != 0xAA) state = WAIT_HEADER_1;
			break;
			
			case READ_NUM_L:
			num_points = b;
			calculated_checksum = b;
			state = READ_NUM_H;
			break;
			
			case READ_NUM_H:
			num_points |= (b << 8);
			calculated_checksum ^= b;
			
			if (num_points == 0) {
				state = WAIT_HEADER_1;
				} else if (num_points > MAX_POINTS) {
				UART_SendString("[LOI] So diem vuot qua RAM!\r\n");
				state = WAIT_HEADER_1;
				} else {
				payload_bytes_to_read = num_points * 8;
				payload_index = 0;
				state = READ_PAYLOAD;
			}
			break;
			
			case READ_PAYLOAD:
			payload_buffer[payload_index++] = b;
			calculated_checksum ^= b;
			if (payload_index >= payload_bytes_to_read) {
				state = READ_CHECKSUM;
			}
			break;
			
			case READ_CHECKSUM:
			if (calculated_checksum == b) {
				ParseAndPrintPath();
				} else {
				UART_SendString("[LOI] Checksum khong khop!\r\n");
			}
			state = WAIT_HEADER_1;
			break;
		}
	}
}

}
đánh giá các nhận xét sau và kiểm tra thêm các lỗ hổng, sau đó chỉ tôi cách sửa code:
LỖI NGHIÊM TRỌNG: updateOdometry() và getWheelVelocities() dùng chung encoder delta

Hiện tại:

getWheelVelocities()

đã cập nhật:

prevLeft
prevRight

sau đó:

updateOdometry()

lại tiếp tục dùng:

encA_ticks - prevLeft

=> delta ≈ 0.

Odometry gần như hỏng.

Cách đúng

Chỉ đọc encoder delta 1 lần.

Ví dụ:

long dl = encA_ticks - prevLeft;
long dr = -encB_ticks + prevRight;

prevLeft = encA_ticks;
prevRight = encB_ticks;

sau đó:

dùng chung dl/dr
cho cả velocity + odometry.
2. Sai dấu encoder phải

Bạn viết:

long dr = -encB_ticks + prevRight;

Nên viết rõ:

long dr = prevRight - encB_ticks;

hoặc:

long dr = -(encB_ticks - prevRight);

để tránh nhầm logic.

3. float & là cú pháp C++, không phải C

ATmega16 GCC mặc định là C.

Đoạn này:

void chuan_hoa(float &angle)

là C++ reference.

Nếu project build bằng GCC C:
→ compile lỗi.

Phải đổi:

void chuan_hoa(float *angle)

và:

while (*angle > PI)

Hoặc build project dạng C++.

4. return {x, y}; cũng là C++

Đoạn:

return {x, y};

không hợp lệ trong C AVR.

Phải:

Point p = {x, y};
return p;
5. Dùng float quá nhiều trên ATmega16

ATmega16:

không có FPU
float rất chậm

Bạn đang dùng:

sin
cos
atan2
sqrt
fabs

liên tục trong loop 20Hz.

Có thể:

CPU quá tải
UART lag
interrupt miss.
Khuyến nghị

Nếu robot line follower:

KHÔNG cần pure pursuit full float.

Chỉ cần:

PID line sensor
hoặc PID encoder đơn giản.

Nếu vẫn muốn path tracking:

dùng fixed-point
hoặc chuyển STM32/ESP32.
6. ISR + biến 32 bit chưa atomic

Ví dụ:

volatile int32_t encA_ticks;

ATmega16 là 8-bit.

Đọc:

encA_ticks

trong main có thể bị ISR sửa giữa chừng.

=> corrupted value.

Phải bảo vệ:
cli();
local = encA_ticks;
sei();

hoặc ATOMIC_BLOCK.

7. sys_time_ms += 2

Không chính xác tuyệt đối.

Với:

8MHz
prescaler 64

overflow:

256 * 64 / 8MHz
= 2.048ms

Sai số tích lũy lớn theo thời gian.

Nên dùng CTC mode

Chuẩn 1ms.

8. PID chưa anti-windup đúng

Bạn có:

if (antiwindup < 255 && antiwindup > -255)

nhưng:

antiwindup chính là pwm cũ
không clamp integral.

Nên:

integral += error * dt;

if(integral > LIM) integral = LIM;
if(integral < -LIM) integral = -LIM;
9. PWM signed nhưng không deadband

Motor DC thực:

PWM nhỏ không quay.

Nên thêm:

if (pwm > 0 && pwm < 40) pwm = 40;