đây là code lập trình atmega16A có sẵn hiện tại củ...

Question

đây là code lập trình atmega16A có sẵn hiện tại của tôi, nãy cải tiến thêm chức năng dò line đen, Module Neway Line Follower tích hợp 5 cảm biến hồng ngoại trả tín hiện digital , tận dụng các hàm ngắt, điều khiển motor, encoder,... sẵn có hãy hướng dẫn tôi bổ xung thêm code, không cần viết lại toàn bộ code, vẫn dữ tính năng như hiện tại, nhưng thay đổi như sau: nếu đang đi mà phát hiện ra line đen, không dùng path đã lưu nữa mà chuyển sang đi theo line đen:

#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*)&theta;

// 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();

		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++;  // v?n d�ng ?? ??m t?ng bytes
				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;
		}
	}
}

}

Answer 1

Đúng hướng là không xoá path đã nhận, mà thêm một cờ chế độ. Khi phát hiện line đen thì chốt sang MODE_LINE, từ đó controlLoop() không chạy pure-pursuit/path nữa mà chạy line-follow.

Dưới đây là các phần cần bổ sung/chỉnh cục bộ, không viết lại toàn bộ code.

1. Gắn 5 cảm biến line vào các chân còn trống

Trong code hiện tại:

PB0..PB7 đã dùng hết cho motor + 74HC595.
PD0/PD1 UART, PD2/PD3 encoder interrupt, PD4/PD5 PWM.
PC0/PC1 encoder, PC2/PC3 nút.
PA2/PA3/PA4 LED 7 đoạn.

Vì vậy nên dùng:

c
// S1 -> trai nhat, S5 -> phai nhat
#define LINE_S1 PA0
#define LINE_S2 PA1
#define LINE_S3 PA5
#define LINE_S4 PA6
#define LINE_S5 PA7

#define LINE_MASK ((1 << LINE_S1) | (1 << LINE_S2) | (1 << LINE_S3) | (1 << LINE_S4) | (1 << LINE_S5))

// Doi gia tri nay neu module cua ban bao den = 1
// Nhieu module LM393: gap den thuong OUT = 0
#define LINE_BLACK_LEVEL 0

Đấu cảm biến theo thứ tự:

text
S1  S2  S3  S4  S5
trái       giữa       phải

2. Thêm biến chế độ chạy

Đặt gần phần biến global:

c
typedef enum {
	MODE_PATH = 0,
	MODE_LINE = 1
} RunMode;

volatile RunMode runMode = MODE_PATH;

volatile uint8_t line_detect_count = 0;

#define LINE_DEBOUNCE_N 2

// Toc do va do bam line, can chinh thuc te
#define LINE_BASE_RATIO 0.65f
#define LINE_LOST_RATIO 0.35f

#define LINE_KP_W 1.20f
#define LINE_KD_W 0.05f
#define LINE_W_MAX 2.80f
#define LINE_SEARCH_W 1.60f

float linePrevErr = 0.0f;
float lineLastErr = 0.0f;

Ý nghĩa:

MODE_PATH: chạy theo path hiện tại như cũ.
MODE_LINE: bỏ path, chạy theo line đen.
LINE_DEBOUNCE_N = 2: phải thấy line 2 lần liên tiếp mới chuyển mode, tránh nhiễu.

3. Thêm hàm khởi tạo và đọc 5 cảm biến

Đặt trước controlLoop():

c
void LineSensor_Init(void) {
	DDRA &= ~LINE_MASK;   // PA0, PA1, PA5, PA6, PA7 lam input
	PORTA |= LINE_MASK;   // bat pull-up noi, tot cho module output open-collector
}

#define LINE_IS_BLACK(port_value, pin) \
	(LINE_BLACK_LEVEL ? ((port_value) & (1 << (pin))) : (!((port_value) & (1 << (pin)))))

uint8_t Line_ReadMask(void) {
	uint8_t v = PINA;
	uint8_t mask = 0;

	// bit0 = trai nhat, bit4 = phai nhat
	if (LINE_IS_BLACK(v, LINE_S1)) mask |= 0x01;
	if (LINE_IS_BLACK(v, LINE_S2)) mask |= 0x02;
	if (LINE_IS_BLACK(v, LINE_S3)) mask |= 0x04;
	if (LINE_IS_BLACK(v, LINE_S4)) mask |= 0x08;
	if (LINE_IS_BLACK(v, LINE_S5)) mask |= 0x10;

	return mask;
}

uint8_t Line_TakeoverReady(void) {
	if (Line_ReadMask() != 0) {
		if (line_detect_count < LINE_DEBOUNCE_N) {
			line_detect_count++;
		}
	} else {
		line_detect_count = 0;
	}

	return (line_detect_count >= LINE_DEBOUNCE_N);
}

Nếu chạy thử mà robot không nhận line đen, đổi:

c
#define LINE_BLACK_LEVEL 0

thành:

c
#define LINE_BLACK_LEVEL 1

4. Thêm hàm tính sai số line

Quy ước:

text
S1 S2 S3 S4 S5
+2 +1  0 -1 -2

Line nằm bên trái thì error dương, robot cần quay trái.

c
uint8_t Line_CalcError(uint8_t mask, float *err) {
	int16_t sum = 0;
	uint8_t count = 0;

	if (mask & 0x01) {
		sum += 2;
		count++;
	}
	if (mask & 0x02) {
		sum += 1;
		count++;
	}
	if (mask & 0x04) {
		sum += 0;
		count++;
	}
	if (mask & 0x08) {
		sum -= 1;
		count++;
	}
	if (mask & 0x10) {
		sum -= 2;
		count++;
	}

	if (count > 0) {
		*err = (float)sum / (float)count;
		lineLastErr = *err;
		return 1;
	}

	*err = lineLastErr;
	return 0;
}

5. Thêm hàm chuyển sang line mode

Đặt sau resetPID() để dùng lại hàm reset PID có sẵn:

c
void SwitchToLineMode(void) {
	runMode = MODE_LINE;
	line_detect_count = 0;

	linePrevErr = 0.0f;
	lineLastErr = 0.0f;

	resetPID(&prevErrL, &intL, &pwml1);
	resetPID(&prevErrR, &intR, &pwmr1);

	cli();
	prevLeft = encA_ticks;
	prevRight = encB_ticks;
	sei();

	lastTime = sys_time_ms;
}

Lưu ý: trong code bạn gửi đang dùng prevLeft, prevRight, x, y, theta, currentTarget, finished, vL, vR, lastTime nhưng chưa thấy khai báo. Nếu trong file gốc đã có thì giữ nguyên. Nếu chưa có, cần khai báo global.

6. Thêm hàm line-follow dùng lại encoder + PID motor hiện tại

Đặt sau applyMotorLimits() và trước controlLoop():

c
void lineFollowLoop(void) {
	uint32_t now = sys_time_ms;
	float dt = (now - lastTime) / 1000.0f;

	if (dt <= 0.0f) dt = 0.01f;
	if (dt > 0.1f) dt = 0.1f;

	lastTime = now;

	int32_t dl, dr;
	readEncoderDelta(&dl, &dr);

	// Van cap nhat odometry de ESP32 van nhan x, y, theta
	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;

	uint8_t mask = Line_ReadMask();

	float lineErr = 0.0f;
	uint8_t onLine = Line_CalcError(mask, &lineErr);

	float base_v = max_speed * LINE_BASE_RATIO;
	float w = 0.0f;

	if (onLine) {
		float dErr = (lineErr - linePrevErr) / dt;

		w = LINE_KP_W * lineErr + LINE_KD_W * dErr;

		linePrevErr = lineErr;
	} else {
		// Mat line tam thoi: quay ve phia lan cuoi nhin thay line
		base_v = max_speed * LINE_LOST_RATIO;

		if (lineLastErr >= 0.0f) {
			w = LINE_SEARCH_W;
		} else {
			w = -LINE_SEARCH_W;
		}
	}

	if (w > LINE_W_MAX) w = LINE_W_MAX;
	if (w < -LINE_W_MAX) w = -LINE_W_MAX;

	// w duong -> quay trai: banh phai nhanh hon banh trai
	vL = base_v - w * WHEEL_BASE / 2.0f;
	vR = base_v + w * WHEEL_BASE / 2.0f;

	if (vL > max_speed) vL = max_speed;
	if (vL < -max_speed) vL = -max_speed;
	if (vR > max_speed) vR = max_speed;
	if (vR < -max_speed) vR = -max_speed;

	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);

	// Giu dung cach dao motor nhu code path hien tai
	Motor_SetSpeed(pwmR_final, pwmL_final);
}

Nếu robot thấy line bên trái nhưng lại quay phải, sửa dòng này:

c
w = LINE_KP_W * lineErr + LINE_KD_W * dErr;

thành:

c
w = -(LINE_KP_W * lineErr + LINE_KD_W * dErr);

hoặc kiểm tra lại thứ tự đấu S1..S5.

7. Chỉnh đầu hàm `controlLoop()`

Trong controlLoop() hiện tại, thay đoạn đầu bằng cấu trúc này. Phần path phía sau giữ nguyên.

c
void controlLoop(void) {
	// Neu da bat line thi khong dung path nua
	if (runMode == MODE_LINE) {
		lineFollowLoop();
		return;
	}

	if (num_points == 0 || finished) {
		resetPID(&prevErrL, &intL, &pwml1);
		resetPID(&prevErrR, &intR, &pwmr1);
		prevLeft = 0;
		prevRight = 0;
		return;
	}

	// Dang chay path ma gap line den -> chuyen sang line-follow
	if (Line_TakeoverReady()) {
		SwitchToLineMode();
		lineFollowLoop();
		return;
	}

	// ===== Tu day tro xuong giu nguyen code path tracking cu =====

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

	// Phan con lai cua controlLoop() giu nguyen...

Tức là logic mới là:

text
Đã ở MODE_LINE?
    Có  -> chạy lineFollowLoop(), bỏ path.
    Không ->
        Chưa có path hoặc đã finished?
            Có -> giữ logic cũ.
            Không ->
                Có phát hiện line đen?
                    Có -> SwitchToLineMode(), từ nay bỏ path.
                    Không -> chạy path như cũ.

8. Gọi init cảm biến trong `main()`

Trong main() thêm:

c
LineSensor_Init();

Ví dụ:

c
int main(void) {
	UART_Init();
	Motor_Init();
	Hardware_Init();
	Led7_Init();
	LineSensor_Init();
	Timer0_Millis_Init();

	sei();

9. Cho line-follow chạy nhanh hơn path-follow

Line-follow nên chạy khoảng 20 ms/lần. Trong while(1), thay đoạn:

c
if (sys_time_ms - last_control_time >= 50) {
	last_control_time = sys_time_ms;
	controlLoop();
}

bằng:

c
uint16_t control_period = (runMode == MODE_LINE) ? 20 : 50;

if (sys_time_ms - last_control_time >= control_period) {
	last_control_time = sys_time_ms;
	controlLoop();
}

Path vẫn chạy 50 ms như cũ. Khi bắt line thì chạy 20 ms để bám line mượt hơn.

10. Reset về path mode khi nhận path mới

Trong case READ_CHECKSUM, đoạn hiện tại:

c
if (calculated_checksum == b) {
	currentTarget = 0;
	finished = 0;
	x = path[0].x;
	y = path[0].y;
	resetPID(&prevErrL, &intL, &pwml1);
	resetPID(&prevErrR, &intR, &pwmr1);
}

nên sửa thành:

c
if (calculated_checksum == b) {
	runMode = MODE_PATH;
	line_detect_count = 0;
	linePrevErr = 0.0f;
	lineLastErr = 0.0f;

	currentTarget = 0;
	finished = 0;

	x = path[0].x;
	y = path[0].y;

	resetPID(&prevErrL, &intL, &pwml1);
	resetPID(&prevErrR, &intR, &pwmr1);

	cli();
	prevLeft = encA_ticks;
	prevRight = encB_ticks;
	sei();

	lastTime = sys_time_ms;
}

Như vậy: nhận path mới thì robot lại chạy path. Nhưng trong lúc đang chạy path, cứ gặp line đen là chuyển sang line-follow và không dùng path nữa.

Thứ tự test nhanh

Nâng robot lên, mở nguồn.
Đưa line đen lần lượt dưới từng cảm biến.
Tạm thời có thể hiển thị mask để kiểm tra:

c
Led7_SetNumber(Line_ReadMask());

Giá trị đúng thường là:

text
S1 đen -> 1
S2 đen -> 2
S3 đen -> 4
S4 đen -> 8
S5 đen -> 16

Nếu toàn ngược, đổi LINE_BLACK_LEVEL.
Nếu robot lắc mạnh, giảm:

c
#define LINE_KP_W 1.20f

xuống 0.8f.

Nếu robot vào cua chậm hoặc trễ, tăng LINE_KP_W lên 1.5f hoặc 1.8f.

Điểm quan trọng nhất là không đặt line-follow trong ISR. ISR của bạn đang dùng cho encoder, UART, timer LED; line-follow nên chạy trong controlLoop() theo tick sys_time_ms, như trên là ổn và ít phá code cũ nhất.

đây là code lập trình atmega16A có sẵn hiện tại củ...

प्रश्न

विचार

उत्तर

1. Gắn 5 cảm biến line vào các chân còn trống

2. Thêm biến chế độ chạy

3. Thêm hàm khởi tạo và đọc 5 cảm biến

4. Thêm hàm tính sai số line

5. Thêm hàm chuyển sang line mode

6. Thêm hàm line-follow dùng lại encoder + PID motor hiện tại

7. Chỉnh đầu hàm `controlLoop()`

8. Gọi init cảm biến trong `main()`

9. Cho line-follow chạy nhanh hơn path-follow

10. Reset về path mode khi nhận path mới

Thứ tự test nhanh

इस प्रश्नोत्तर को साझा करें

प्रश्न

विचार

उत्तर

1. Gắn 5 cảm biến line vào các chân còn trống

2. Thêm biến chế độ chạy

3. Thêm hàm khởi tạo và đọc 5 cảm biến

4. Thêm hàm tính sai số line

5. Thêm hàm chuyển sang line mode

6. Thêm hàm line-follow dùng lại encoder + PID motor hiện tại

7. Chỉnh đầu hàm controlLoop()

8. Gọi init cảm biến trong main()

9. Cho line-follow chạy nhanh hơn path-follow

10. Reset về path mode khi nhận path mới

Thứ tự test nhanh

इस प्रश्नोत्तर को साझा करें

7. Chỉnh đầu hàm `controlLoop()`

8. Gọi init cảm biến trong `main()`