อบรมหุ่นยนตพื้นฐาน มหาวิทยาลัยธนบุรี 20-22 มค.2567

คู่มือการสอน >>>>>>>

/////////////////ใช้อ่านได้

#include <SoftwareSerial.h>

SoftwareSerial portOne(A1, A2);

String readString;

char DATA;

int DLS=50;

 

 

int MA=4;

int MB=5;

int MC=6;

int MD=7;

 

int ME=10;

int MF=11;

int MG=8;

int MH=9;

void setup() 

{

Serial.begin(115200);

 portOne.begin(9600);

Serial.println("RUN");

pinMode(MA, OUTPUT);

pinMode(MB, OUTPUT);

pinMode(MC, OUTPUT);

pinMode(MD, OUTPUT);

 

pinMode(ME, OUTPUT);

pinMode(MF, OUTPUT);

pinMode(MG, OUTPUT);

pinMode(MH, OUTPUT);

}

void loop() 

{

 

  

if(portOne.available())

{

DATA = portOne.read();

Serial.print("DATA= ");

Serial.print(DATA);

Serial.println("");

delay(10);

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

if(DATA=='U')////เดินหน้า

{

GG();//เดินหน้า

}

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

if(DATA=='D')///ถอยหลัง

{

BK();//ถอย

}

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

if(DATA=='L')////ซ้าย

{

LL();//หมุนซ้าย

}

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

 

if(DATA=='R')//ขวา

{

RR();//หมุนขวา

}

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

if(DATA=='M')///ยิง

{

YG();

}

if(DATA=='N')///กลับ

{

SK();

}

 

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

 

 

if(DATA=='Z')///หยุด

{

ST();

}

 

 

if(DATA=='A')

{

 

}

 

 

}

 

}////////////loop

 

 

 

void GG()//เดินหน้า

{

digitalWrite(MA, LOW);

digitalWrite(MB, HIGH);//

digitalWrite(MC, LOW);

digitalWrite(MD, HIGH);

   delay(DLS);  

}

 

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

void ST()//หยุด

{

 digitalWrite(MA, LOW);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, LOW); 

digitalWrite(ME, LOW);

digitalWrite(MF, LOW);//

 digitalWrite(MG, LOW);

digitalWrite(MH, LOW);//

}

////////////////////////////////----------------------------------------------------ถอยหลัง

void BK()//ถอย

{

digitalWrite(MA, HIGH);

digitalWrite(MB, LOW);//

digitalWrite(MC, HIGH);

digitalWrite(MD, LOW);//

   delay(DLS);   

}

////////////////////////////////----------------------------------------------------หมุนขวา

void RR()//หมุนขวา

{

digitalWrite(MA, HIGH);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, HIGH);

   delay(DLS);   

}

////////////////////////////////----------------------------------------------------หมุนซ้าย

void LL()//หมุนซ้าย

{

digitalWrite(MA, LOW);

digitalWrite(MB, HIGH);//

digitalWrite(MC, HIGH);

digitalWrite(MD, LOW);

   delay(DLS);  

}

 

void SK()//

{

digitalWrite(MG, LOW);

digitalWrite(MH, HIGH);//

   delay(DLS); 

 

}

void YG()//

{

digitalWrite(MG, HIGH);

digitalWrite(MH, LOW);//

   delay(DLS); 

 

}

 

 

#include <PS2X_lib.h>                  // เรียกใช้งานไลบรารีสำหรับจอยสติ๊ก PS2

#include <Servo.h>

Servo myservo1;////ตีวยืด

Servo myservo2;////ตีวคีบ

int potpin = 0;

int pos1=90;

int pos2=90;

int DLS=50;

int MA=4;

int MB=5;

int MC=6;

int MD=7;

 

int ME=10;

int MF=11;

int MG=8;

int MH=9;

 

///////////////////////////////////////////////////////---------------------------------

#include <PS2X_lib.h>                  // เรียกใช้งานไลบรารีสำหรับจอยสติ๊ก PS2

#include <Servo.h>

Servo myservo1;////ตีวยืด

Servo myservo2;////ตีวคีบ

int potpin = 0;

int pos1=90;

int pos2=90;

int DLS=50;

int MA=4;

int MB=5;

int MC=6;

int MD=7;

 

int ME=10;

int MF=11;

int MG=8;

int MH=9;

 

///////////////////////////////////////////////////////---------------------------------

#define PS2_DAT   A1                   // 

#define PS2_CMD   A2                 // 

#define PS2_CLK   A3

#define PS2_SEL   A4                  //

PS2X ps2x;                             // ประกาศตัวแปรสำหรับจอยสติ๊ก PS2

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

int posA = 20 ;

void setup()

{

pinMode(MA, OUTPUT);

pinMode(MB, OUTPUT);

pinMode(MC, OUTPUT);

pinMode(MD, OUTPUT);

 

pinMode(ME, OUTPUT);

pinMode(MF, OUTPUT);

pinMode(MG, OUTPUT);

pinMode(MH, OUTPUT);

 

digitalWrite(MA, LOW);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, LOW);

 

 

  myservo1.attach(2); //ตัวยืด

  myservo2.attach(3);//ตัวคีบ

  myservo1.write(pos1);

  myservo2.write(pos2);

  delay(100);

  Serial.begin(57600);

  Serial.println("Connecting");        // แสดงข้อความเพื่อแจ้งว่า กำลังเชื่อมต่อกับจอยสติ๊ก

  while (true)                         // วนรอการเชื่อมต่อกับจอยสติ๊ก

  {

    int error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, false, false);

    if (error == 0)                    // กรณีที่เชื่อมต่อได้ Error = 0

    {

      Serial.println("OK");            // แสดงข้อความว่า เชื่อมต่อกับจอยสติ๊กเรียบร้อยแล้ว

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

      break;                           // ออกจาก while(true)

    }

    delay(500);                        // หน่วงเวลา 500 มิลลิวินาทีเพื่อรอการเชื่อมต่อครั้งต่อไปในกรณีที่เชื่อมต่อไม่สำเร็จ

  }

}

void loop()

{

  ps2x.read_gamepad(false, false);     // อ่านข้อมูลจาก PS2 Controller

////////////////////////////////----------------------------------------------------

  if (ps2x.Button(PSB_CIRCLE)&&pos2<150)         // ถ้าปุ่มวงกลมถูกกด

  {

Serial.println("O");Serial.println(pos2);

SK();

  }

////////////////////////////////----------------------------------------------------

  else if (ps2x.Button(PSB_CROSS)&&pos2>60)     // ถ้าปุ่มกากบาทถูกกด

  {

Serial.println("X");Serial.println(pos2);

 

  }

////////////////////////////////----------------------------------------------------

  else if (ps2x.Button(PSB_SQUARE))    // ถ้าปุ่มสี่เหลี่ยมถูกกด

  {

Serial.println("S"); 

  }

////////////////////////////////----------------------------------------------------

  else if (ps2x.Button(PSB_TRIANGLE))  // ถ้าปุ่มสามเหลี่ยมถูกกด

  {

Serial.println("T");

YG();

  }

  

  ////////////////////////////////----------------------------------------------------

  

  else if (ps2x.Button(PSB_L1))        // ถ้าปุ่ม L1 ถูกกด

  {

Serial.println("L1");

 

  }

  else if (ps2x.Button(PSB_L2)&&pos1<170)        // ถ้าปุ่ม L2 ถูกกด

  {

    Serial.println("L2");

      Serial.println(pos1);

YI();

  }

  else if (ps2x.Button(PSB_R1))        // ถ้าปุ่ม R1 ถูกกด

  {

    Serial.println("R1");

 

  }

  else if (ps2x.Button(PSB_R2)&&pos1>30)        // ถ้าปุ่ม R2 ถูกกด

  {

    Serial.println("R2");Serial.println(pos1);

YU();

  }

////////////////////////////////----------------------------------------------------

  else if (ps2x.Button(PSB_START))     // ถ้าปุ่ม Start ถูกกด

  {

Serial.println("Start"); 

 DLS=50;// แสดงข้อความว่า Start

  }

  else if (ps2x.Button(PSB_SELECT))    // ถ้าปุ่ม Select ถูกกด

  {

Serial.println("Select");

DLS=10;        

  }

////////////////////////////////----------------------------------------------------

  else if (ps2x.Button(PSB_PAD_UP))    // ถ้าปุ่ม Up ถูกกด

  {

     Serial.println("UP");

    GG();//เดินหน้า

  }

  

  else if (ps2x.Button(PSB_PAD_DOWN))  // ถ้าปุ่ม Down ถูกกด

  {

Serial.println("DOWN");

BK();//ถอย

  }

  else if (ps2x.Button(PSB_PAD_LEFT))  // ถ้าปุ่ม Left ถูกกด

  {

Serial.println("LEFT");

LL();

  }

  else if (ps2x.Button(PSB_PAD_RIGHT)) // ถ้าปุ่ม Right ถูกกด

  {

 Serial.println("RIGHT");

 RR();

  }

////////////////////////////////----------------------------------------------------

  else

  {

digitalWrite(MA, LOW);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, LOW);

  }

}

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

////////////////////////////////----------------------------------------------------เดินหน้า

void GG()//เดินหน้า

{

digitalWrite(MA, LOW);

digitalWrite(MB, HIGH);//

digitalWrite(MC, LOW);

digitalWrite(MD, HIGH);

   delay(DLS); 

 digitalWrite(MA, LOW);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, LOW);  

}

////////////////////////////////----------------------------------------------------ถอยหลัง

void BK()//ถอย

{

digitalWrite(MA, HIGH);

digitalWrite(MB, LOW);//

digitalWrite(MC, HIGH);

digitalWrite(MD, LOW);//

   delay(DLS); 

 digitalWrite(MA, LOW);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, LOW);  

}

////////////////////////////////----------------------------------------------------หมุนขวา

void RR()//หมุนขวา

{

digitalWrite(MA, HIGH);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, HIGH);

   delay(DLS); 

 digitalWrite(MA, LOW);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, LOW);  

}

////////////////////////////////----------------------------------------------------หมุนซ้าย

void LL()//หมุนซ้าย

{

digitalWrite(MA, LOW);

digitalWrite(MB, HIGH);//

digitalWrite(MC, HIGH);

digitalWrite(MD, LOW);

   delay(DLS); 

 digitalWrite(MA, LOW);

digitalWrite(MB, LOW);//

digitalWrite(MC, LOW);

digitalWrite(MD, LOW);  

}

 

////////////////////////////////----------------------------------------------------จบ

void KEEP()

{

 pos2--;

  myservo2.write(pos2);

  delay(20);

}

void UKEEP()

{

  pos2++;

  myservo2.write(pos2);

  delay(20);

}

void YI()///ยืดเข้า

{

  pos1++;

  myservo1.write(pos1);

  delay(10);

}

void YU()//ยืดออก

{

  pos1--;

  myservo1.write(pos1);

  delay(10);

}

 

void SK()//

{

digitalWrite(MG, LOW);

digitalWrite(MH, HIGH);//

 

   delay(DLS); 

 digitalWrite(MG, LOW);

digitalWrite(MH, LOW);//

}

void YG()//เ

{

digitalWrite(MG, HIGH);

digitalWrite(MH, LOW);//

 

   delay(DLS); 

 digitalWrite(MG, LOW);

digitalWrite(MH, LOW);//

}