Krang2Smpu5060

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

//////// ตัวแปรเก็บค่าแสงจากเซนเซอร์ (เหลือ S0 และ S2)

int S0, S2;

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

int cen0 = 450;

int cen2 = 450;

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

const int v_before = 67;  

const int v_after = 0;    

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

int servo1 = 10;  

int servo2 = 11;  

int servo3 = 12;  

Servo sv1;  

Servo sv2;  

Servo sv3;  

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

const int MPU_addr = 0x68;

float gyroZ_offset = 0;    

float current_angleZ = 0;  

unsigned long last_time;

float target_angle = 0;

bool is_straight_init = false;

// ฟังก์ชันรีเซ็ตค่าไจโรให้เป็น 0 ทันที

void resetGyro() {

  current_angleZ = 0;

  target_angle = 0;

  is_straight_init = false;

  last_time = millis();

}

void setup() {

  Serial.begin(9600);

  OLED.begin(SSD1306_SWITCHCAPVCC, 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.clearDisplay();

  OLED.setTextColor(WHITE, BLACK);

  OLED.setCursor(0, 0);

  OLED.setTextSize(1);

  OLED.println("KEEP ROBOT STILL!");

  OLED.println("Calibrating Gyro...");

  OLED.display();

  Wire.begin();

  Wire.beginTransmission(MPU_addr);

  Wire.write(0x6B);

  Wire.write(0);    

  Wire.endTransmission(true);

  delay(1000);

  long gyroZ_sum = 0;

  int samples = 500;

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

    Wire.beginTransmission(MPU_addr);

    Wire.write(0x43);

    Wire.endTransmission(false);

    Wire.requestFrom(MPU_addr, 6, true);

    Wire.read(); Wire.read();

    Wire.read(); Wire.read();

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

    gyroZ_sum += gz;

    delay(4);

  }

  gyroZ_offset = (float)gyroZ_sum / (float)samples;

  resetGyro(); // เซ็ตค่าเริ่มต้นเป็น 0

  int tempS0 = analogRead(0);

  int tempS2 = analogRead(2);

  cen0 = tempS0 - 200;

  cen2 = tempS2 - 200;

  OLED.clearDisplay();

  OLED.setCursor(0, 0);

  OLED.println("Calibration Done!");

  OLED.print("Offset: "); OLED.println(gyroZ_offset, 1);

  OLED.display();

  delay(2000);

  leave();

}

void updateMPU() {

  unsigned long current_time = millis();

  float dt = (current_time - last_time) / 1000.0;

  if (dt <= 0.001) {

    return;

  }

  last_time = current_time;

  Wire.beginTransmission(MPU_addr);

  Wire.write(0x43);

  Wire.endTransmission(false);

  Wire.requestFrom(MPU_addr, 6, true);

  Wire.read(); Wire.read();

  Wire.read(); Wire.read();

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

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

  if (abs(gyroZ_rate) < 1.0) {

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

  S2 = analogRead(2);  

}

void sensor() {

  while (true) {

    analogs();

    updateMPU();

    OLED.clearDisplay();              

    OLED.setTextColor(WHITE, BLACK);  

    OLED.setCursor(0, 0);            

    OLED.setTextSize(1);              

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

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

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

    OLED.display();                

    delay(100);

  }

}

void gyro_monitor() {

  while (true) {

    analogs();

    updateMPU();

    Serial.print("Gyro Offset: ");

    Serial.print(gyroZ_offset);

    Serial.print(" | Current Angle Z: ");

    Serial.println(current_angleZ);

    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)  

{

  updateMPU();

  if (!is_straight_init) {

    target_angle = 0;

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

}

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

void B(int base_speed, float gyro_gain)  

{

  resetGyro(); // รีเซ็ตไจโรเป็น 0 เมื่อเริ่มถอยหลัง

  bool is_back_init = false;

  while (1) {

    analogs();

    updateMPU();

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

      if (!is_back_init) {

        is_back_init = true;

      }

      float angle_error = current_angleZ - 0;

      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 (S0 < cen0) { run(100, -100); }

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

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

        run(0, 0);

        break;

      }

    }

  }

}

// ฟังก์ชัน PT (แทรก resetGyro() ด้านหน้า)

void PT(int base_speed, float gyro_gain)  

{

  resetGyro();

  while (1) {

    T(base_speed, gyro_gain);

    if ((S0 < cen0) || (S2 < cen2)) {

      run(-80, -80);

      delay(275);

      run(0, 0);

      delay(100);

      break;

    }

  }

}

// ฟังก์ชัน PD (แทรก resetGyro() ด้านหน้า)

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

{

  resetGyro();

  unsigned long start_time = millis();

  while (1) {

    T(base_speed, gyro_gain);

    if (millis() - start_time > times) {

      run(0, 0);

      delay(500);

      break;

    }

  }

}

// ฟังก์ชัน PG (แทรก resetGyro() ด้านหน้า)

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

{

  resetGyro();

  unsigned long start_time = millis();

  while (1) {

    T(base_speed, gyro_gain);

    if (millis() - start_time > times) {

      run(0, 0);

      delay(500);

      break;

    }

  }

}

// ฟังก์ชัน BD (แทรก resetGyro() ด้านหน้า)

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

{

  resetGyro();

  unsigned long start_time = millis();

  while (1) {

    analogs();

    updateMPU();

    float angle_error = current_angleZ - 0;

    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;

    }

  }

}

// ฟังก์ชัน PL (แทรก resetGyro() ด้านหน้า)

void PL(int base_speed, float gyro_gain)  

{

  resetGyro();

  while (1) {

    T(base_speed, gyro_gain);

    if ((S0 < cen0) || (S2 < cen2)) {

      run(0, 0);

      delay(100);

      break;

    }

  }

}

// ฟังก์ชันเลี้ยวซ้าย (รีเซ็ตไจโรเป็น 0 ก่อนเริ่มหมุน)

void TL(float target_deg, int speed) {

  resetGyro();

  while (1) {

    updateMPU();

    if (current_angleZ >= target_deg) {

      run(0, 0);

      delay(100);

      break;

    }

    run(-speed, speed);

    delay(3);

  }

}

// ฟังก์ชันเลี้ยวขวา (รีเซ็ตไจโรเป็น 0 ก่อนเริ่มหมุน)

void TR(float target_deg, int speed) {

  resetGyro();

  while (1) {

    updateMPU();

    if (current_angleZ <= -target_deg) {

      run(0, 0);

      delay(100);

      break;

    }

    run(speed, -speed);

    delay(3);

  }

}

void leave() {

  OLED.display();                  

  sv1.write(v_before);

  sv2.write(v_before);

  sv3.write(v_before);

  delay(500);

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

  B(50, 2.5);

}

void menu3() {

  ST(800);

  PT(80, 2.5);

  TL(90, 80);

  PL(80, 2.5);

  leave();

}

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

void menu5() { PG(2000, 150, 2.5); }

void menu6() { TR(90, 80);  }

void menu7() { TL(90, 80); }

void menu8() { gyro_monitor(); }

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

}

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

//////// ตัวแปรเก็บค่าแสงจากเซนเซอร์ (เหลือ S0 และ S2)

int S0, S2;

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

int cen0 = 450;

int cen2 = 450;

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

const int v_before = 67;  

const int v_after = 0;    

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

int servo1 = 10;  

int servo2 = 11;  

int servo3 = 12;  

Servo sv1;  

Servo sv2;  

Servo sv3;  

void setup() {

  Serial.begin(9600); 

  OLED.begin(SSD1306_SWITCHCAPVCC, 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);  

  // อ่านค่าแสงเซนเซอร์เตรียมไว้

  int tempS0 = analogRead(0);

  int tempS2 = analogRead(2);

  cen0 = tempS0 - 200;

  cen2 = tempS2 - 200;

  OLED.clearDisplay();

  OLED.setCursor(0, 0);

  OLED.setTextSize(1);

  OLED.println("System Ready!");

  OLED.display();

  delay(1000); 

  leave();

}

void analogs() {

  S0 = analogRead(0);  

  S2 = analogRead(2);  

}

void sensor() {

  while (true) {

    analogs();

    OLED.clearDisplay();              

    OLED.setTextColor(WHITE, BLACK);  

    OLED.setCursor(0, 0);             

    OLED.setTextSize(1);              

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

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

    OLED.display();                 

    delay(100);

  }

}

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 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)) { /* ว่างไว้ */ }

  delay(100);

}

// ------------------ ฟังก์ชันหลัก: กรอกความเร็ว มอเตอร์ซ้าย ขวา และความเร็วตอนเลี้ยว ------------------

void T(int speed_left, int speed_right, int turn_speed)  

{

  analogs();

  // วิ่งตามเส้นปกติ

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

    run(speed_left, speed_right); 

  }

  // ถ้า S0 เจอเส้น -> เลี้ยวขวาแก้ทาง

  if (S0 < cen0) { 

    run(turn_speed, -turn_speed); 

  }

  // ถ้า S2 เจอเส้น -> เลี้ยวซ้ายแก้ทาง

  if (S2 < cen2) { 

    run(-turn_speed, turn_speed); 

  }

}

// ฟังก์ชันเดินหน้าด้วย T (กำหนดเวลา + กรอกความเร็วซ้าย/ขวา/เลี้ยว)

void PD(int T_time, int spl, int spr, int sturn) {  

  unsigned long start_time = millis();

  while (millis() - start_time < T_time) {

    T(spl, spr, sturn); 

  }

  run(0, 0);

  delay(10);

}

// ฟังก์ชันวิ่งตามเส้นจนกว่าจะเจอเส้นตัด (พร้อมกำหนดเวลา timeout)

void PT(int T_time, int spl, int spr, int sturn)  

{

  unsigned long start_time = millis();

  while (millis() - start_time < T_time) {

    analogs();

    T(spl, spr, sturn); 

    if ((S0 < cen0) || (S2 < cen2)) {

      run(0, 0);

      delay(100);

      break; 

    }

  }

  run(0, 0);

  delay(100);

}

// ถอยหลังตามเวลา

void B(int base_speed, int T_time)  

{

  run(-base_speed, -base_speed);

  delay(T_time);

  run(0, 0);

  delay(50);

}

// เลี้ยวซ้ายด้วยเวลา

void TL(int T_time, int speed) {

  run(-speed, speed);

  delay(T_time);

  run(0, 0);

  delay(100);

}

// เลี้ยวขวาด้วยเวลา

void TR(int T_time, int speed) {

  run(speed, -speed);

  delay(T_time);

  run(0, 0);

  delay(100);

}

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

}

/*                         **MENU**                            */

void menu2() { 

  PD(1000, 80, 80, 100);      // เดินหน้าด้วย T เป็นเวลา 1 วินาที (เวลา, ซ้าย, ขวา, เลี้ยวแก้)

  PT(3000, 80, 80, 100);      // วิ่งตามเส้นหาเส้นตัด (เวลาสูงสุด 3 วินาที)

  TR(500, 80);                // เลี้ยวขวา 0.5 วินาที

  B(50, 1000);                // ถอยหลัง 1 วินาที

}

void menu3() { 

  PD(800, 70, 90, 100);       // เดินหน้าด้วย T แบบชดเชยล้อ 0.8 วินาที

  PT(3000, 80, 80, 100);      // วิ่งตามเส้นหาเส้นตัด

  TL(500, 80);                // เลี้ยวซ้าย 0.5 วินาที

  leave();                    // ทำงานเซอร์โว

}

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

void menu5() { PD(2000, 100, 100, 100); } 

void menu6() { TR(500, 80);  }

void menu7() { TL(500, 80); }

void menu8() { /* เมนูว่าง */ } 

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

}