PID 4 sensor

วิ่งตามเส้น PID4 SENSOR

สนามตารางPID

CODE

//////// ผนวกไลบรารี่ ////////////

#include <Wire.h>

#include <SPI.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

#include <Servo.h> 

Adafruit_SSD1306 OLED(-1);

///////////ตั้งค่าปุ่มกด///////////////////

int button =  2; /// กำหนดปุ่มกดสวิตซ์ขา 2

int ss1 = 10;  // เปิดใช้งาน s1 ที่พอร์ต 10

int ss2 = 11;  // เปิดใช้งาน s1 ที่พอร์ต 11

int ss3 = 12;  // เปิดใช้งาน s1 ที่พอร์ต 12

Servo sv1;     // ประกาศการใช้งาน sv1

Servo sv2;     // ประกาศการใช้งาน sv2

Servo sv3;     // ประกาศการใช้งาน sv3

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

////////////ตั้งค่าพอร์ตมอเตอร์////////////////////

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

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

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

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

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

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

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

/////////////////ตั้งค่าพอร์ต Analog//////////////

#define A0 600 // ค่ากลางเซนเซอร์ A0 (ซ้ายสุด)

#define A1 600// ค่ากลางเซนเซอร์ A1 (ซ้ายกลาง)

#define A2 600 // ค่ากลางเซนเซอร์ A2 (ขวากลาง)

#define A3 600// ค่ากลางเซนเซอร์ A3 (ขวาสุด)

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

int s0 ; 

int s1 ;

int s2 ;

int s3 ;

int motorSpeed;

int baseSpeed = 130;

int rightSpeed,leftSpeed;

int maxSpeed = 250;

int sum_error = 0;

// PID

int error = 0;

int pre_error = 0;

int Kp = 20;  ///20

int Kd =200;///200

int Ki = 0;

void analogs()

{

  s0 = analogRead(0);

  s1 = analogRead(1);

  s2 = analogRead(2);

  s3 = analogRead(3);

}

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

void setup() {

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

  pinMode(2, INPUT); // ตั้งค่าขา 2 เป็น INPUT

  ////////กำหนดสัญญาณทุกขาเป็น OUTPUT /////////

  pinMode(DL1, OUTPUT);

  pinMode(DL2, OUTPUT);

  pinMode(PWML, OUTPUT);

  pinMode(DR1, OUTPUT);

  pinMode(DR2, OUTPUT);

  pinMode(PWMR, OUTPUT);

  sv1.attach(ss1); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv1

  sv2.attach(ss2); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv2

  sv3.attach(ss3); // เรียกใช้งานเตรียมตอบสนองเซอร์โว sv3

  sv_set();

}

void loop() {

  int sw = digitalRead(button);     // ให้ sw อ่านค่า digital จากพอร์ต 2(button)

  int nob = analogRead(7);          // ให้ nob เทียบเท่าค่า A7

  int menu = map(nob, 0, 1023, 0, 10); // เทียบอัตราส่วนของพอร์ต A7 จาก 0-1023 เพื่อทำเป็นเมนู 0-12

  OLED.clearDisplay();              // ล้างค่าหน้าจอ

  OLED.setTextColor(WHITE, BLACK);  //สีอักษรเป็นสีขาว ,พื้นหลังดำ

  OLED.setCursor(0, 0);       // เซตตำแหน่ง 0,0

  OLED.setTextSize(2);        // เซตขนาดอักษรมีขนาดเป็น 2

  OLED.print("   ");         // วรรค

  OLED.println(menu);        // แสดงค่า Menu ที่ได้จากการ map nob ให้เหลือ 0-12

  OLED.setTextSize(1);        // เซตขนาดอักษรมีขนาดเป็น 2

  OLED.println("  KruRo");     // พิมพ์คำว่า kruro

  OLED.print("  ");                      // วรรค

  OLED.print(nob);                     // แสดงค่าที่อ่านได้จาก nob หรือ Analog7

  OLED.println(" Robot");     // พิมพ์คำว่า Robot

  OLED.display();

  if ((sw == 1) and (menu == 0))

  { sensor();

  }

  if ((sw == 1) and (menu == 1))

  {

    menu1();

  }

  if ((sw == 1) and (menu == 2))

  { 

    menu2();

  }

  if ((sw == 1) and (menu == 3))

  {

    menu3();

  }

  if ((sw == 1) and (menu == 4))

  {

   menu4();

  }

  if ((sw == 1) and (menu == 5))

  {

   menu5();

  }

  if ((sw == 1) and (menu == 6))

  {

  menu6();

  }

  if ((sw == 1) and (menu == 7))

  {

  menu7();

  }

  if ((sw == 1) and (menu == 8))

  {

  menu8();

  }

  if ((sw == 1) and (menu == 9))

  {

  menu9();

  }

  delay(100);

}

void sensor()

{

  while (true) {

      analogs();

      OLED.clearDisplay();

      OLED.setTextColor(WHITE, BLACK);  //สีอักษรเป็นสีขาว ,พื้นหลังดำ

      OLED.setCursor(0, 0);       // เซตตำแหน่ง 0,0

      OLED.setTextSize(1);        // เซตขนาดอักษรมีขนาดเป็น 1

      OLED.print("  cenValue_1 = "); OLED.println(s0);  // แสดงค่าเซนเซอร์ S0

      OLED.print("  cenValue_2= "); OLED.println(s1);  // แสดงค่าเซนเซอร์ S1

      OLED.print("  cenValue_3 = "); OLED.println(s2);  // แสดงค่าเซนเซอร์ S2

      OLED.print("  cenValue_4 = "); OLED.println(s3);  // แสดงค่าเซนเซอร์ S3

      OLED.display();

      delay(50);

    }

  }

  /////////////////////// ฟังก์ชันเซฮร์โว //////////////////////////

  void up(){sv1.write(0);delay(1000); } // เซอร์โวยก

  void down() {sv1.write(30);delay(1000); }// เซอร์โววาง

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

void L(){L0();L1();delay(50);} /////// เลี้ยวซ้าย

void L0(){while (1){analogs();run(-100,100);if(s0<A0){run(0,0);break;}}} //S0

void L1(){while (1){analogs();run(-100,100);if(s1<A1){run(0,0);break;}}} //S1

void L2(){while (1){analogs();run(-100,100);if(s2<A2){run(0,0);break;}}} //S2

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

void R(){R0();R1();R2();delay(100);} ///////// เลี้ยว

void R0(){while (1){analogs();run(100,-100);if(s3<A3){run(0,0);break;}}} //S0

void R1(){while (1){analogs();run(100,-100);if(s2<A2){run(0,0);break;}}} //S1

void R2(){while (1){analogs();run(100,-100);if(s1<A1){run(0,0);break;}}} //S2

/////////////////////////////ฟังก์ชันเซอร์โว////////////////////////////////////////

void sv_knob() {

  while(true){

  int vr = analogRead(A7);  // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 7

  int nob = map(vr, 0, 1023, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180

  OLED.clearDisplay();    // เคลียร์หน้าจอ oled

  OLED.setTextColor(WHITE, BLACK);  //สีอักษรเป็นสีขาว ,พื้นหลังดำ

  OLED.setCursor(0, 0);       // เซตตำแหน่ง 0,0

  OLED.setTextSize(2);        // เซตขนาดอักษรมีขนาดเป็น 2

  OLED.print("SV1 = ");     // พิมพ์คำว่า SV1 = 

  OLED.println(nob);     // นำค่า nob มาแสดงใน oled

  OLED.display();        // เปิดฟังก์ชันแสดงผล

  sv1.write(nob);        // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้

  delay(50);             // หน่วงเวลา 0.05 วินาที

}}

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

void sv_set()   // เซตเซอร์โว

{sv1.write(40); // ค่าองศาเซอร์โวตอนเปิดบอร์ด

  }

/////////////////////////ฟังก์ชันเส้นทาง////////////////////////////////////////

/////////////////////////ฟังก์ชันเส้นทาง////////////////////////////////////////

void P()     ///////////// ฟังก์ชันแทกเส้นเจอคู่ดำหยุด/////////////////////////////

{while (1){ 

  s0 = analogRead(0);

  s1 = analogRead(1);

  s2 = analogRead(2);

  s3 = analogRead(3);

if ((s1 > A1) and (s2 > A2)){run(150, 150);}

if (s1 < A1){run(-50, 100);}

if (s2 < A2){run(100, -50);}

//if (s0 < A0){run(-40, 75);}

//if (s3 < A3){run(75, -40);}

if (((s1 < A1) and (s2 < A2)) or ((s1 < A1) and (s0 < A0)) or ((s2 < A2) and (s3 < A3)))

    {run(100, 100);delay(180);run(0, 0);delay(100);break;}}}

void P(int t)     ///////////// ฟังก์ชันแทกเส้น/////////////////////////////

{ int j = 0;

  while (1){ 

  s0 = analogRead(0);

  s1 = analogRead(1);

  s2 = analogRead(2);

  s3 = analogRead(3);

if ((s1 > A1) and (s2 > A2)){run(75, 75);}

if (s1 < A1){run(-30, 75);}

if (s2 < A2){run(75, -30);}

//if (s0 < A0){run(-40, 75);}

//if (s3 < A3){run(75, -40);}

if (((s1 < A1) and (s2 < A2)) or ((s1 < A1) and (s0 < A0)) or ((s2 < A2) and (s3 < A3))){run(75, 75);}

if (j>t){run(0, 0);break;}

j++;}}

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

void b(){run(-75,-70);delay(1000);run(0,0);delay(600);} // ถอยหลัง

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

/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////

/////////////////////////////ฟังก์ชันมอเตอร์///////////////////////////////////

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

{

  if (spl > 0)

  {

    digitalWrite(DL1, LOW);

    digitalWrite(DL2, HIGH);

    analogWrite(PWML, spl);

  }

  else if (spl < 0)

  {

    digitalWrite(DL1, HIGH);

    digitalWrite(DL2, LOW);

    analogWrite(PWML, -spl);

  }

  else

  {

    digitalWrite(DL1, HIGH);

    digitalWrite(DL2, HIGH);

  }

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

  if (spr > 0)

  {

    digitalWrite(DR1, LOW);

    digitalWrite(DR2, HIGH);

    analogWrite(PWMR, spr);

  }

  else if (spr < 0)

  {

    digitalWrite(DR1, HIGH);

    digitalWrite(DR2, LOW);

    analogWrite(PWMR, -spr);

  }

  else

  {

    digitalWrite(DR1, HIGH);

    digitalWrite(DR2, HIGH);

  }

}

//////////////////////////////// เรียกใช้ code menu ////////////

void menu1()  /// code 1 ที่นี่

{

  sv_knob();

}

void menu2()   /// code 2 ที่นี่

{

  P(); 

}

void menu3()   /// code 3 ที่นี่

{

  P(2000);

}

void menu4()   /// code 4 ที่นี่

{L();

}

void menu5()   /// code 5 ที่นี่

{R();

  }

void menu6()   /// code 6 ที่นี่

{b();

}

void menu7()   /// code 7 ที่นี่

{

  trackpid(); L();

}

void menu8()   /// code 8 ที่นี่

{trackPID ();

}

void menu9()   /// code 9 ที่นี่

{

  trackpid();

}

void trackPID ()

{

  while (1){ 

  s0 = analogRead(0);

  s1 = analogRead(1);

  s2 = analogRead(2);

  s3 = analogRead(3);

  if(error<99) { PID(); }

  if(error>99) { run(100, 100);delay(180);run(0, 0);delay(100);break;}

}}

void trackpid ()

{

  while (1){ 

  s0 = analogRead(0);

  s1 = analogRead(1);

  s2 = analogRead(2);

  s3 = analogRead(3);

  if(error<99) { pid(); }

  if(error>99) { run(100, 100);delay(180);run(0, 0);delay(100);break;}

}}

void pid (){

    if (s0>A0&&s1>A1&&s2>A2&&s3>A3) { error = 0 ; }

    else if(s0>A0&&s1>A1&&s2<A2&&s3>A3){error = 3;}

    else if(s0>A0&&s1>A1&&s2>A2&&s3<A3){error = 5;}

    else if(s0<A0&&s1>A1&&s2>A2&&s3>A3){error =-5;} 

    else if(s0>A0&&s1<A1&&s2>A2&&s3>A3){error =-3;} 

    else if(s0<A0&&s1<A1&&s2<A2&&s3<A3){error =100;}

    else if(s0<A0&&s1<A1&&s2>A2&&s3>A3){error =101;} 

    else if(s0>A0&&s1>A1&&s2<A2&&s3<A3){error =102;}

   motorSpeed = Kp*error + Kd*(error - pre_error) + Ki*(sum_error);

   leftSpeed = baseSpeed + motorSpeed;

   rightSpeed = baseSpeed - motorSpeed;

   if(leftSpeed > maxSpeed) leftSpeed = maxSpeed;

   if(rightSpeed > maxSpeed) rightSpeed = maxSpeed;

   if(leftSpeed < -maxSpeed) leftSpeed = -maxSpeed;

   if(rightSpeed < -maxSpeed) rightSpeed = -maxSpeed;

   run(leftSpeed,rightSpeed); 

   pre_error = error;

   sum_error += error;          

}

void PID (){

    if (s0>A0&&s1>A1&&s2>A2&&s3>A3) { error = 0 ; }

    else if(s0>A0&&s1>A1&&s2<A2&&s3>A3){error = 3;}

    else if(s0>A0&&s1>A1&&s2>A2&&s3<A3){error = 5;}

    else if(s0<A0&&s1>A1&&s2>A2&&s3>A3){error =-5;} 

    else if(s0>A0&&s1<A1&&s2>A2&&s3>A3){error =-3;} 

    else if(s0<A0&&s1<A1&&s2>A2&&s3>A3){error =-7;} 

    else if(s0>A0&&s1>A1&&s2<A2&&s3<A3){error = 7;}

    else if(s0<A0&&s1<A1&&s2<A2&&s3<A3){error =100;}

   motorSpeed = Kp*error + Kd*(error - pre_error) + Ki*(sum_error);

   leftSpeed = baseSpeed + motorSpeed;

   rightSpeed = baseSpeed - motorSpeed;

   if(leftSpeed > maxSpeed) leftSpeed = maxSpeed;

   if(rightSpeed > maxSpeed) rightSpeed = maxSpeed;

   if(leftSpeed < -maxSpeed) leftSpeed = -maxSpeed;

   if(rightSpeed < -maxSpeed) rightSpeed = -maxSpeed;

   run(leftSpeed,rightSpeed); 

   pre_error = error;

   sum_error += error;          

}