#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_BNO08x.h>
#include <sh2_SensorValue.h>
// --- กำหนดค่า OLED ---
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// --- กำหนดค่า BNO085 ---
Adafruit_BNO08x bno08x(OLED_RESET);
sh2_SensorValue_t sensorValue;
// --- กำหนดขาอุปกรณ์ต่างๆ ---
const int PIEZO_PIN = 12;
const int BUTTON_PIN = 0; // ขาปุ่มกด (Active LOW)
// TB6612 Motor Driver Pins
const int PWMA = 13;
const int AIN1 = 15;
const int AIN2 = 2;
const int PWMB = 4;
const int BIN1 = 16;
const int BIN2 = 17;
// MCP3208 Pins (SPI)
const int SPI_SCK = 18;
const int SPI_MISO = 19;
const int SPI_MOSI = 23;
const int CS_MCP3208_FRONT = 5; // F0-F7
const int CS_MCP3208_BACK = 14; // B0-B7
// ตัวแปรเก็บค่าเซนเซอร์ IR
uint16_t frontIR[8];
uint16_t backIR[8];
// ตัวแปร Calibration แสง
uint16_t frontBlack[8], frontWhite[8];
uint16_t backWhite[8], backBlack[8];
uint16_t thresholdF[8];
uint16_t thresholdB[8];
// ตัวแปรไจโร
volatile float initialYaw = 0.0;
volatile float currentYaw = 0.0;
// Handle สำหรับ FreeRTOS Task ของ Core 0
TaskHandle_t GyroTaskHandle;
// --- Task สำหรับ Core 0 (อ่านไจโรอย่างเดียว) ---
void gyroTask(void *pvParameters) {
while (true) {
if (bno085.getSensorEvent(&sensorValue)) {
if (sensorValue.sensorId == SH2_ROTATION_VECTOR) {
float qr = sensorValue.un.rotationVector.real;
float qi = sensorValue.un.rotationVector.i;
float qj = sensorValue.un.rotationVector.j;
float qk = sensorValue.un.rotationVector.k;
float yaw = atan2(2.0 * (qr * qk + qi * qj), 1.0 - 2.0 * (qj * qj + qk * qk)) * 180.0 / PI;
currentYaw = yaw - initialYaw;
}
}
vTaskDelay(5 / portTICK_PERIOD_MS);
}
}
// ฟังก์ชันอ่านค่าจาก MCP3208
uint16_t readMCP3208(int csPin, uint8_t channel) {
digitalWrite(csPin, LOW);
SPI.transfer(0x06 | ((channel & 0x07) >> 2));
uint8_t highByte = SPI.transfer((channel & 0x03) << 6);
uint8_t lowByte = SPI.transfer(0x00);
digitalWrite(csPin, HIGH);
return ((highByte & 0x0F) << 8) | lowByte;
}
// ฟังก์ชันควบคุมมอเตอร์
void moveMotor(int speedA, int speedB) {
if (speedA >= 0) {
digitalWrite(AIN1, HIGH); digitalWrite(AIN2, LOW); analogWrite(PWMA, speedA);
} else {
digitalWrite(AIN1, LOW); digitalWrite(AIN2, HIGH); analogWrite(PWMA, -speedA);
}
if (speedB >= 0) {
digitalWrite(BIN1, HIGH); digitalWrite(BIN2, LOW); analogWrite(PWMB, speedB);
} else {
digitalWrite(BIN1, LOW); digitalWrite(BIN2, HIGH); analogWrite(PWMB, -speedB);
}
}
// ฟังก์ชันเช็คการกดปุ่มแบบรอปล่อย (Debounce)
void waitForButtonPress() {
while (digitalRead(BUTTON_PIN) == HIGH) {
delay(10);
}
delay(50);
while (digitalRead(BUTTON_PIN) == LOW) {
delay(10);
}
delay(50);
}
// --- ฟังก์ชันเดินหน้าแบบ PID จนกว่า F0-F7 จะเจอสีดำทั้งหมด ---
void FS_Forward_PID(int baseSpeed, float Kp, float Ki, float Kd) {
const int MAX_SPEED = 250;
int lastError = 0;
float integral = 0;
while (true) {
for (int i = 0; i < 8; i++) {
frontIR[i] = readMCP3208(CS_MCP3208_FRONT, i);
}
bool allBlack = true;
for (int i = 0; i < 8; i++) {
if (frontIR[i] < thresholdF[i]) {
allBlack = false;
break;
}
}
if (allBlack) {
moveMotor(0, 0);
tone(PIEZO_PIN, 2500, 300);
delay(300);
break;
}
long weightedSum = 0;
long sum = 0;
for (int i = 0; i < 8; i++) {
int val = map(frontIR[i], thresholdF[i] - 200, thresholdF[i] + 200, 0, 1000);
val = constrain(val, 0, 1000);
weightedSum += (long)val * (i - 3.5) * 10;
sum += val;
}
int error = 0;
if (sum > 0) {
error = weightedSum / sum;
} else {
error = (lastError > 0) ? 35 : -35;
}
float derivative = error - lastError;
integral += error;
integral = constrain(integral, -100, 100);
int correction = (Kp * error) + (Ki * integral) + (Kd * derivative);
lastError = error;
int speedLeft = baseSpeed + correction;
int speedRight = baseSpeed - correction;
speedLeft = constrain(speedLeft, 0, MAX_SPEED);
speedRight = constrain(speedRight, 0, MAX_SPEED);
moveMotor(speedLeft, speedRight);
delay(5);
}
}
// --- ฟังก์ชันถอยหลังแบบ PID จนกว่า B0-B7 จะเจอสีดำทั้งหมด ---
void BS_Backward_PID(int baseSpeed, float Kp, float Ki, float Kd) {
const int MAX_SPEED = 250;
int lastError = 0;
float integral = 0;
while (true) {
for (int i = 0; i < 8; i++) {
backIR[i] = readMCP3208(CS_MCP3208_BACK, i);
}
bool allBlack = true;
for (int i = 0; i < 8; i++) {
if (backIR[i] < thresholdB[i]) {
allBlack = false;
break;
}
}
if (allBlack) {
moveMotor(0, 0);
tone(PIEZO_PIN, 2500, 300);
delay(300);
break;
}
long weightedSum = 0;
long sum = 0;
for (int i = 0; i < 8; i++) {
int val = map(backIR[i], thresholdB[i] - 200, thresholdB[i] + 200, 0, 1000);
val = constrain(val, 0, 1000);
weightedSum += (long)val * (i - 3.5) * 10;
sum += val;
}
int error = 0;
if (sum > 0) {
error = weightedSum / sum;
} else {
error = (lastError > 0) ? 35 : -35;
}
float derivative = error - lastError;
integral += error;
integral = constrain(integral, -100, 100);
int correction = (Kp * error) + (Ki * integral) + (Kd * derivative);
lastError = error;
int speedLeft = -baseSpeed - correction;
int speedRight = -baseSpeed + correction;
speedLeft = constrain(speedLeft, -MAX_SPEED, 0);
speedRight = constrain(speedRight, -MAX_SPEED, 0);
moveMotor(speedLeft, speedRight);
delay(5);
}
}
// --- ฟังก์ชันเลี้ยวซ้ายด้วยไจโร ---
void TurnLeft_Gyro(int speed, float targetAngle) {
float targetYaw = currentYaw - targetAngle;
while (true) {
if (currentYaw <= targetYaw) {
moveMotor(0, 0);
tone(PIEZO_PIN, 2000, 150);
delay(200);
break;
}
moveMotor(-speed, speed);
delay(5);
}
}
// --- ฟังก์ชันเลี้ยวขวาด้วยไจโร ---
void TurnRight_Gyro(int speed, float targetAngle) {
float targetYaw = currentYaw + targetAngle;
while (true) {
if (currentYaw >= targetYaw) {
moveMotor(0, 0);
tone(PIEZO_PIN, 2000, 150);
delay(200);
break;
}
moveMotor(speed, -speed);
delay(5);
}
}
void setup() {
Serial.begin(115200);
pinMode(PIEZO_PIN, OUTPUT);
pinMode(BUTTON_PIN, INPUT_PULLUP);
pinMode(PWMA, OUTPUT); pinMode(AIN1, OUTPUT); pinMode(AIN2, OUTPUT);
pinMode(PWMB, OUTPUT); pinMode(BIN1, OUTPUT); pinMode(BIN2, OUTPUT);
pinMode(CS_MCP3208_FRONT, OUTPUT);
pinMode(CS_MCP3208_BACK, OUTPUT);
digitalWrite(CS_MCP3208_FRONT, HIGH);
digitalWrite(CS_MCP3208_BACK, HIGH);
SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI);
if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
Serial.println(F("SSD1306 allocation failed"));
while(1);
}
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
// เริ่มต้น BNO085 และเซ็ตค่ามุม 0
display.setCursor(0,0);
display.print("Init BNO085...");
display.display();
if (bno085.begin_I2C()) {
bno085.enableReport(SH2_ROTATION_VECTOR);
delay(500);
for(int i=0; i<10; i++) {
if (bno085.getSensorEvent(&sensorValue)) {
if (sensorValue.sensorId == SH2_ROTATION_VECTOR) {
float qr = sensorValue.un.rotationVector.real;
float qi = sensorValue.un.rotationVector.i;
float qj = sensorValue.un.rotationVector.j;
float qk = sensorValue.un.rotationVector.k;
initialYaw = atan2(2.0 * (qr * qk + qi * qj), 1.0 - 2.0 * (qj * qj + qk * qk)) * 180.0 / PI;
break;
}
}
delay(20);
}
}
// ปิ๊บ 1 ครั้งเมื่อเปิดเครื่องและเซ็ต 0 เสร็จ
tone(PIEZO_PIN, 2000, 150);
delay(300);
// --- Step 1 ---
display.clearDisplay();
display.setCursor(0,0);
display.println("Step 1:");
display.println("F:Black | B:White");
display.println("Press & Release Btn");
display.display();
waitForButtonPress();
for (int i = 0; i < 8; i++) {
frontBlack[i] = readMCP3208(CS_MCP3208_FRONT, i);
backWhite[i] = readMCP3208(CS_MCP3208_BACK, i);
}
tone(PIEZO_PIN, 2000, 150); // ปิ๊บ 1 ครั้งจบ Step 1
delay(300);
// --- Step 2 ---
display.clearDisplay();
display.setCursor(0,0);
display.println("Step 2:");
display.println("F:White | B:Black");
display.println("Press & Release Btn");
display.display();
waitForButtonPress();
for (int i = 0; i < 8; i++) {
frontWhite[i] = readMCP3208(CS_MCP3208_FRONT, i);
backBlack[i] = readMCP3208(CS_MCP3208_BACK, i);
thresholdF[i] = (frontBlack[i] + frontWhite[i]) / 2;
thresholdB[i] = (backWhite[i] + backBlack[i]) / 2;
}
tone(PIEZO_PIN, 2500, 200); // ปิ๊บ 1 ครั้งคำนวณเสร็จ
display.clearDisplay();
display.setCursor(0,0);
display.println("Calibration Done!");
display.display();
delay(1000);
// เปิด Task ทำงานไจโรที่ Core 0
xTaskCreatePinnedToCore(gyroTask, "GyroTask", 4096, NULL, 1, &GyroTaskHandle, 0);
}
void loop() {
// --- ตัวอย่างการเรียกใช้งานภารกิจหลักใน Core 1 ---
if (digitalRead(BUTTON_PIN) == LOW) {
delay(50);
while(digitalRead(BUTTON_PIN) == LOW);
// 1. เดินหน้าตามเส้นจนเจอเส้นดำทั้งหมด (ความเร็ว 150, Kp=1.2, Ki=0.0, Kd=0.6)
FS_Forward_PID(150, 1.2, 0.0, 0.6);
delay(500);
// 2. เลี้ยวซ้าย 90 องศา (ความเร็ว 120)
TurnLeft_Gyro(120, 90.0);
delay(500);
// 3. ถอยหลังตามเส้นจนเจอเส้นดำทั้งหมด (ความเร็ว 150, Kp=1.2, Ki=0.0, Kd=0.6)
BS_Backward_PID(150, 1.2, 0.0, 0.6);
delay(500);
}
// อัปเดตการแสดงผลหน้าจอ
display.clearDisplay();
display.setCursor(0, 0);
display.print("Yaw: "); display.println(currentYaw);
display.display();
moveMotor(0, 0);
delay(20);
}