Krang

#include <SPI.h>

#include <Wire.h>

#include <Servo.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

Adafruit_SSD1306 OLED(-1);

/////// motor R ////////

#define DR1 7   /// กำหนดสัญญาณดิจิตอลขวาที่ 1 พอร์ต 7

#define DR2 8   /// กำหนดสัญญาณดิจิตอลขวาที่ 2 พอร์ต 8

#define PWMR 6  /// กำหนดสัญญาณ PWM ขวาพอร์ต 6

/////////////////////////////////

#define DL1 4   // กำหนดสัญญาณดิจิตอลซ้ายที่ 1 พอร์ต 4

#define DL2 9   // กำหนดสัญญาณดิจิตอลซ้ายที่ 2 พอร์ต 9

#define PWML 5  /// กำหนดสัญญาณ PWM ซ้ายพอร์ต 5

const int button = 2;

//////// ตัวแปรเก็บค่าแสงจากเซนเซอร์

int S0, S1, S2, S3, S4;

////////// ค่ากลางเซนเซอร์ (จะถูกคำนวณอัตโนมัติตอนเปิดเครื่อง) //////////

int cen0 = 450;

int cen1 = 450;

int cen2 = 450;

int cen3 = 450;

int cen4 = 450;

/////////////////////// องศา เซอร์โว ///////////////////////

const int v_before = 67;  // กำหนดค่ายกมือขึ้น servo 0

const int v_after = 0;    // กำหนดค่ายกมือลง servo 0

/////////////// ตั้งพอร์ตเซอร์โว ////////////////////////////////

int servo1 = 10;  

int servo2 = 11;  

int servo3 = 12;  

Servo sv1;  

Servo sv2;  

Servo sv3;  

/////////////////////// MPU6050 ตัวแปรและค่าคงที่ ///////////////////////

const int MPU_addr = 0x68; // I2C address ของ MPU6050

float gyroZ_offset = 0;    // ออฟเซ็ตสำหรับไจโรแกน Z

float current_angleZ = 0;  // เก็บค่ามุมสะสม

unsigned long last_time;

// ตัวแปรสำหรับควบคุมการวิ่งตรงด้วยไจโร

float target_angle = 0;

bool is_straight_init = false;

void setup() {

  OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // กำหนดแอดเดรสของพอร์ตจอเป็น 0x3C

  pinMode(DL1, OUTPUT);

  pinMode(DL2, OUTPUT);

  pinMode(PWML, OUTPUT);

  pinMode(DR1, OUTPUT);

  pinMode(DR2, OUTPUT);

  pinMode(PWMR, OUTPUT);

  pinMode(button, INPUT);

  ///////////// ตั้งพอร์ตเซอร์โว ////////////

  sv1.attach(servo1);  

  sv2.attach(servo2);  

  sv3.attach(servo3);  

  ///////////// แสดงข้อความกำลังเริ่มต้นระบบบนจอ OLED ////////////

  OLED.clearDisplay();

  OLED.setTextColor(WHITE, BLACK);

  OLED.setCursor(0, 0);

  OLED.setTextSize(1);

  OLED.println("System Initializing...");

  OLED.display();

  ///////////// ตั้งค่า MPU6050 ////////////

  Wire.begin();

  Wire.beginTransmission(MPU_addr);

  Wire.write(0x6B); // PWR_MGMT_1 register

  Wire.write(0);    // wake up MPU-6050

  Wire.endTransmission(true);

  // 1. คาริเบตไจโรอัตโนมัติ (อ่านค่า 200 ครั้งตอนนิ่งๆ หาค่า Error เฉลี่ย)

  long gyroZ_sum = 0;

  for (int i = 0; i < 200; i++) {

    Wire.beginTransmission(MPU_addr);

    Wire.write(0x43); // เริ่มที่เรจิสเตอร์ Gyro X

    Wire.endTransmission(false);

    Wire.requestFrom(MPU_addr, 6, true);

    Wire.read(); Wire.read(); // ข้าม Gyro X

    Wire.read(); Wire.read(); // ข้าม Gyro Y

    int16_t gz = Wire.read() << 8 | Wire.read(); // Gyro Z

    gyroZ_sum += gz;

    delay(3);

  }

  gyroZ_offset = (float)gyroZ_sum / 200.0;

  // 2. อ่านค่าเซนเซอร์แสง 5 ตัวตอนเปิดเครื่อง แล้วลบด้วย 200 มาเก็บเป็นค่าอ้างอิง

  int tempS0 = analogRead(0);

  int tempS1 = analogRead(1);

  int tempS2 = analogRead(2);

  int tempS3 = analogRead(3);

  int tempS4 = analogRead(6);

  cen0 = tempS0 - 200;

  cen1 = tempS1 - 200;

  cen2 = tempS2 - 200;

  cen3 = tempS3 - 200;

  cen4 = tempS4 - 200;

  // 3. แสดงผลค่าคาริเบตไจโรและค่าเซนเซอร์แสงบนจอ OLED

  OLED.clearDisplay();

  OLED.setCursor(0, 0);

  OLED.println("Calibration Done!");

  OLED.print("Gyro Offset: "); OLED.println(gyroZ_offset, 1); // แสดงค่า Offset ไจโรที่เซตไว้

  OLED.print("Cen0: "); OLED.println(cen0);

  OLED.print("Cen1: "); OLED.println(cen1);

  OLED.display();

  delay(2000); // หน่วงเวลา 2 วินาทีเพื่อให้ทันอ่านค่าบนหน้าจอ

  last_time = millis();

  leave();

}

/////////////////////// ฟังก์ชันอัปเดตค่า MPU6050 ///////////////////////

void updateMPU() {

  unsigned long current_time = millis();

  float dt = (current_time - last_time) / 1000.0; // คำนวณเวลาที่ผ่านไป (วินาที)

  last_time = current_time;

  Wire.beginTransmission(MPU_addr);

  Wire.write(0x43); 

  Wire.endTransmission(false);

  Wire.requestFrom(MPU_addr, 6, true);

  Wire.read(); Wire.read(); // Gyro X

  Wire.read(); Wire.read(); // Gyro Y

  int16_t gz = Wire.read() << 8 | Wire.read(); // Gyro Z

  float gyroZ_rate = ((float)gz - gyroZ_offset) / 131.0; 

  current_angleZ += gyroZ_rate * dt;

}

void set_servo() {

  while (1) {

    int vr = analogRead(A7);             

    int nob = map(vr, 0, 1023, 0, 180);  

    OLED.clearDisplay();                   

    OLED.setTextColor(WHITE, BLACK);     

    OLED.setCursor(0, 0);                

    OLED.setTextSize(2);                 

    OLED.println(nob);                   

    OLED.display();                      

    sv1.write(nob);                      

    sv2.write(nob);                      

    sv3.write(nob);                      

    delay(50);                           

  }

}

void analogs() {

  S0 = analogRead(0);  

  S1 = analogRead(1);  

  S2 = analogRead(2);  

  S3 = analogRead(3);  

  S4 = analogRead(6);  

}

void sensor() {

  while (true) {

    analogs();

    updateMPU(); 

    OLED.clearDisplay();              

    OLED.setTextColor(WHITE, BLACK);  

    OLED.setCursor(0, 0);             

    OLED.setTextSize(1);              

    OLED.print(" S0:"); OLED.print(S0); OLED.print(" S1:"); OLED.println(S1); 

    OLED.print(" S2:"); OLED.print(S2); OLED.print(" S3:"); OLED.println(S3); 

    OLED.print(" S4:"); OLED.println(S4);

    OLED.print("AngZ: "); OLED.println(current_angleZ); 

    OLED.display();                 

    delay(100);

  }

}

void loop() {

  int sw = digitalRead(button);        

  int nob = analogRead(7);             

  int menu = map(nob, 0, 1023, 0, 9);  

  OLED.clearDisplay();                 

  OLED.setTextColor(WHITE, BLACK);     

  OLED.setCursor(0, 0);                

  OLED.setTextSize(3);                 

  OLED.print("   ");                 

  OLED.println(menu);                  

  OLED.setTextSize(1);                 

  OLED.println(" ");

  OLED.println("  FB:OAT.KT.ROBOT");

  OLED.println(" ");

  OLED.print("  ");  

  OLED.print(nob);   

  OLED.println(" MeNu");

  OLED.display();

  int switchs = 1;

  if ((sw == switchs) and (menu == 0)) { sensor(); }

  if ((sw == switchs) and (menu == 1)) { set_servo(); }

  if ((sw == switchs) and (menu == 2)) { menu2(); }

  if ((sw == switchs) and (menu == 3)) { menu3(); }

  if ((sw == switchs) and (menu == 4)) { menu4(); }

  if ((sw == switchs) and (menu == 5)) { menu5(); }

  if ((sw == switchs) and (menu == 6)) { menu6(); }

  if ((sw == switchs) and (menu == 7)) { menu7(); }

  if ((sw == switchs) and (menu == 8)) { menu8(); }

  delay(100);

}

//////////////////////////////////////////////////////////////////

// ฟังก์ชันเดินหน้าคุมตรงด้วยไจโร

void T(int base_speed, float gyro_gain)  

{

  analogs();

  updateMPU(); 

  if ((S0 > cen0) and (S2 > cen2)) { 

    if (!is_straight_init) {

      target_angle = current_angleZ;

      is_straight_init = true;

    }

    float angle_error = current_angleZ - target_angle;

    int correction = angle_error * gyro_gain; 

    int left_speed  = base_speed - correction;

    int right_speed = base_speed + correction;

    left_speed  = constrain(left_speed, 0, 255);

    right_speed = constrain(right_speed, 0, 255);

    run(left_speed, right_speed); 

  } 

  else {

    is_straight_init = false;

    if (S0 < cen0) { run(100, -100); }

    if (S2 < cen2) { run(-100, 100); }

  }

}

// ฟังก์ชันถอยหลังคุมตรงด้วยไจโร

void B(int base_speed, float gyro_gain)  

{

  float back_target_angle = 0;

  bool is_back_init = false;

  while (1) {

    analogs();

    updateMPU(); 

    if ((S3 > cen3) and (S4 > cen4)) { 

      if (!is_back_init) {

        back_target_angle = current_angleZ;

        is_back_init = true;

      }

      float angle_error = current_angleZ - back_target_angle;

      int correction = angle_error * gyro_gain; 

      int left_speed  = -base_speed + correction;

      int right_speed = -base_speed - correction;

      left_speed  = constrain(left_speed, -255, 0);

      right_speed = constrain(right_speed, -255, 0);

      run(left_speed, right_speed); 

    } 

    else {

      is_back_init = false;

      if (S3 < cen3) { run(100, -100); }

      if (S4 < cen4) { run(-100, 100); }

      if ((S3 < cen3) and (S4 < cen4)) {

        run(0, 0);

        break;

      }

    }

  }

}

// ฟังก์ชันวิ่งหาเส้น T ด้วยไจโร

void PT(int base_speed, float gyro_gain)  

{

  while (1) {

    T(base_speed, gyro_gain); 

    if (S1 < cen1) {

      run(-80, -80);

      delay(275);

      run(0, 0);

      delay(100);

      break;

    }

  }

}

// ฟังก์ชันเดินหน้าจนหมดเวลาด้วยไจโร

void PD(int times, int base_speed, float gyro_gain)  

{

  unsigned long start_time = millis();

  while (1) {

    T(base_speed, gyro_gain); 

    if (millis() - start_time > times) {

      run(0, 0);

      delay(500);

      break;

    }

  }

}

// ฟังก์ชันถอยหลังจนหมดเวลาด้วยไจโร

void BD(int times, int base_speed, float gyro_gain)  

{

  unsigned long start_time = millis();

  float back_target_angle = 0;

  bool is_back_init = false;

  while (1) {

    analogs();

    updateMPU(); 

    if (!is_back_init) {

      back_target_angle = current_angleZ;

      is_back_init = true;

    }

    float angle_error = current_angleZ - back_target_angle;

    int correction = angle_error * gyro_gain; 

    int left_speed  = -base_speed + correction;

    int right_speed = -base_speed - correction;

    left_speed  = constrain(left_speed, -255, 0);

    right_speed = constrain(right_speed, -255, 0);

    run(left_speed, right_speed); 

    if (millis() - start_time > times) {

      run(0, 0);

      delay(500);

      break;

    }

  }

}

// ฟังก์ชันวิ่งผ่านจุดทิ้งถุงยังชีพด้วยไจโร

void PL(int base_speed, float gyro_gain)  

{

  while (1) {

    T(base_speed, gyro_gain); 

    if (S1 < cen1) {

      run(0, 0);

      delay(100);

      break;

    }

  }

}

// ฟังก์ชันเลี้ยวซ้ายด้วยไจโร (TL = Turn Left: ระบุองศา, ความเร็ว)

void TL(float target_deg, int speed) {

  updateMPU();

  float start_angle = current_angleZ;

  float target_angle = start_angle + target_deg;

  while (1) {

    updateMPU();

    if (current_angleZ >= target_angle) {

      run(0, 0);

      delay(100);

      break;

    }

    run(-speed, speed);

  }

}

// ฟังก์ชันเลี้ยวขวาด้วยไจโร (TR = Turn Right: ระบุองศา, ความเร็ว)

void TR(float target_deg, int speed) {

  updateMPU();

  float start_angle = current_angleZ;

  float target_angle = start_angle - target_deg;

  while (1) {

    updateMPU();

    if (current_angleZ <= target_angle) {

      run(0, 0);

      delay(100);

      break;

    }

    run(speed, -speed);

  }

}

void leave() {

  OLED.clearDisplay();              

  OLED.setTextColor(WHITE, BLACK);  

  OLED.setCursor(0, 0);             

  OLED.setTextSize(1);              

  OLED.println("leave out");        

  OLED.display();                   

  sv1.write(v_before);

  sv2.write(v_before);

  sv3.write(v_before);

  delay(500);

  OLED.clearDisplay();       

  OLED.println("keep int");  

  OLED.display();            

  sv1.write(v_after);

  sv2.write(v_after);

  sv3.write(v_after);

  delay(500);

}

void ST(int T_time) {  

  run(100, 100);

  delay(T_time);

  run(0, 0);

  delay(10);

}

void Fall(int T_time) {  

  run(-130, -130);

  delay(T_time);

  run(0, 0);

  delay(10);

}

void LD(int T_time) {  

  run(-130, 130);

  delay(T_time);

  run(0, 0);

  delay(10);

}

void RD(int T_time) {  

  run(130, -130);

  delay(T_time);

  run(0, 0);

  delay(10);

}

/////////////////////////////////////////////////////////////////////

/*                         **MENU**                                  */

void menu2() { 

  ST(1000); 

  PT(80, 2.5); 

  TR(90, 80); // ตัวอย่างการใช้เลี้ยวขวา 90 องศาด้วยไจโร

  B(50, 2.5); 

}

void menu3() { 

  ST(800); 

  PT(80, 2.5); 

  TL(90, 80); // ตัวอย่างการใช้เลี้ยวซ้าย 90 องศาด้วยไจโร

  PL(80, 2.5);

  leave(); 

}

void menu4() { B(50, 2.5); }

void menu5() { PD(2000, 80, 2.5); }

void menu6() { ST(1000); PT(80, 2.5); TR(90, 80); Fall(1000); }

void menu7() { ST(500); leave(); }

void menu8() { leave(); }

///////////////////////////////////////////////////////////////////////

void run(int spl, int spr)  

{

  // มอเตอร์ซ้าย

  digitalWrite(DL1, spl > 0 ? LOW : HIGH);

  digitalWrite(DL2, spl > 0 ? HIGH : LOW);

  analogWrite(PWML, abs(spl));

  // มอเตอร์ขวา

  digitalWrite(DR1, spr > 0 ? LOW : HIGH);

  digitalWrite(DR2, spr > 0 ? HIGH : LOW);

  analogWrite(PWMR, abs(spr));

}