ตัวอย่าง Source Code ของ Arm Robot Big Gripper (Servo Motor MG946R 7 ตัว) 6 axis

// Arm Robot Big Gripper (Servo Motor MG946R 7 ตัว) 6 axis
// www.Infinityrobotic.com
// ดู Circuit Diagram จาก www.Infinityrobotic.com

#include

// STEP 1. ประกาศตัวแปร Servo Motor

Servo myservo1;  // servo ตัวที่ 1 แกน x
Servo myservo2;  // servo ตัวที่ 2 แกน y
Servo myservo3;  // servo ตัวที่ 3 แกน y
Servo myservo4;  // servo ตัวที่ 4 แกน z
Servo myservo5;  // servo ตัวที่ 5 แกน a
Servo myservo6;  // servo ตัวที่ 6 แกน b
Servo myservo7;  // servo ตัวที่ 7 แกน c

int Axisx = 10;    // ค่าตำแหน่งเริ่มต้นของ Servo1               แกน x
int Axisy = 150;   // ค่าตำแหน่งเริ่มต้นของ Servo2 และ Servo3  แกน y
int Axisz = 180;   // ค่าตำแหน่งเริ่มต้นของ Servo4             แกน z
int Axisa = 180;   // ค่าตำแหน่งเริ่มต้นของ Servo5             แกน a
int Axisb = 95;    // ค่าตำแหน่งเริ่มต้นของ Servo6             แกน b
int Axisc = 150;   // ค่าตำแหน่งเริ่มต้นของ Servo7             แกน c

int now; // ประกาศตัวแปร Loop Swich

void setup()

{

  // STEP 2. กำหนดขาใน Arduino ที่ต้องการเป็นขาควบคุม Servo (Arduino UNO UR3)

  pinMode(3,INPUT);    // กำหนดให้ขา D3 เป็นขาสวิตซ์ควบคุม Servo
  myservo1.attach(5);  // กำหนดให้ขา D5 เป็นขาควบคุม Servo1   แกน x (ฐานตัวหมุนล่าง)
  myservo2.attach(6);  // กำหนดให้ขา D6 เป็นขาควบคุม Servo2   แกน y (Servo Motor 2 ตัว ต้องทำมุมหมุนเท่ากัน)
  myservo3.attach(7);  // กำหนดให้ขา D7 เป็นขาควบคุม Servo3   แกน y (Servo Motor 2 ตัว ต้องทำมุมหมุนเท่ากัน)
  myservo4.attach(8);  // กำหนดให้ขา D8 เป็นขาควบคุม Servo4   แกน z (Servo Motor จุดระหว่าง Robot A กับ C)
  myservo5.attach(9);  // กำหนดให้ขา D9 เป็นขาควบคุม Servo5   แกน a (Servo Motor จุดระหว่าง Robot C กับ E)
  myservo6.attach(10); // กำหนดให้ขา D10 เป็นขาควบคุม Servo6  แกน b (Servo Motor คอหมุน Gripper)
  myservo7.attach(11);  //กำหนดให้ขา D11 เป็นขาควบคุม Servo7  แกน c (Gripper)

////////////////////////////////////////////////////////////////////////////////////////////////
                             
    myservo1.write(Axisx);    // เป็นการบอกว่าแกนไหนใช้ชื่อ Servo ตัวไหน
    myservo2.write(Axisy);
    myservo3.write(Axisy);
    myservo4.write(Axisz);  
    myservo5.write(Axisa);
    myservo6.write(Axisb);
    myservo7.write(Axisc);
   
 }
void loop()
{

  // STEP 3. สั่งให้ Servo หมุนไปหนีบในตำแหน่งที่ต้องการ 

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

  // สั่งให้เริ่ม Start การทำงานเมื่อกดสวิตซ์
 
 now=digitalRead(3);
    if(now==LOW)
  {
  for(Axisx = 10; Axisx < 100; Axisx += 2)  // รอบการสั่งให้หมุนไปจาก 10 ไปจนถึง 100 องศา ทีละ 2 องศา ถ้า for(Axisx = 10; Axisx < 100; Axisx += 2) แต่ถ้า for(Axisx = 100; Axisx > 10; Axisx -= 2)

  {                                 
    myservo1.write(Axisx);
    delay(50);
  }
   
    for(Axisc = 150; Axisc >90 ; Axisc -= 2)  // รอบการสั่งให้หมุนไปจาก 150 ที่หนีบอยู่ให้อ้าออกไปจนถึง 90 องศา ทีละ 2 องศา
  { 
    myservo7.write(Axisc);

    delay(50);     // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
    }
   
   for(Axisy = 150; Axisy > 50; Axisy -= 2)  // รอบการสั่งให้หมุนไปจาก 150 ไปจนถึง 50 องศา ทีละ 2 องศา
  {                   

    myservo2.write(Axisy);

    myservo3.write(Axisy);
   delay(50);
    }
   
  for(Axisa = 180; Axisa >140 ; Axisa -= 2)  // รอบการสั่งให้หมุนไปจาก 180 ที่อ้าอยู่ให้แขน a ยื่นออกไปจนถึง 140 องศา ทีละ 2 องศา
  { 
    myservo5.write(Axisa);

    delay(50);     // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
    }
   
    for(Axisz = 180; Axisz >160 ; Axisz -= 2)  // รอบการสั่งให้หมุนไปจาก 180 ที่อ้าอยู่ใหแขน z ยื่นออกไปจนถึง 160 องศา ทีละ 2 องศา
  { 
    myservo4.write(Axisz);

    delay(50);     // หน่วงเวลา
    }
   

   
for(Axisc = 90; Axisc   { 
    myservo7.write(Axisc);

    delay(50);     // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
    }
    
              
     for(Axisa = 140; Axisa   { 
    myservo5.write(Axisa);

    delay(50);     // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
    }    
    
    
   for(Axisb = 95; Axisb >0 ; Axisb -= 2)  // รอบการสั่งให้หมุนไปจาก 95 ที่อ้าอยู่ใหแขน b ยื่นออกไปจนถึง 0 องศา ทีละ 2 องศา
  { 
    myservo6.write(Axisb);

    delay(50);     // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
    }

    
      for(Axisz = 160; Axisz   { 
    myservo4.write(Axisz);

    delay(50);     // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
    }
      

        
    
    
  for(Axisy = 50; Axisy<=150; Axisy += 2)     // รอบการสั่งให้หมุนกลับจาก 150 มาที่ 0 องศา

  {                               
    myservo2.write(Axisy);
    myservo3.write(Axisy);
    delay(50); // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
  }

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

  // สั่งให้หมุนกลับ

  for(Axisx = 100; Axisx>=10; Axisx -= 2)     // รอบการสั่งให้หมุนกลับจาก 100 มาที่ 10 องศา

  {                               

    myservo1.write(Axisx);
    delay(50); // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
  }
   for(Axisy = 150; Axisy > 50; Axisy -= 2)  // รอบการสั่งให้หมุนไปจาก 50 ไปจนถึง 150 องศา ทีละ 2 องศา
  {                   

    myservo2.write(Axisy);
    myservo3.write(Axisy);
   delay(50); // หน่วงเวลานิดหน่อย  delay(1000)= 1 วินาที
   
  }
 

 
   for(Axisa = 180; Axisa >140 ; Axisa -= 2)  // รอบการสั่งให้หมุนไปจาก 180 ที่หดอยู่ใหแขน a ยื่นไปจนถึง 140 องศา ทีละ 2 องศา
  { 
    myservo5.write(Axisa);

    delay(50);    
    }    
    
    
   for(Axisb = 0; Axisb   { 
    myservo6.write(Axisb);

    delay(50);   
    }
 
 
 
 for(Axisa = 140; Axisa>130 ; Axisa -= 2)  // รอบการสั่งให้หมุนไปจาก 140 ที่ยื่นอยู่ให้แขน a ลงไปจนถึง 130 องศา ทีละ 2 องศา
  { 
    myservo5.write(Axisa);

    delay(50);    
    }    
 

  for(Axisc = 150; Axisc >90 ; Axisc -= 2)  // รอบการสั่งให้หมุนไปจาก 150 หนีบอยู่ให้อ้าออกไปจนถึง 90 องศา ทีละ 2 องศา
  { 
    myservo7.write(Axisc);

    delay(50);   

  }

 
   for(Axisa = 130; Axisa  { 
    myservo5.write(Axisa);

    delay(50);    
    }    
    
     

  for(Axisy = 50; Axisy<=150; Axisy += 2)     // รอบการสั่งให้หมุนกลับจาก 150 มาที่ 50 องศา
  {                               
    myservo2.write(Axisy);
    myservo3.write(Axisy);
    delay(50);
  }
 
  for(Axisc = 90; Axisc<=150; Axisc += 2)     // รอบการสั่งให้หมุนกลับจาก 90 ที่อ้าอยู่มาหนีบที่ 150 องศา

  {

    myservo7.write(Axisc);
    delay(50);                       // หน่วงเวลา

  }
 
 now=HIGH;
 delay(500);
  }
 delay(100);
}

// ========================  Good Luck www.Infinityrobotic.com   ======================================
// หมายเหตุ: ถ้า Servo Motor ไม่ทำงานส่วนใหญ่เกิดจาก Servo Motor กินกระแสค่อนข้างเยอะ ต้องใช้ Supply (5-6 v) กระแสสูงๆ จ่ายไฟเลี้ยงให้บอร์ด Arduino Sensor Shield 5.0 
// ตัวอย่างใน Youtube ใช้ แบตเตอรี่ชาร์จของ Sapa 2800 mA *4 ก้อนครับ