LAB 15 ภารกิจกลาง

#include <SPI.h>

#include <Wire.h>

#include <Servo.h>

#include <Adafruit_MCP3008.h>

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

Adafruit_SSD1306 OLED(-1);

Adafruit_MCP3008 adc;

#define PWML  3  //   motor L

#define IN1L  4  //  

#define IN2L  6  //

#define PWMR  5  //   motor R

#define IN1R  15  //  

#define IN2R  14  //

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

const int button = 2;

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

int S1 = 0;   

int S2 = 0;  

int S3 = 0;   

int S4 = 0;  

//////////ค่ากลางเซนเซอร์  เอาไว้เปนียบเทียบค่าแสง//////////

int CEN1 = 550;

int CEN2 = 350;

int CEN3 = 550;

int CEN4 = 550;  

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

{while (1){ 

    S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

if ((S2 > CEN2) and (S3 > CEN3)){run(100, 100);}

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

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

//if (S1 < CEN1){run(-40, 75);}

//if (S4 < CEN4){run(75, -40);}

if (((S2 < CEN2) and (S3 < CEN3)) or ((S2 < CEN2) and (S1 < CEN1)) or ((S3 < CEN3) and (S4 < CEN4)))

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

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

{ int j = 0;

  while (1){ 

    S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

if ((S2 > CEN2) and (S3 > CEN3)){run(75, 75);}

if (S2 < CEN2){run(-30, 75);}

if (S3 < CEN3){run(75, -30);}

//if (S1 < CEN1){run(-40, 75);}

//if (S4 < CEN4){run(75, -40);}

if (((S2 < CEN2) and (S3 < CEN3)) or ((S2 < CEN2) and (S1 < CEN1)) or ((S3 < CEN3) and (S4 < CEN4))){run(75, 75);}

if (j>t){run(0, 0);tone(3,900,500);break;}

j++;}}

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

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

void R(){R3();R2();delay(100);tone(3,1000,50);} ///////// เลี้ยว

void L0(){while (1){  S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

       run(-100,100);if(S1<CEN1){run(0,0);break;}}} //S0

void L1(){while (1){  S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

       run(-100,100);if(S2<CEN2){run(0,0);break;}}} //S1

void L2(){while (1){  S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3R

       run(-100,100);if(S3<CEN3){run(0,0);break;}}} //S2

void L3(){while (1){  S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

       run(-100,100);if(S4<CEN4){run(0,0);break;}}} //S3

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

void R0(){while (1){  S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

       run(100,-100);if(S1<CEN1){run(0,0);break;}}} //S0

void R1(){while (1){  S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

       run(100,-100);if(S2<CEN2){run(0,0);break;}}} //S1

void R2(){while (1){  S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

       run(100,-100);if(S3<CEN3){run(0,0);break;}}} //S2

void R3(){while (1){  S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

       run(100,-100);if(S4<CEN4){run(0,0);break;}}} //S3

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

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

const int v_up = 90 ;     // <---- กรอกองศาเซอร์โวตรงนี้

const int v_down = 20;    // <----

const int v_keep = 85;    // <----

const int v_paste = 30;   // <----

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

int servo1 = 9;  // เปิดใช้งาน s1 ที่พอร์ต 10 zackrobot & 7 kruroRobot

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

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

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

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

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

void setup()

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

  pinMode(PWML,OUTPUT);

  pinMode(IN1L,OUTPUT);

  pinMode(IN2L,OUTPUT);

  pinMode(PWMR,OUTPUT); 

  pinMode(IN1R,OUTPUT);

  pinMode(IN2R,OUTPUT);

  pinMode(button, INPUT);

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

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

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

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

 adc.begin(13, 11, 12, 10);//เริ่มจากใช้ adc จาก library

}

void set_servo(){

  while(1){

 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.println(nob);     // นำค่า nob มาแสดงใน oled

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

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

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

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

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

}

}

void analogADC()

{

  int S1 = adc.readADC(0);   // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch0

  int S2 = adc.readADC(1);   // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch1

  int S3 = adc.readADC(2);   // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch2

  int S4 = adc.readADC(3);   // กำหนดตัวแปรเพื่อเก็บค่า ADC Ch3

}

void sensor()

{

  while (true) {

     S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

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

  OLED.setTextColor(WHITE,BLACK);   //สีอักษรเป็นสีขาว ,พื้นหลังดำ  จอที่ใช้ไม่สามารถเปลี่ยนสีได้

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

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

  OLED.print(" S1 : ");              //  แสดงข้อความ S1 :

  OLED.println(S1);      //  แสดงค่า S1 

  OLED.print(" S2 : ");          //  แสดงข้อความ S2 :

   OLED.println(S2);     //  แสดงค่า S2

  OLED.print(" S3 : ");          //  แสดงข้อความ S3 :

   OLED.println(S3);     //  แสดงค่า S3

  OLED.print(" S4 : ");          //  แสดงข้อความ S4 :

   OLED.println(S4);      //  แสดงค่า S4

  OLED.display();  // สั่งให้ จอแสดงผล

delay(100);

  }

}

void loop()

{ 

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

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

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

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

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

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

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

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

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

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

  OLED.println(" ");

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

  OLED.println(" ");

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

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

  OLED.println(" MeNu");

  OLED.display();

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

  { 

    sensor();

  }

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

  {

   set_servo();

  }

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

  }

  delay(100);

}

/*while (1) {                    // ให้วนลูบ

       S1 = adc.readADC(0);   // อ่านค่า Analog จาก MCP3008 ขาที่ 0

       S2 = adc.readADC(1);   // อ่านค่า Analog จาก MCP3008 ขาที่ 1

       S3 = adc.readADC(2);   // อ่านค่า Analog จาก MCP3008 ขาที่ 2

       S4 = adc.readADC(3);   // อ่านค่า Analog จาก MCP3008 ขาที่ 3

    if ((S2>cen2) and (S3>cen3)){ Motor(100,100);}  //เซนเซอร์ 2 และ 3 เจอขาว ให้มอเตอร์ซ้ายและขวาเดินหน้าด้วยความเร็ว 100

    if(S2<cen2)    {Motor(0,100); }   // เซนเซอซ้ายเจอดำ

    if(S3<cen3)     {Motor(100,0); }  // เซนเซอขวาเจอดำ

    if(S1<cen1)    {Motor(-100,100); }   // เซนเซอซ้ายเจอดำ   

    if(S4<cen4)     {Motor(100,-100); }  // เซนเซอขวาเจอดำ

  */

void up() {    /*เก็บกล่อง/กระป๋อง*/

sv2.write(v_paste);delay(500); 

sv1.write(v_down);delay(500); 

}

void down() { /*วางกล่อง/กระป๋อง*/

sv1.write(v_down);delay(500);

sv2.write(v_paste);delay(500);

sv1.write(v_up);delay(500);

}

/*                             ***MENU***                             */

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

{

 P();

}

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

{

 L();

}

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

{

 R();

}

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

{ 

 b();

}

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

{ 

}

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

{

}

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

{

}

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

//  ฟังก์ชั่นสั่งงานมอเตอร์  Motor(ความเร็วซ้าย,ความเร็วขวา);

//

//  Motor(100,100);     //  เดินหน้าทั้งสองล้อ

//  Motor(-100,-100);   //  ถอยหลังทั้งสองล้อ

//  Motor(-100,100);    //  ล้อซ้ายถอย ล้อขวาเดินหน้า

//  Motor(100,-100);    //  ล้อซ้ายเดินหน้า ล้อขวาถอย

//  Motor(0,0);         //  หยุดทั้ง สองล้อ

//

//  สามารถกำหนดความเร็วได้  -255 ถึง 255

//

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

void run(int sl,int sr){     

  if(sr>255)

    sr = 255;

  else if(sr<-255)

    sr = -255;

  if(sl>255)

    sl = 255;

  else if(sl<-255)

    sl = -255;

  if(sl>0){

      digitalWrite(IN1L,HIGH);

      digitalWrite(IN2L,LOW);

      analogWrite(PWML,sl);

  }else if(sl<0){    

      digitalWrite(IN1L,LOW);

      digitalWrite(IN2L,HIGH);

      analogWrite(PWML,-sl);

  }else{        

      digitalWrite(IN1L,HIGH);

      digitalWrite(IN2L,HIGH);

  }  

  if(sr>0){

      digitalWrite(IN1R,HIGH);

      digitalWrite(IN2R,LOW);

      analogWrite(PWMR,sr);

  }else if(sr<0){    

      digitalWrite(IN1R,LOW);

      digitalWrite(IN2R,HIGH);

      analogWrite(PWMR,-sr);

  }else{        

      digitalWrite(IN1R,HIGH);

      digitalWrite(IN2R,HIGH);

  }    

}