Motor

#define PWML  5  //   motor L

#define IN1L  4  //

#define IN2L  9  //

#define PWMR  6  //   motor R

#define IN1R  7  //

#define IN2R  8  //

#define buttonPin 2

void setup() {

pinMode(buttonPin,INPUT);

pinMode(PWML,OUTPUT);

pinMode(IN1L,OUTPUT);

pinMode(IN2L,OUTPUT);

pinMode(PWMR,OUTPUT);

pinMode(IN1R,OUTPUT);

pinMode(IN2R,OUTPUT);

}

void loop(){

int sw = digitalRead(buttonPin);  //กำหนดตัวแปร sw ให้มีค่าเท่ากับ ค่าที่อ่านได้จาก digital Pin 2  หรือ ปุ่ม OK บนบอร์ด

if (sw==1){

   run(100,100);delay(500);    //มอเตอร์ ซ้ายและขวาหมุนไปข้างหน้า ความเร็ว 100 หน่วงเวลา 500 มิลลิวินาที

    run(100,-100);delay(500);   //มอเตอร์ ซ้ายเดินหน้า มอเตอร์ขวา ถอยหลัง ความเร็ว 100 หน่วงเวลา 500 มิลลิวินาที

    run(100,100);delay(500);    //มอเตอร์ ซ้ายและขวาหมุนไปข้างหน้า ความเร็ว 100 หน่วงเวลา 500 มิลลิวินาที

    run(100,-100);delay(500);  //มอเตอร์ ซ้ายเดินหน้า มอเตอร์ขวา ถอยหลัง ความเร็ว 100 หน่วงเวลา 500 มิลลิวินาที

   run(100,100);delay(500);   //มอเตอร์ ซ้ายและขวาหมุนไปข้างหน้า ความเร็ว 100 หน่วงเวลา 500 มิลลิวินาที

   run(100,-100);delay(500);  //มอเตอร์ ซ้ายเดินหน้า มอเตอร์ขวา ถอยหลัง ความเร็ว 100 หน่วงเวลา 500 มิลลิวินาที

    run(100,100);delay(500);   //มอเตอร์ ซ้ายและขวาหมุนไปข้างหน้า ความเร็ว 100 หน่วงเวลา 500 มิลลิวินาที

    run(100,-100);delay(500);  //มอเตอร์ ซ้ายเดินหน้า มอเตอร์ขวา ถอยหลัง ความเร็ว 100 หน่วงเวลา 500 มิลลิวินาที

   run(0,0);}                 // มอเตอร์ซ้ายและขวาหยุด

}

void run(int spl, int spr)   // ประกาศฟังก์ชัน run(กำลังมอเตอร์ซ้าาย,กำลังมอเตอร์ขวา);

{

  if (spl > 0)

  {

    digitalWrite(IN1L, LOW);

    digitalWrite(IN2L, HIGH);

    analogWrite(PWML, spl);

  }

  else if (spl < 0)

  { spl= abs(spl);

    digitalWrite(IN1L, HIGH);

    digitalWrite(IN2L, LOW);

    analogWrite(PWML, spl);

  }

  else

  {

    digitalWrite(IN1L, HIGH);

    digitalWrite(IN2L, HIGH);

  }

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

  if (spr > 0)

  {

    digitalWrite(IN1R, LOW);

    digitalWrite(IN2R, HIGH);

    analogWrite(PWMR, spr);

  }

  else if (spr < 0)

  {spr= abs(spr);

    digitalWrite(IN1R, HIGH);

    digitalWrite(IN2R, LOW);

    analogWrite(PWMR, spr);

  }

  else

  {

    digitalWrite(IN1R, HIGH);

    digitalWrite(IN2R, HIGH);

  }

}

#include <Wire.h>

#include <math.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

// --- กำหนดค่าจอ OLED ---

#define SCREEN_WIDTH 128

#define SCREEN_HEIGHT 64 

#define OLED_RESET    -1 

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// --- กำหนดค่า MPU-6050 ---

const int MPU_addr = 0x68; 

int16_t axis_X, axis_Y, axis_Z;

// --- กำหนดพินมอเตอร์และปุ่มกด ---

#define PWML   5  // motor L

#define IN1L   4  

#define IN2L   9  

#define PWMR   6  // motor R

#define IN1R   7  

#define IN2R   8  

#define buttonPin 2

void setup() {

  Serial.begin(115200);

  Wire.begin();

  // 1. ตั้งค่าพินมอเตอร์และปุ่ม

  pinMode(buttonPin, INPUT);

  pinMode(PWML, OUTPUT);

  pinMode(IN1L, OUTPUT);

  pinMode(IN2L, OUTPUT);

  pinMode(PWMR, OUTPUT);

  pinMode(IN1R, OUTPUT);

  pinMode(IN2R, OUTPUT);

  // 2. ปลุก MPU-6050 จากโหมด Sleep

  Wire.beginTransmission(MPU_addr);

  Wire.write(0x6B); 

  Wire.write(0);    

  Wire.endTransmission(true);

  // 3. เริ่มต้นใช้งานจอ OLED

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {

    Serial.println(F("ไม่พบจอ OLED!"));

    while(1);

  }

  display.clearDisplay();

  display.setTextColor(SSD1306_WHITE);

  display.setTextSize(1);

  display.setCursor(10, 25);

  display.println(F("Gyro 4-Way Control"));

  display.display();

  delay(1500);

}

void loop() {

  int sw = digitalRead(buttonPin);  // อ่านค่าปุ่มกด (ใช้เป็นสวิตช์เปิด-ปิดระบบควบคุม)

  // --- ส่วนที่ 1: อ่านค่าและคำนวณมุมเอียงจาก MPU-6050 ---

  Wire.beginTransmission(MPU_addr);

  Wire.write(0x3B); 

  Wire.endTransmission(false);

  Wire.requestFrom(MPU_addr, 6, true);

  axis_X = Wire.read() << 8 | Wire.read();

  axis_Y = Wire.read() << 8 | Wire.read();

  axis_Z = Wire.read() << 8 | Wire.read();

  float accel_x = (float)axis_X / 16384.0;

  float accel_y = (float)axis_Y / 16384.0;

  float accel_z = (float)axis_Z / 16384.0;

  // คำนวณมุม Pitch (ก้ม-เงย) และ Roll (เอียงซ้าย-ขวา)

  float pitch = atan(-accel_x / sqrt(accel_y * accel_y + accel_z * accel_z)) * 180.0 / PI;

  float roll = atan(accel_y / sqrt(accel_x * accel_x + accel_z * accel_z)) * 180.0 / PI;

  // --- ส่วนที่ 2: เงื่อนไขการสั่งงาน 4 ทิศทาง (ทำงานเมื่อกดปุ่ม sw == 1) ---

  String robotState = "STOP";

  if (sw == 1) {

    // เช็คการเอียงหน้า-หลัง (Pitch)

    if (pitch > 15.0) {

      run(120, 120);    // เดินหน้า

      robotState = "FORWARD";

    } 

    else if (pitch < -15.0) {

      run(-120, -120);  // ถอยหลัง

      robotState = "BACKWARD";

    } 

    // เช็คการเอียงซ้าย-ขวา (Roll) ถ้าไม่ได้ก้ม/เงย

    else if (roll > 15.0) {

      run(120, -120);   // เลี้ยวขวา (ซ้ายเดินหน้า, ขวาถอยหลัง)

      robotState = "TURN RIGHT";

    } 

    else if (roll < -15.0) {

      run(-120, 120);   // เลี้ยวซ้าย (ซ้ายถอยหลัง, ขวาเดินหน้า)

      robotState = "TURN LEFT";

    } 

    else {

      run(0, 0);        // ตั้งตรง: หยุด

      robotState = "IDLE";

    }

  } else {

    run(0, 0);          // ถ้าไม่ได้กดปุ่ม ให้หยุดมอเตอร์

    robotState = "WAIT BTN";

  }

  // --- ส่วนที่ 3: แสดงผลข้อมูลลงบนจอ OLED ---

  display.clearDisplay();

  display.setTextSize(1);

  display.setCursor(0, 0);

  display.print(F("Pitch: ")); display.print(pitch, 1);

  display.setCursor(0, 12);

  display.print(F("Roll:  ")); display.print(roll, 1);

  display.setCursor(0, 25);

  display.print(F("---------------------"));

  display.setCursor(0, 36);

  display.print(F("Action:"));

  display.setTextSize(2);

  display.setCursor(0, 48);

  display.print(robotState);

  display.display();

  delay(100);

}

// --- ฟังก์ชันควบคุมมอเตอร์ ---

void run(int spl, int spr) {

  if (spl > 0) {

    digitalWrite(IN1L, LOW);

    digitalWrite(IN2L, HIGH);

    analogWrite(PWML, spl);

  } else if (spl < 0) {

    spl = abs(spl);

    digitalWrite(IN1L, HIGH);

    digitalWrite(IN2L, LOW);

    analogWrite(PWML, spl);

  } else {

    digitalWrite(IN1L, HIGH);

    digitalWrite(IN2L, HIGH);

    analogWrite(PWML, 0);

  }

  if (spr > 0) {

    digitalWrite(IN1R, LOW);

    digitalWrite(IN2R, HIGH);

    analogWrite(PWMR, spr);

  } else if (spr < 0) {

    spr = abs(spr);

    digitalWrite(IN1R, HIGH);

    digitalWrite(IN2R, LOW);

    analogWrite(PWMR, spr);

  } else {

    digitalWrite(IN1R, HIGH);

    digitalWrite(IN2R, HIGH);

    analogWrite(PWMR, 0);

  }

}