LAB 8 หุ่นยนต์เดินตามเส้น

#include <SPI.h>

#include <Wire.h>

#include <Adafruit_MCP3008.h>

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 buttonPin = 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 setup()

{

  pinMode(PWML,OUTPUT);

  pinMode(IN1L,OUTPUT);

  pinMode(IN2L,OUTPUT);

  pinMode(PWMR,OUTPUT); 

  pinMode(IN1R,OUTPUT);

  pinMode(IN2R,OUTPUT);

  pinMode(buttonPin, INPUT);

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

}

void loop()

{ 

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

if (sw==1){

                 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); }  // เซนเซอขวาเจอดำ

  }}}

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

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

//

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

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

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

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

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

//

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

//

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

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

  }    

}