CODE
#include <Adafruit_MCP3008.h>
Adafruit_MCP3008 adc1, adc2;
/////// motor R ////////
#define MLD1 14 //DIR1A
#define MLD2 3 //DIR2A
#define MLEN 5 //PWMA
/////// motor L ////////
#define MRD1 15 //DIR1B
#define MRD2 9 //DIR2B
#define MREN 6 //PWMB
const int buttonPin = 2;
int Sensor = A0; // ขาเซ็นเซอร์หน้าสำหรับเช็คสตาร์ท
// ตัวแปรควบคุมความเร็วและ PID
int sp = 200;
float Kp = 1.7, Ki = 0.0, Kd = 6.0;
float error = 0, power = 0;
// ตัวแปรช่วยคำนวณ PID (Delta Time & Anti-Windup)
float last_error = 0;
float integral_sum = 0;
unsigned long previous_time = 0;
// ตัวแปรเก็บค่าดิบจาก MCP3008 (16 ช่อง)
uint16_t adcValue_1, adcValue_2, adcValue_3, adcValue_4;
uint16_t adcValue_5, adcValue_6, adcValue_7, adcValue_8;
uint16_t adcValue_9, adcValue_10, adcValue_11, adcValue_12;
uint16_t adcValue_13, adcValue_14, adcValue_15, adcValue_16;
// ตารางเก็บค่า Min (ดำ) และ Max (ขาว) สำหรับ Calibration
uint16_t min_sensor_values[16];
uint16_t max_sensor_values[16];
// ==========================================
// SETUP FUNCTION
// ==========================================
void setup() {
Serial.begin(9600);
pinMode(MRD1, OUTPUT);
pinMode(MRD2, OUTPUT);
pinMode(MREN, OUTPUT);
pinMode(MLD1, OUTPUT);
pinMode(MLD2, OUTPUT);
pinMode(MLEN, OUTPUT);
pinMode(buttonPin, INPUT_PULLUP); // ปรับเป็น INPUT หรือ INPUT_PULLUP ตามฮาร์ดแวร์ปุ่ม
pinMode(Sensor, INPUT);
// กำหนดขา SPI สำหรับ MCP3008 สองตัว (cs, clock, mosi, miso)
adc1.begin(13, 11, 12, 10);
adc2.begin(13, 11, 12, 8);
// 1. ทำการคาลิเบรตเซ็นเซอร์ก่อนวิ่ง
auto_calibrate_sensors();
// 2. รอสัญญาณกดปุ่มและเช็คเซ็นเซอร์หน้าเพื่อเริ่มออกตัว
while (1) {
int sw = digitalRead(buttonPin);
if (sw == LOW) { // เปลี่ยนเป็น HIGH ถ้าวงจรปุ่มเป็นแบบ Active High
delay(300);
while (1) {
int front = analogRead(Sensor);
if (front > 100) {
delay(100);
break;
}
}
break;
}
}
}
// ==========================================
// LOOP FUNCTION
// ==========================================
void loop() {
// ตัวอย่างเรียกใช้งานฟังก์ชัน PID วิ่งจับเส้น
PID_run(1.7, 0.0, 6.0, 200, 10000); // วิ่งด้วย PID ความเร็ว 200 เป็นเวลา 10 วินาที
stop_robot();
}
// ==========================================
// SENSOR & MOTOR FUNCTIONS
// ==========================================
void readADC() {
adcValue_1 = adc1.readADC(0);
adcValue_2 = adc1.readADC(1);
adcValue_3 = adc1.readADC(2);
adcValue_4 = adc1.readADC(3);
adcValue_5 = adc1.readADC(4);
adcValue_6 = adc1.readADC(5);
adcValue_7 = adc1.readADC(6);
adcValue_8 = adc1.readADC(7);
adcValue_9 = adc2.readADC(0);
adcValue_10 = adc2.readADC(1);
adcValue_11 = adc2.readADC(2);
adcValue_12 = adc2.readADC(3);
adcValue_13 = adc2.readADC(4);
adcValue_14 = adc2.readADC(5);
adcValue_15 = adc2.readADC(6);
adcValue_16 = adc2.readADC(7);
}
void Motor(int sl, int sr) {
if (sr > 255) sr = 255;
else if (sr < -255) sr = -255;
if (sl > 255) sl = 255;
else if (sl < -255) sl = -255;
if (sr > 0) {
digitalWrite(MRD1, LOW);
digitalWrite(MRD2, HIGH);
analogWrite(MREN, sr);
} else if (sr < 0) {
digitalWrite(MRD1, HIGH);
digitalWrite(MRD2, LOW);
analogWrite(MREN, -sr);
} else {
digitalWrite(MRD1, LOW);
digitalWrite(MRD2, LOW);
}
if (sl > 0) {
digitalWrite(MLD1, LOW);
digitalWrite(MLD2, HIGH);
analogWrite(MLEN, sl);
} else if (sl < 0) {
digitalWrite(MLD1, HIGH);
digitalWrite(MLD2, LOW);
analogWrite(MLEN, -sl);
} else {
digitalWrite(MLD1, LOW);
digitalWrite(MLD2, LOW);
}
}
void move_robot(int spp) {
if (spp >= 0) {
if (spp > sp) {
Motor(sp, -(spp - sp));
} else {
Motor(sp, sp - spp);
}
} else if (spp < 0) {
if (spp < -sp) {
Motor(spp + sp, sp);
} else {
Motor(sp + spp, sp);
}
}
}
// ==========================================
// CALIBRATION FUNCTION
// ==========================================
void auto_calibrate_sensors() {
Serial.println("--- START SENSOR AUTO CALIBRATION ---");
// 1. บันทึกค่าสีขาว
while (digitalRead(buttonPin) == HIGH) {}
delay(300);
readADC();
uint16_t white_vals[16] = {
adcValue_1, adcValue_2, adcValue_3, adcValue_4,
adcValue_5, adcValue_6, adcValue_7, adcValue_8,
adcValue_9, adcValue_10, adcValue_11, adcValue_12,
adcValue_13, adcValue_14, adcValue_15, adcValue_16
};
for (int i = 0; i < 16; i++) { max_sensor_values[i] = white_vals[i]; }
delay(1000);
// 2. บันทึกค่าสีดำ
while (digitalRead(buttonPin) == HIGH) {}
delay(300);
readADC();
uint16_t black_vals[16] = {
adcValue_1, adcValue_2, adcValue_3, adcValue_4,
adcValue_5, adcValue_6, adcValue_7, adcValue_8,
adcValue_9, adcValue_10, adcValue_11, adcValue_12,
adcValue_13, adcValue_14, adcValue_15, adcValue_16
};
for (int i = 0; i < 16; i++) { min_sensor_values[i] = black_vals[i]; }
Serial.println("Calibration Completed!");
delay(1000);
}
// ==========================================
// ERROR & PID CALCULATION
// ==========================================
void calculate_error() {
long sum = 0;
long weighted_sum = 0;
int active_sensors = 0;
float weights[16] = {-7.5, -6.5, -5.5, -4.5, -3.5, -2.5, -1.5, -0.5, 0.5, 1.5, 2.5, 3.5, 4.5, 5.5, 6.5, 7.5};
uint16_t sensor_vals[16] = {
adcValue_1, adcValue_2, adcValue_3, adcValue_4,
adcValue_5, adcValue_6, adcValue_7, adcValue_8,
adcValue_9, adcValue_10, adcValue_11, adcValue_12,
adcValue_13, adcValue_14, adcValue_15, adcValue_16
};
for (int i = 0; i < 16; i++) {
// map ค่าให้อยู่ในช่วง 0 - 1000 ตามค่า Min/Max ที่คาลิเบรตไว้
long mapped_val = map(sensor_vals[i], min_sensor_values[i], max_sensor_values[i], 1000, 0);
if (mapped_val < 0) mapped_val = 0;
if (mapped_val > 1000) mapped_val = 1000;
if (mapped_val > 100) {
weighted_sum += mapped_val * weights[i];
sum += mapped_val;
active_sensors++;
}
}
if (active_sensors > 0) {
error = (float)weighted_sum / sum;
}
}
float updatePid(float current_error) {
unsigned long current_time = millis();
float dt = (current_time - previous_time) / 1000.0;
if (dt <= 0) dt = 0.001;
previous_time = current_time;
float P = current_error * Kp;
integral_sum += current_error * dt;
if (integral_sum > 500) integral_sum = 500;
else if (integral_sum < -500) integral_sum = -500;
float I = integral_sum * Ki;
float D = ((current_error - last_error) / dt) * Kd;
last_error = current_error;
return P + I + D;
}
// ==========================================
// NAVIGATION & PID RUN
// ==========================================
void PID_run(float kP, float kI, float kD, int kS, unsigned long t) {
Kp = kP; Ki = kI; Kd = kD; sp = kS;
last_error = 0;
integral_sum = 0;
previous_time = millis();
unsigned long startTime = millis();
while (1) {
unsigned long currentTime = millis();
if (currentTime - startTime > t) {
Motor(0, 0);
break;
}
readADC();
calculate_error();
power = updatePid(error);
move_robot(power);
}
}
void stop_robot() {
while (1) {
Motor(0, 0);
}
}