#define F_CPU 8000000UL #include <avr/io.h> #incl...

#define F_CPU 8000000UL

#include <avr/io.h>
#include <util/delay.h>
#include <math.h>
#include <avr/interrupt.h>
// ===== Cau hinh TB6612 =====
#define AIN1 PB0
#define AIN2 PB1
#define STBY PB2
#define BIN1 PB3
#define BIN2 PB4
#define PWMA PD4 // OC1B
#define PWMB PD5 // OC1A
// ===== Cau hinh LED 7 va 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 1 (Tram)
#define D2_PIN PA3 // Transistor 2 (Chuc)
#define D3_PIN PA4 // Transistor 3 (Donvi)

// Ma Hex hien thi so 0->9 (Cathode chung)
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}; // Ma LED 3 chu so
volatile uint8_t current_digit = 0; // Chi so hien tai
#define MAX_POINTS 40

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

Point path[MAX_POINTS];
// Cau hinh UART 9600 baud
#define BAUD 9600
#define UBRR_VAL ((F_CPU / (8UL * BAUD)) - 1)

typedef enum {
WAIT_HEADER_1, WAIT_HEADER_2,
READ_NUM_L, READ_NUM_H,
READ_CHECKSUM, READ_PAYLOAD
} 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 calculated_checksum = 0;

// ===== Dem UART 64 bytes =====
#define RX_BUF_SIZE 64
#define RX_BUF_MASK (RX_BUF_SIZE - 1)

volatile uint8_t rx_buf[RX_BUF_SIZE];
volatile uint8_t rx_head = 0;
volatile uint8_t rx_tail = 0;

void UART_Init(void) {
UCSRA = (1 << U2X);
UBRRH = (uint8_t)(UBRR_VAL >> 8);
UBRRL = (uint8_t)UBRR_VAL;

text

// Bat ngu nhan UART
UCSRB = (1 << RXEN) | (1 << TXEN) | (1 << RXCIE);
UCSRC = (1 << URSEL) | (1 << UCSZ1) | (1 << UCSZ0);
}

// Ngu nhan UART
ISR(USART_RXC_vect) {
rx_buf[rx_head & RX_BUF_MASK] = UDR;
rx_head++;
}

uint8_t UART_Available(void) {
return (uint8_t)(rx_head - rx_tail);
}

uint8_t UART_Receive(void) {
return rx_buf[rx_tail++ & RX_BUF_MASK];
}

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

void UART_SendString(const char* str) {
while (*str) {
UART_Transmit(*str++);
}
}
// ===== Odometry & Path Tracking =====
#define PI 3.14159265359f
#define WHEEL_BASE 0.15f

#define WHEEL_DIAMETER 0.072f
#define WHEEL_CIRC (PI * WHEEL_DIAMETER)
#define ENCODER_RES 616.0f

#define TICK_TO_M (WHEEL_CIRC / ENCODER_RES)

#define Kp 90.0f
#define Ki 00.0f
#define Kd 0.0f
#define LOOKAHEAD_DIST 0.14f
volatile uint8_t speed_cm = 19; // Toc do (cm/s)
volatile float max_speed = 0.19f; // Toc do max (m/s)
// ===== Bien Odometry =====
uint8_t parse_point_idx = 0;
uint8_t parse_byte_idx = 0;
// Bien 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;
volatile float vL_filt = 0.0f;
volatile float vR_filt = 0.0f;
#define VEL_ALPHA 0.4f
#define PID_INTEGRAL_MAX 50.0f
#define MOTOR_DEADBAND 10 // Deadband PWM

// Chuan hoa goc
void chuan_hoa(float *angle) {
while (*angle > PI) *angle -= 2 * PI;
while (*angle < -PI) *angle += 2 * PI;
}

// Khoi tao LED 7 doan
void Led7_Init(void) {
HC595_DDR |= (1 << DS_PIN) | (1 << SH_CP_PIN) | (1 << ST_CP_PIN);
DIGIT_DDR |= (1 << D1_PIN) | (1 << D2_PIN) | (1 << D3_PIN);
DIGIT_PORT &= ~((1 << D1_PIN) | (1 << D2_PIN) | (1 << D3_PIN));
}

// Dich 8 bit vao 74HC595
void HC595_ShiftOut(uint8_t data) {
for (int8_t i = 7; i >= 0; i--) {
if (data & (1 << i)) {
HC595_PORT |= (1 << DS_PIN);
} else {
HC595_PORT &= ~(1 << DS_PIN);
}
HC595_PORT |= (1 << SH_CP_PIN);
HC595_PORT &= ~(1 << SH_CP_PIN);
}
HC595_PORT |= (1 << ST_CP_PIN);
HC595_PORT &= ~(1 << ST_CP_PIN);
}

void Led7_SetNumber(uint16_t number) {
if (number > 999) number = 999;
uint8_t tram = number / 100;
uint8_t chuc = (number / 10) % 10;
uint8_t donvi = number % 10;
if (tram == 0) {
led_buffer[0] = 0x00;
} else {
led_buffer[0] = LED_MAP[tram];
}
if (tram == 0 && chuc == 0) {
led_buffer[1] = 0x00;
} else {
led_buffer[1] = LED_MAP[chuc];
}
led_buffer[2] = LED_MAP[donvi];
}
// Bit dau phay
#define DP_BIT 0x80

void Led7_SetFloat(float number, uint8_t decimal_places) {
if (number > 99.9) number = 99.9;
if (number < 0) number = 0;
if (decimal_places == 1) {
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;
if (chuc == 0) led_buffer[0] = 0x00;
else led_buffer[0] = LED_MAP[chuc];
led_buffer[1] = LED_MAP[donvi] | DP_BIT;
led_buffer[2] = LED_MAP[thapphan];
}
else if (decimal_places == 2) {
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;
led_buffer[0] = LED_MAP[donvi] | DP_BIT;
led_buffer[1] = LED_MAP[phan_muoi];
led_buffer[2] = LED_MAP[phan_tram];
}
}
// Bien dem encoder
volatile int32_t encA_ticks = 0;
volatile int32_t encB_ticks = 0;
volatile uint32_t sys_time_ms = 0;
void Motor_Init(void) {
DDRB |= (1 << AIN1) | (1 << AIN2) | (1 << STBY) | (1 << BIN1) | (1 << BIN2);
DDRD |= (1 << PWMA) | (1 << PWMB);
PORTB |= (1 << STBY);
TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << WGM10);
TCCR1B = (1 << WGM12) | (1 << CS11);
OCR1B = 0;
OCR1A = 0;
}
// Dieu khien toc do motor
void Motor_SetSpeed(int16_t speedA, int16_t speedB) {
if (speedA > 0) {
PORTB |= (1 << AIN1);
PORTB &= ~(1 << AIN2);
if (speedA > 255) speedA = 255;
OCR1B = speedA;
} else if (speedA < 0) {
PORTB &= ~(1 << AIN1);
PORTB |= (1 << AIN2);
if (speedA < -255) speedA = -255;
OCR1B = -speedA;
} else {
PORTB &= ~(1 << AIN1);
PORTB &= ~(1 << AIN2);
OCR1B = 0;
}
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;
}
}

// ===== Path Tracking & PID =====
void readEncoderDelta(int32_t *dl, int32_t *dr) {
cli();
int32_t tempA = encA_ticks;
int32_t tempB = encB_ticks;
sei();
*dl = tempA - prevLeft;
*dr = tempB - prevRight;
prevLeft = tempA;
prevRight = tempB;
}

// Tinh toc do 2 banh
void getWheelVelocities(float *vL, float *vR, float dt) {
int32_t dl, dr;
readEncoderDelta(&dl, &dr);

text

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

// Cap nhat Odometry
void updateOdometry(float dt, int32_t dl, int32_t dr) {

text

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;

chuan_hoa(&theta);
}

// Tim diem lookahead
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];
Point p;
p.x = x;
p.y = y;
return p;
}

// PID control
float PID(float error, float *prevErr, float *integral, float dt) {
*integral += error * dt;
if (*integral > PID_INTEGRAL_MAX) *integral = PID_INTEGRAL_MAX;
if (*integral < -PID_INTEGRAL_MAX) *integral = -PID_INTEGRAL_MAX;
float derivative = (error - *prevErr) / dt;
float out = Kp * error + Ki * (*integral) + Kd * derivative;
*prevErr = error;
return out;
}

// Reset PID
void resetPID(float *prevErr, float *integral, float *pwm) {
*prevErr = 0.0f;
*integral = 0.0f;
*pwm = 0.0f;
}

// Gioi han PWM
int16_t applyMotorLimits(float pwm) {
int16_t pwm_int = (int16_t)pwm;
if (pwm_int > 255) pwm_int = 255;
if (pwm_int < -255) pwm_int = -255;
if (pwm_int > 0 && pwm_int < MOTOR_DEADBAND) pwm_int = MOTOR_DEADBAND;
if (pwm_int < 0 && pwm_int > -MOTOR_DEADBAND) pwm_int = -MOTOR_DEADBAND;
return pwm_int;
}

// Dung motor
void brake(void) {
Motor_SetSpeed(0, 0);
}

// Vong dieu khien chinh
void controlLoop(void) {
if (num_points == 0 || finished) {
resetPID(&prevErrL, &intL, &pwml1);
resetPID(&prevErrR, &intR, &pwmr1);
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);

if (dist < 0.08f && currentTarget < num_points - 1) {
currentTarget++;
return;
}

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

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;
int32_t dl, dr;
readEncoderDelta(&dl, &dr);
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;
updateOdometry(dt, dl, dr);
float vL_meas = (dl * TICK_TO_M) / dt;
float vR_meas = (dr * TICK_TO_M) / dt;
vL_filt = VEL_ALPHA * vL_meas + (1.0f - VEL_ALPHA) * vL_filt;
vR_filt = VEL_ALPHA * vR_meas + (1.0f - VEL_ALPHA) * vR_filt;
float errL = vL - vL_meas;
float errR = vR - vR_meas;
float pwmL = PID(errL, &prevErrL, &intL, dt);
float pwmR = PID(errR, &prevErrR, &intR, dt);
pwml1 = pwmL;
pwmr1 = pwmR;
int16_t pwmL_final = applyMotorLimits(pwmL);
int16_t pwmR_final = applyMotorLimits(pwmR);
Motor_SetSpeed(pwmR_final, pwmL_final);
}

// ===== Cac ham in =====
void UART_PrintInt(uint16_t num) {
if (num == 0) {
UART_Transmit('0');
return;
}
char buf[6];
int8_t i = 0;
while (num > 0) {
buf[i++] = (num % 10) + '0';
num /= 10;
}
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;
}
char buf[12];
int8_t i = 0;
while (num > 0) {
buf[i++] = (num % 10) + '0';
num /= 10;
}
while (i > 0) {
UART_Transmit(buf[--i]);
}
}
void UART_PrintFloat(float num) {
if (num < 0) {
UART_Transmit('-');
num = -num;
}
uint16_t int_part = (uint16_t)num;
float dec_part = num - int_part;
uint16_t dec_int = (uint16_t)(dec_part * 1000.0 + 0.5);
UART_PrintInt(int_part);
UART_Transmit('.');
if (dec_int < 100) UART_Transmit('0');
if (dec_int < 10) UART_Transmit('0');
UART_PrintInt(dec_int);
}
void Hardware_Init(void) {
DDRD &= ~((1 << PD2) | (1 << PD3));
PORTD |= (1 << PD2) | (1 << PD3);
DDRC &= ~((1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3));
PORTC |= (1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3);
MCUCR |= (1 << ISC01) | (1 << ISC00) | (1 << ISC11) | (1 << ISC10);
GICR |= (1 << INT0) | (1 << INT1);
}

void Timer0_Millis_Init(void) {
TCCR0 = (1 << CS01) | (1 << CS00);
TIMSK |= (1 << TOIE0);
}

// ===== Interrupt handlers =====
ISR(TIMER0_OVF_vect) {
sys_time_ms += 2;
DIGIT_PORT &= ~((1 << D1_PIN) | (1 << D2_PIN) | (1 << D3_PIN));
HC595_ShiftOut(led_buffer[current_digit]);
if (current_digit == 0) {
DIGIT_PORT |= (1 << D1_PIN);
}
else if (current_digit == 1) {
DIGIT_PORT |= (1 << D2_PIN);
}
else if (current_digit == 2) {
DIGIT_PORT |= (1 << D3_PIN);
}
current_digit++;
if (current_digit > 2) current_digit = 0;
}

// Ngu encoder A
ISR(INT1_vect) {
if (PINC & (1 << PC1)) {
encA_ticks++;
} else {
encA_ticks--;
}
}

// Ngu encoder B
ISR(INT0_vect) {
if (PINC & (1 << PC0)) {
encB_ticks++;
} else {
encB_ticks--;
}
}
// ===== Main =====
static void UART_SendFloatRaw(float f, uint8_t* checksum) {
uint8_t* p = (uint8_t*)&f;
for (uint8_t i = 0; i < sizeof(float); i++) {
UART_Transmit(p[i]);
*checksum ^= p[i];
}
}
void SendStatusToESP32(void) {
uint8_t chk = 0;

text

// �p ki?u con tr? ?? l?y t?ng byte c?a bi?n float
uint8_t px = (uint8_t)&x;
uint8_t py = (uint8_t)&y;
uint8_t ptheta = (uint8_t)θ

// 1. G?i Header b�o hi?u g�i tin Status
UART_Transmit(0xCC);
UART_Transmit(0x33);

// 2. G?i 4 bytes c?a t?a ?? X
for(uint8_t i = 0; i < 4; i++) {
UART_Transmit(px[i]);
chk ^= px[i];
}

// 3. G?i 4 bytes c?a t?a ?? Y
for(uint8_t i = 0; i < 4; i++) {
UART_Transmit(py[i]);
chk ^= py[i];
}

// 4. G?i 4 bytes c?a g�c Theta
for(uint8_t i = 0; i < 4; i++) {
UART_Transmit(ptheta[i]);
chk ^= ptheta[i];
}
// 2. G?i payload (x, y, theta) v tnh checksum
UART_SendFloatRaw(x, &chk);
UART_SendFloatRaw(y, &chk);
UART_SendFloatRaw(theta, &chk);

// 5. G?i byte Checksum ch?t h?
// 3. G?i byte Checksum ch?t h?
UART_Transmit(chk);
}
int main(void) {
UART_Init();
Motor_Init();
Hardware_Init();
Led7_Init();
Timer0_Millis_Init();

text

sei();
_delay_ms(100);
Led7_SetFloat(max_speed, 2);
_delay_ms(1000);
uint32_t last_print_time = 0;
uint32_t last_control_time = 0;
uint32_t last_button_time = 0;
while (1) {
if (num_points == 0 || finished == 1) {
if (sys_time_ms - last_button_time >= 200) {
if (!(PINC & (1 << PC2))) {
if (speed_cm < 30) speed_cm++;
max_speed = speed_cm / 100.0f;
Led7_SetFloat(max_speed, 2);
last_button_time = sys_time_ms;
}
else if (!(PINC & (1 << PC3))) {
if (speed_cm > 8) speed_cm--;
max_speed = speed_cm / 100.0f;
Led7_SetFloat(max_speed, 2);
last_button_time = sys_time_ms;
}
}
}
if (sys_time_ms - last_control_time >= 50) {
last_control_time = sys_time_ms;
controlLoop();
}
if (sys_time_ms - last_print_time >= 100) {
last_print_time = sys_time_ms;
SendStatusToESP32();
}
if (UART_Available()) {
uint8_t b = UART_Receive();

text
	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 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 |= ((uint16_t)b << 8);
		calculated_checksum ^= b;
		if (num_points == 0) {
			state = WAIT_HEADER_1;
		} else if (num_points > MAX_POINTS) {
			state = WAIT_HEADER_1;
		} else {
			payload_bytes_to_read = num_points * 8;
			payload_index = 0;
			parse_point_idx = 0;
			parse_byte_idx = 0;
			state = READ_PAYLOAD;
		}
		break;
		
		case READ_PAYLOAD: {
			uint8_t *ptr;
			if (parse_byte_idx < 4)
			ptr = (uint8_t*)&path[parse_point_idx].x;
			else
			ptr = (uint8_t*)&path[parse_point_idx].y;

			ptr[parse_byte_idx & 3] = b;
			calculated_checksum ^= b;

			parse_byte_idx++;
			if (parse_byte_idx >= 8) {
				parse_byte_idx = 0;
				parse_point_idx++;
			}

			payload_index++;  
			if (payload_index >= payload_bytes_to_read)
			state = READ_CHECKSUM;
			break;
		}
		
		case READ_CHECKSUM:
		if (calculated_checksum == b) {
			currentTarget = 0;
			finished = 0;
			x = path[0].x;
			y = path[0].y;
			resetPID(&prevErrL, &intL, &pwml1);
			resetPID(&prevErrR, &intR, &pwmr1);
		}
		state = WAIT_HEADER_1;
		break;
	}
}

}
}
vấn đề uart: hiện tại esp32 đang gửi dữ liệu cho atmega bằng cách gửi nguyên mảng path (kích thước lớn)cho nó. Bây giờ tôi muốn ngoài gửi path, còn một chế độ khác là gửi cặp bộ : 2 số uint16, 1 số uint 8 liên tục để điều khiển pwm trực tiếp.
code esp32 hiện tại:
#include <Arduino.h>
#include <vector>
#include <Wire.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#define LED_PIN 8
// --- CẤU HÌNH UART CHO AVR ---
#define AVR_RX_PIN 20
#define AVR_TX_PIN 21
HardwareSerial SerialAVR(1); // Sử dụng bộ UART1

struct Point { float x; float y; };
std::vector<Point> path;

unsigned long lastTime = 0;
float vL = 0;
float vR = 0;
float offsetx = 0;
float offsety = 0;

// Các biến lưu vị trí thực tế do ATmega báo cáo
float current_x = 0;
float current_y = 0;
float current_theta = 0;

bool isPathReady = false;

const char* ssid = "P203";
const char* password = "203withlove";
const char* udpAddress = "192.168.80.39"; // IP của máy tính Python
const int udpPort = 12345;
WiFiUDP udp;

struct PacketHeader {
uint32_t timestamp;
float x;
float y;
float theta;
uint16_t num_points;
};

#pragma pack(push,1)
struct StatusPacket {
uint8_t header1;
uint8_t header2;
uint32_t timestamp;
float x;
float y;
float theta;
uint16_t num_points;
};
#pragma pack(pop)

// --- BIẾN STATE MACHINE NHẬN TỌA ĐỘ TỪ ATMEGA ---
enum RX_STATUS_STATE {
WAIT_START_1,
WAIT_START_2,
READ_PAYLOAD,
READ_CHECKSUM
};
RX_STATUS_STATE rxStatusState = WAIT_START_1;
uint8_t statusBuffer[12]; // Chứa 3 biến float: x, y, theta (3 * 4 = 12 bytes)
uint8_t statusIndex = 0;
uint8_t statusChecksum = 0;

// ================= HÀM TRUYỀN PATH SANG ATMEGA =================
void sendPathToAVR() {
if (path.empty()) return;

text
uint16_t num_points = path.size();
uint8_t chk = 0; 

// 1. Gửi Header (0xAA 0xAA)
SerialAVR.write(0xAA);
//delayMicroseconds(200);
SerialAVR.write(0xAA);
//delayMicroseconds(200);

// 2. Gửi số lượng điểm (2 bytes)
uint8_t num_L = num_points & 0xFF;
uint8_t num_H = (num_points >> 8) & 0xFF;
SerialAVR.write(num_L);
//delayMicroseconds(200);
SerialAVR.write(num_H);
//delayMicroseconds(200);
chk ^= num_L;
chk ^= num_H;

// 3. Gửi tọa độ X, Y
for (const auto& p : path) {
    uint8_t* x_bytes = (uint8_t*)&p.x;
    uint8_t* y_bytes = (uint8_t*)&p.y;

    for (int i = 0; i < 4; i++) {
        SerialAVR.write(x_bytes[i]);
        chk ^= x_bytes[i];
    }
    for (int i = 0; i < 4; i++) {
        SerialAVR.write(y_bytes[i]);
        chk ^= y_bytes[i];
    }
}

// 4. Gửi Checksum
SerialAVR.write(chk);
//delayMicroseconds(200);
Serial.println("[UART] Da truyen toan bo path sang ATmega16A.");

}

void initWiFi() {
WiFi.begin(ssid, password);
Serial.print("Connecting to WiFi");
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
Serial.println("\nConnected!");
Serial.print("IP: ");
Serial.println(WiFi.localIP());
udp.begin(12346);
digitalWrite(LED_PIN, HIGH);
delay(500);
digitalWrite(LED_PIN, LOW);
}

// ================= HÀM GỬI UDP STATUS LÊN PYTHON =================
// ================= HÀM GỬI UDP STATUS LÊN PYTHON =================
// ================= HÀM GỬI UDP STATUS LÊN PYTHON =================
void sendStatusToPC() {
StatusPacket pkt;

text
pkt.header1 = 0xAA;
pkt.header2 = 0x55;
pkt.timestamp = millis();

// TRẢ LẠI NHƯ CŨ: Không cộng thêm offset nữa vì ATmega đã tính sẵn tọa độ thực rồi!
pkt.x = current_x;
pkt.y = current_y;

pkt.theta = current_theta;
pkt.num_points = path.size();

udp.beginPacket(udpAddress, udpPort);
udp.write((uint8_t*)&pkt, sizeof(pkt));
udp.endPacket();

}

// ================= HÀM ĐỌC LỆNH UDP TỪ PYTHON =================
void checkIncomingCommand() {
int packetSize;

text
while ((packetSize = udp.parsePacket()) > 0) {
    if (packetSize == 11) {
        uint8_t buf[11];
        udp.read(buf, 11);
        
        if (buf[0] == 0xBB && buf[1] == 0x77) {
            path.clear();
            path.reserve(200); 
            vL = 0;
            vR = 0;
            Serial.println("\n[LỆNH] Dang nhan Path moi...");
        }
        else if (buf[0] == 0xBB && buf[1] == 0x66) {
            float target_x, target_y;
            memcpy(&target_x, &buf[2], 4);
            memcpy(&target_y, &buf[6], 4);
            uint8_t chk = 0;
            for(int i = 2; i < 10; i++) chk ^= buf[i];
            
            if (chk == buf[10]) {
                path.push_back({target_x, target_y});
            }
        }
        else if (buf[0] == 0xBB && buf[1] == 0x88) {
            if (!path.empty()) {
                isPathReady = true; 
                Serial.print("\n[START] Nhan thanh cong ");
                Serial.print(path.size());
                Serial.println(" diem. Bat dau chay!");
                offsetx = path[0].x; 
                offsety = path[0].y; 
                
                sendPathToAVR();
            }
        }
    } 
    else {
        udp.flush();
    }
}

}

// ================= HÀM ĐỌC STATUS UART TỪ ATMEGA =================
void checkIncomingStatusFromAVR() {
while (SerialAVR.available() > 0) {
uint8_t b = SerialAVR.read();

text
    switch (rxStatusState) {
        case WAIT_START_1:
            if (b == 0xCC) rxStatusState = WAIT_START_2;
            break;

        case WAIT_START_2:
            if (b == 0x33) {
                statusIndex = 0;
                statusChecksum = 0;
                rxStatusState = READ_PAYLOAD;
            } else {
                rxStatusState = WAIT_START_1;
            }
            break;

        case READ_PAYLOAD:
            statusBuffer[statusIndex++] = b;
            statusChecksum ^= b; // Tính mã XOR
            if (statusIndex >= 12) { 
                rxStatusState = READ_CHECKSUM;
            }
            break;

        case READ_CHECKSUM:
            if (b == statusChecksum) {
                // 1. Nhận thành công -> Giải mã mảng byte thành Float
                memcpy(&current_x, &statusBuffer[0], 4);
                memcpy(&current_y, &statusBuffer[4], 4);
                memcpy(&current_theta, &statusBuffer[8], 4);
                // 2. Lập tức bắn UDP lên Python
                //Serial.println(current_x);
                sendStatusToPC();
            }
            rxStatusState = WAIT_START_1;
            break;
    }
}

}

void setup() {
Serial.begin(115200);

// Khởi tạo UART giao tiếp với AVR

SerialAVR.begin(9600, SERIAL_8N1, AVR_RX_PIN, AVR_TX_PIN);

text
initWiFi();
lastTime = millis();

}

void loop() {
// Luôn luôn chạy 2 tiến trình lắng nghe song song:
checkIncomingCommand(); // 1. Nhặt UDP từ PC
checkIncomingStatusFromAVR(); // 2. Nhặt UART từ ATmega16A

text
sendStatusToPC();
delay(50);

}