ทดสอบโค้ดควบคุมแขนกล Robot Armด้วย Arduino

เดียวว่างๆ จะไปช่วยซื้ออาหารปลานะครับ อิอิ

สำเร็จแล้วได้งาน ตอนนี้ได้งานให้ไปทำหน้าที่ขายอาหารปลา ชวนเด็กมาใให้อาหารปลา ให้เด็กๆได้ศึกกษากัน และทดสอบความทนทานของเซอร์โว
ฟังก์ชั่นเพิ่มตามนี้
1.สอนให้จำตำแหน่งทำงานซ้ำๆ
2’.บันทึกตำแหน่งทำงานใน EEPROM
3.นำข้อมูลจากEEPROMมาใช้เมื่อทำงานใหม่
4.สั่งให้ทำงานแบบรอบเดียวหยุด
5.สั่งให้ทำงานซ้ำหลายรอบได้โดยไม่หยุด
6.บันทึกทับข้อมูลเก่าใน eeprom
มีฟังก์ืชันอะไรที่น่าสนใจเพิ่มเติมแนะนำด้วย

ตัวที่2 จะใช้ DC servo moter + encoder ใช้จับแผงโซล่าเซล ให้เคลื่อนที่ไปตามดวงอาทิตย์ ดูมอเตอร์มาหลายรุ่นแต่ยังเลือกไม่ถูก ทั้ง ac และ dc
สเปคเบื้องต้น ยังเลือกไม่ถูกใจ ราคาแพง
1.ใช้ไฟ DC ปลอดภัย
2.ขนาดมอเตอร์ใช้ตัวเล็กราคาไม่แพงเดี๋ยวนี้แผงโซล่าเซลมีแบบบางน้ำหนนักเบามาก ที่่ประมาณใว้ยกของได้ 1-3 กิโล
3.ใช้บอร์ดarduino เป็น คอนโทลเลอร์
ขอข้อมูลแนะนำครับ

โค้ดล่าสุดครับ

  /*  Arduino Robotic Arm with Grab
 *  More info: http://www.ardumotive.com/
 *  arduino pro mini 328 broad 
 *  Dev: Michalis Vasilakis Data: 12/12/2016 Ver: 1.1 */

#include <EEPROM.h>
#include <Servo.h>

//Create servo objects to control servo motors
Servo up_down;  //servo3
Servo forward_backward;//servo6
Servo rotate;//servo2
Servo grab;//servo1

//เพิ่มเซอร์โว4,5
  Servo up_down4;//servo4
  Servo up_down5;//servo5

//Constants
const int startBT = 7;
const int teachBT = 8;
const int autoBT = 11;
//2562-09-06 const int buzzer = 9;
const int buzzer = 10;

const int potForwardBackward = A3;//servo6
const int potUpDown = A2;//servo3
const int potRotate = A1;//server2
const int potGrab = A0;//เซอร์โว1

//เพิ่มเซอร์โว2ตัว
  const int potUpDown4 = A4;//เซอร์โว4
  const int potUpDown5 = A5;//เซอร์โว5

//Max and Min values for servos ! Change them to meet your setup !
const int minGrab=180;
const int maxGrab=126;
const int minRotate=0;
const int maxRotate=100;
const int minUpDown=5;
const int maxUpDown=100;
const int minForwardBackward=160;

//const int maxForwardBackward=80;
  const int maxForwardBackward=20;//25620911ทำให้องศาการหมุนเซอร์โวเพิ่มจาก90เป็น180องศาโดยประมาณ

//เพิ่มเซอร์โว4,5
  const int minUpDown4=5;
  const int maxUpDown4=100;
  const int minUpDown5=5;
  const int maxUpDown5=100;
  
//-------------------------------------//
//Variables ประกาศตัวแปร
int readAuto,readUpDown,readForwardBackward,readRotate,readGrab,readTeach,readStart,readUpDown4,readUpDown5;
int teachUpDown[100],teachForwardBackward[100],teachRotate[100],teachGrab[100],teachUpDown4[100],teachUpDown5[100];

boolean started =false;
int index = 1;
int count = 1;
int eeppos = 0;
int indexend ;
int saveindex=1;
int saveeeppos=0;
int runauto = 1 ;
//Change this to fo faster!
  int stepSpeed = 10; //25620910 20 to 50  ช้าลง
  int stepSpeed2 = 10; //25620920 
  int stepSpeed3 = 30; //25620920
   
void setup() {
  //Attach Servo motors
  forward_backward.attach(3);
  up_down.attach(4);
  rotate.attach(5); //portokali xontro
  grab.attach(6);
   
  //เพิ่มเซอร์โว4,5
    up_down4.attach(2);//servo4
    up_down5.attach(9);//servo5
   
  //Inputs-Outputs
  
  pinMode(teachBT, INPUT_PULLUP);//pin8
  pinMode(startBT, INPUT_PULLUP);//pin7
  pinMode(autoBT, INPUT_PULLUP);//pin11
  pinMode(buzzer, OUTPUT);//pin 10
  
  //Do a smooth movement on startup, from home potion to pot position:
  
  indexend = EEPROM.read(0);//อ่านค่าใน eeppos มาเก็บ
//  readEeprom();//เพิ่มอ่านข้อมูลจากEEPROM มาใช้งาน
  readInputs();
  goHome();
  goPot();
   Serial.begin(9600);
  Serial.println(indexend);
// Serial.println("--end-setup()--");
  eeppos = 0;
}

void loop() {
  Serial.println("..25621023_5_39..");

//เมื่อเปิดสวิทช์
if (!started){

  readInputs();
  moveServos();
  readEeprom();//เพิ่มอ่านข้อมูลจากEEPROM มาใช้งาน 
 
  delay(100); 

//1เมื่อกดปุ่มสอนการเคลื่อนที่
if (readTeach==LOW){
    
  savePosition();//บันทึกข้อมูลใน index[],EEPROM manual
  tone(buzzer,500);
  delay(500);
  noTone(buzzer);
}

//2.Manualเมื่อกดปุ่มสตาร์ท
if (readStart==LOW){
  tone(buzzer,700);
  started=true;
  delay(1000);
  noTone(buzzer);

 }


}

  else{
    goHome();
    runTeach();
 
}


//3.Autoเมื่อกดปุ่มauto

if (readAuto==LOW){

  tone(buzzer,700);
  delay(500);
  noTone(buzzer);
  goHome();
  autorun();

}

  Serial.println("--end loop--");

}

void autorun(){
Serial.println("--startAutoRun--");
goHome();
    runTeach();
    Serial.print("runauto = ");
    Serial.println(runauto);      
goHome();
    runTeach();
    Serial.print("runauto = ");
    Serial.println(runauto++);    
goHome();
    runTeach();
    Serial.print("runauto = ");
    Serial.println(runauto++);    
goHome();    
    runTeach();
    Serial.print("runauto = ");
    Serial.println(runauto++);    
goHome();
    runTeach();
    Serial.print("runauto = ");
    Serial.println(runauto++);    
/*
Serial.print("runauto = ");
Serial.println(runauto);
    if (runauto <= 2){
      runTeach();
     delay(500); 
      runauto++;      
Serial.print("runauto = ");
Serial.println(runauto);      
    }  
*/
  runauto = 1;  
  Serial.println("--endAutoRun--");
}

void readInputs(){
//1.เก็บข้อมูลในตัวแปร เป็นค่าเริ่มต้น
//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
//Read potentiometers of servo1-servo6 อ่านค่าปัจจุบันของตัวต้านทานปรับค่าได้  
  
  //servo3
  readUpDown = analogRead(potUpDown);
  //delay(50);ใส่ดีเลย์ทำให้กระตุก ทำงานช้า
  readUpDown = map(readUpDown,0,1023,minUpDown,maxUpDown);

 

//  Serial.print("readInput_readUpDown="); 
//  Serial.println(readUpDown);
     
  //servo6
  readForwardBackward = analogRead(potForwardBackward); 
  readForwardBackward = map(readForwardBackward,0,1023,minForwardBackward,maxForwardBackward);
 
//  Serial.print("readInput_ForwardBackward = ");
//  Serial.println(readForwardBackward);

  //servo2
  readRotate = analogRead(potRotate);
  readRotate = map(readRotate,0,1023,minRotate,maxRotate);
//  Serial.print("readInput_readRotate = ");
//  Serial.println(readRotate);
  
  //servo1
  readGrab = analogRead(potGrab);
  readGrab = map(readGrab,0,1023,minGrab,maxGrab);
//  Serial.print("readInput_readGrab = ");
//  Serial.println(readGrab);
  
  //เพิ่มเซอร์โว4,5
  //servo4
  readUpDown4 = analogRead(potUpDown4);
  readUpDown4 = map(readUpDown4,0,1023,minUpDown4,maxUpDown4);
//  Serial.print("readInput_readUpDown4 = ");
//  Serial.println(readUpDown4);
  //servo5
  readUpDown5 = analogRead(potUpDown5);
  readUpDown5 = map(readUpDown5,0,1023,minUpDown5,maxUpDown5);
//  Serial.print("readInput_readUpDown5 = ");
//  Serial.println(readUpDown5);
   
  //Read buttons
  readTeach = digitalRead(teachBT);
 
  readStart = digitalRead(startBT);

  readAuto = digitalRead(autoBT);
  
  Serial.println("--readInput--");
  
}
void moveServos(){
//ลำดับการทำงานของเซอร์โว 3,6,2,1,4,5
  
  up_down.write(readUpDown);
//  delay(stepSpeed);
//  Serial.print("moveServos_up_down = ");
//  Serial.println(readUpDown);
//  Serial.println(up_down);
  
  forward_backward.write(readForwardBackward);
//  delay(stepSpeed);
//  Serial.print("moveServos_forward_backward = ");
//  Serial.println(readForwardBackward);
//  Serial.println(forward_backward);
  
  
  rotate.write(readRotate);
//  delay(stepSpeed);
  
  grab.write(readGrab);
//  delay(stepSpeed);
  //เพิ่มเซอร์โว4,5
  up_down4.write(readUpDown4);  
//  delay(stepSpeed);
  
  up_down5.write(readUpDown5);
//  delay(stepSpeed);
  
  Serial.println("--moveServos--");
}

//บันทึกข้อมูลเมื่อมีการให้เคลื่อนที่ทีละตำแหน่ง เข้า INDEX[],EEPROM()
void savePosition(){
//  Serial.println(eeppos);//0
// Serial.println(index);//1
//ลำดับการทำงานของเซอร์โว 3,6,2,1,4,5
//2.บันทึกข้อมูลใน  index[1],EEPORM เริ่มตำแหน่งที่1
//เริ่ม index=1,eeppos=0
  
 
  
  eeppos = eeppos+1;
  teachUpDown[index] = readUpDown;//index[1]ข้อมูลมาจากreadInputs,moveServos 
  EEPROM.write(eeppos,readUpDown);//เก็บข้อมูลใน eeppos(1)
  
  Serial.print("index = "); 
  Serial.println(index);
  Serial.print("EEPROM = ");
  Serial.println(eeppos);
 // Serial.println(teachUpDown[index]);

  eeppos = eeppos+1;
  teachForwardBackward[index] = readForwardBackward;
  EEPROM.write(eeppos,readForwardBackward);

  Serial.print("index = "); 
  Serial.println(index);
  Serial.print("EEPROM = ");
  Serial.println(eeppos);
//  Serial.println(teachForwardBackward[index]);
  
  eeppos = eeppos+1;
  teachRotate[index] = readRotate;
  EEPROM.write(eeppos,readRotate);

  Serial.print("index = ");
  Serial.println(index);
  Serial.print("EEPROM = ");
  Serial.println(eeppos);
//  Serial.println(teachRotate[index]);
  
  eeppos = eeppos+1;
  teachGrab[index] = readGrab;
  EEPROM.write(eeppos,readGrab);

  Serial.print("index = ");
  Serial.println(index);
  Serial.print("EEPROM = ");
  Serial.println(eeppos);
//  Serial.println(teachGrab[index]);
  
  //เพิ่มเซอร์โว4,5
  eeppos = eeppos+1;
  teachUpDown4[index] = readUpDown4;
  EEPROM.write(eeppos,readUpDown4);

  Serial.print("index = ");
  Serial.println(index);
  Serial.print("EEPROM = ");
  Serial.println(eeppos);
//  Serial.println(teachUpDown4[index]);
  
  eeppos = eeppos+1;
  teachUpDown5[index] = readUpDown5;
  EEPROM.write(eeppos,readUpDown5);

  Serial.print("index = ");
  Serial.println(index);
  Serial.print("EEPROM = ");
  Serial.println(eeppos);
//  Serial.println(teachUpDown5[index]);
  
/*
//indexสุดท้ายเก็บที่ index[0],eepposสุดท้ายเก็บที่ eeppos[0]
  teachUpDown[indexend] = readUpDown; 
  teachForwardBackward[indexend] = readForwardBackward;
  teachRotate[indexend] = readRotate; 
  teachGrab[indexend] = readGrab;
  teachUpDown4[indexend] = readUpDown4;
  teachUpDown5[indexend] = readUpDown5;
*/  
  EEPROM.write(0,index);//บันทึกข้อมูลสุดท้าย
  
/*eepposend = index * 6;
  Serial.println("indexend = "); 
  Serial.println(indexend);  
*/ 
  indexend = EEPROM.read(0);//อ่านข้อมูลสุดท้าย
  
Serial.println("--savePosition--");
  index++; 
//เก็บข้อมูลเข้าindex ,eeprom
   saveindex = indexend+1;//2
   saveeeppos = eeppos;//6 

/*โค้ดทดลอง   
   saveindex = indexend+1;//2
   saveeeppos = eeppos;//6 
*/   
}
//4.นำข้อมูลมาใช้งาน อ่านค่าจาก eeppos ไปเก็บใน index ที่ 1 ถึง indexที่ eeppos/6
//-----------------------

void readEeprom(){
//ลำดับการทำงานของเซอร์โว
//  25620930//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
//    eeppos = 0;
//    index  = 1;
 
//  Serial.println("indexend = "); 
  
//  Serial.println(indexend); 

  if (count <= indexend) {
 Serial.print("count = ");
  Serial.println(count);//1 ตั้งค่าเริ่มนับในsetup
 Serial.print("indexend = ");
  Serial.println(indexend);//29 อ่านค่าใน eeppos ที่ 0
 Serial.print("index = ");
  Serial.println(index);//1 ตั้งค่าเริ่มต้นในsetup
  eeppos = eeppos+1;//1,7,13,19,...   
  readUpDown = EEPROM.read(eeppos);//1
  teachUpDown[index] = readUpDown;   

//ข้อมูลถูกต้อง  
  Serial.print("EEPROM = ");
  Serial.println(eeppos);//1
//  Serial.print("teachUpDown[index] = ");
//  Serial.println(teachUpDown[index]);
//  
  eeppos = eeppos+1;//2,8,14,20,...
  readForwardBackward = EEPROM.read(eeppos);
  teachForwardBackward[index] = readForwardBackward;
//ข้อมูลถูกต้อง
  Serial.print("EEPROM = ");
  Serial.println(eeppos);//2
//  Serial.print("teachForwardBackward[index] = ");//2
//  Serial.println(teachForwardBackward[index]);//2
//  
  eeppos = eeppos+1;//3,9,15,21,...
  readRotate = EEPROM.read(eeppos);
  teachRotate[index] = readRotate;

  Serial.print("EEPROM = ");
  Serial.println(eeppos);//3
// Serial.print("readRotate = "); 
//  Serial.println(readRotate);

  eeppos = eeppos+1;//4,10,16,22,...
  readGrab = EEPROM.read(eeppos);
  teachGrab[index] = readGrab;
  Serial.print("EEPROM = ");
  Serial.println(eeppos);//3
//  Serial.print("EEPROM_readGrab = ");
//  Serial.println(readGrab);
  
  eeppos = eeppos+1;//5,11,17,23,...
  readUpDown4 = EEPROM.read(eeppos);
  teachUpDown4[index] = readUpDown4;
  Serial.print("EEPROM = ");
  Serial.println(eeppos);//4
//  Serial.print("EEPROM_readUpDown4 = ");
//  Serial.println(readUpDown4);

  eeppos = eeppos+1;//6,12,18,24,...
  readUpDown5 = EEPROM.read(eeppos);
  teachUpDown5[index] = readUpDown5;
  Serial.print("EEPROM = ");
  Serial.println(eeppos);//6
//  Serial.print("EEPROM_readUpDown5 = ");
//  Serial.println(readUpDown5);
  
Serial.println("--endreadEeprom()--");
     
     index++;
     count++;
  }
//เมื่อสอนการเคลื่อนที่ใหม่ให้เริ่มต้นที่index=1,eeppos=0 เพื่อทับข้อมูลเก่า 
  if (count > indexend ){
    index  = saveindex;//1
    eeppos = saveeeppos;//0
    Serial.print("index = ");
    Serial.println(saveindex);//1
   Serial.print("eeppos = "); 
    Serial.println(saveeeppos);//0
  }

}
// ดึงข้อมูลจาก index[] มาใช้

void runTeach(){
  
//ลำดับการทำงานของเซอร์โวเปลี่ยนลำดับการทำงานเซอรืโว25620919//3,6,2,1,4,5
Serial.print("indexend = ");//เป็นindexสุดท้ายที่บันทึกใน eeppos(0) = index
Serial.println(indexend);//จำนวนชุดตำแหน่งเซอร์โวที่เก็บใน eeppos(0) = index
index = indexend;
Serial.println(index);//จำนวนชุดตำแหน่งของเซอร์โว6ตัว

 
  for (int i=0; i<index-1; i++){

//servo3 UpDown
if (teachUpDown[i] < teachUpDown[i+1]){
  for (int j = teachUpDown[i]; j<= teachUpDown[i+1]; j++){
    up_down.write(j);
    delay(stepSpeed3);
    Serial.print("teachUpDown3 = ");
    Serial.println(j);   
  }
}
else if (teachUpDown[i] > teachUpDown[i+1]){
  for (int j = teachUpDown[i]; j>= teachUpDown[i+1]; j--){
    up_down.write(j);
    delay(stepSpeed3);
  }
}
else{
  up_down.write(teachUpDown[i]);
  delay(stepSpeed3);
}

//servo6 ForwardBackward
if (teachForwardBackward[i] < teachForwardBackward[i+1]){
  for (int j = teachForwardBackward[i]; j<= teachForwardBackward[i+1]; j++){
    forward_backward.write(j);
    delay(stepSpeed);
    Serial.print("teachForward6 = ");
    Serial.println(j);   
  }
}
else if (teachForwardBackward[i] > teachForwardBackward[i+1]){
  for (int j = teachForwardBackward[i]; j>= teachForwardBackward[i+1]; j--){
    forward_backward.write(j);
    delay(stepSpeed);
  }
}
else{
  forward_backward.write(teachForwardBackward[i]);
}

//servo2 Rotate
if (teachRotate[i] < teachRotate[i+1]){
  for (int j = teachRotate[i]; j<= teachRotate[i+1]; j++)
  {
    rotate.write(j);
    delay(stepSpeed);
    Serial.print("teachRotate2=");
    Serial.println(j); 
  }
}
else if (teachRotate[i] > teachRotate[i+1]){
  for (int j = teachRotate[i]; j>= teachRotate[i+1]; j--){
    rotate.write(j);
    delay(stepSpeed);
  }  
}
else{
  rotate.write(teachRotate[i]);
  delay(stepSpeed);
 }

//servo1 Grab
if (teachGrab[i] < teachGrab[i+1]){
  for (int j = teachGrab[i]; j<= teachGrab[i+1]; j++){
    delay(stepSpeed3);
    grab.write(j);   
    delay(stepSpeed3);
    Serial.print("teachGrab1 = ");
    Serial.println(j);   
  }
}
else if (teachGrab[i] > teachGrab[i+1]){
  for (int j = teachGrab[i]; j>= teachGrab[i+1]; j--){
    delay(stepSpeed3);
    grab.write(j);    
    delay(stepSpeed3);
  } 
}
else{
  delay(stepSpeed3);
  grab.write(teachGrab[i]);
  delay(stepSpeed3);  
}
//servo4 UpDown4
if (teachUpDown4[i] < teachUpDown4[i+1]){
  for (int j = teachUpDown4[i]; j<= teachUpDown4[i+1]; j++){
    up_down4.write(j);
    delay(stepSpeed3);
    Serial.print("teachUpDown4 = ");
    Serial.println(j);   
  }
}
else if (teachUpDown4[i] > teachUpDown4[i+1]){
  for (int j = teachUpDown4[i]; j>= teachUpDown4[i+1]; j--){
    up_down4.write(j);
    delay(stepSpeed);
  }    
}
else{
  up_down4.write(teachUpDown4[i]);
  delay(stepSpeed);
}
//servo5 UpDown5
if (teachUpDown5[i] < teachUpDown5[i+1]){
  for (int j = teachUpDown5[i]; j<= teachUpDown5[i+1]; j++){
    up_down5.write(j);
    delay(stepSpeed3);
    Serial.print("teachUpDown5 = ");
    Serial.println(j); 
  }
}
else if (teachUpDown5[i] > teachUpDown5[i+1]){
  for (int j = teachUpDown5[i]; j>= teachUpDown5[i+1]; j--){
    up_down5.write(j);
    delay(stepSpeed3);
  }
}
else{
  up_down5.write(teachUpDown5[i]);
  delay(stepSpeed3);
  }


}  
  started=false; 
  Serial.println("--endrunTeach--");
}

//Change values if it's necessary...
void goHome(){
//ลำดับการทำงานของเซอร์โว

//servo3 UpDown 5-100
//servo6 ForwardBackward 160-80
//servo2 Rotate 0-100
//servo1 Grab 180-126
//servo4 UpDown4 5-100
//servo5 UpDown5 5-100
//25621008
//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5

//servo3 UpDown 5-100
if (readUpDown < 32){
  for (int j = readUpDown; j<=32; j++){
    //up_down.write(j);
    delay(stepSpeed3);
     up_down.write(j);
    delay(stepSpeed3);
  }
}
else if (readUpDown > 32){
  for (int j = readUpDown; j>=32; j--){
    //up_down.write(j);
    delay(stepSpeed3);
     up_down.write(j);
    delay(stepSpeed3);
  }
}
else{
 // up_down.write(32);
 delay(stepSpeed3);
  up_down.write(32);
  delay(stepSpeed3);
}

//servo6 ForwardBackward 160-80
  if (readForwardBackward < 80){
  for (int j = readForwardBackward; j<=80; j++){
    delay(stepSpeed);
    forward_backward.write(j);
    delay(stepSpeed);
//    Serial.print("goHome_forward_backward=");
//    Serial.println(j);
  }
}
else if (readForwardBackward > 80){
  for (int j = readForwardBackward; j>=80; j--){
    delay(stepSpeed);
    forward_backward.write(j);
    delay(stepSpeed);
  }
}
else{
  delay(stepSpeed);
  forward_backward.write(80); 
}


//servo2 Rotate 0-100
//0
//50
if (readRotate < 50){
  for (int j = readRotate; j<=50; j++){
    delay(stepSpeed);
    rotate.write(j);
    delay(stepSpeed);
  }
}
else if (readRotate > 50){
  for (int j = readRotate; j>=50; j--){
    delay(stepSpeed);
    rotate.write(j);
    delay(stepSpeed);
  }
}
else{
  delay(stepSpeed); 
  rotate.write(50);
}
//servo1 Grab 180-126
if (readGrab < 150){
  for (int j = readGrab; j<=150; j++){
    delay(stepSpeed3);
    grab.write(j);
    delay(stepSpeed2);
  }
}
else if (readGrab > 150){
  for (int j = readGrab; j>=150; j--){
    delay(stepSpeed3);
    grab.write(j);
    delay(stepSpeed3);
  }
}
else{
  delay(stepSpeed3);
  grab.write(150);
  delay(stepSpeed3);
} 

//servo4 UpDown4 5-100
//32
//50
//32
if (readUpDown4 < 60){
  for (int j = readUpDown4; j<=60; j++){
    delay(stepSpeed3);
    up_down4.write(j);
    delay(stepSpeed3);
  }
}
else if (readUpDown4 > 60){
  for (int j = readUpDown4; j>=60; j--){
    delay(stepSpeed3);
    up_down4.write(j);
    delay(stepSpeed3);
  }
}
else{
  up_down4.write(60);
  delay(stepSpeed3);
  delay(stepSpeed3);
}

//servo5 UpDown5 5-100
if (readUpDown5 < 32){
  for (int j = readUpDown5; j<=32; j++){
    delay(stepSpeed3);
    up_down5.write(j);
    delay(stepSpeed3);
  }
}
else if (readUpDown5 > 32){
  for (int j = readUpDown5; j>=32; j--){
    delay(stepSpeed3);
    up_down5.write(j);
    delay(stepSpeed3);
  }
}  
//25620911 เพิ่ม else   
else{
  //up_down5.write(32);
  delay(stepSpeed3);
  up_down5.write(32);//25620911
  delay(stepSpeed3);
}

//Always start from home position จุดเริ่มต้นไปตำแหน่งใหม่
teachForwardBackward[0]= 80;
//delay(stepSpeed);
teachUpDown[0]=32;
//delay(stepSpeed);
teachRotate[0]=0;
delay(stepSpeed);
teachGrab[0]=150;
//teachGrab[0]=148;
delay(stepSpeed);
//เพิ่มเซอร์โว4,5
//teachUpDown4[0]=32;
teachUpDown4[0]=60;
delay(stepSpeed);
teachUpDown5[0]=32;
delay(stepSpeed);
  Serial.println("--end goHome--");
}
//ไปยังตำแหน่งที่ต้องการ
void goPot(){
//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
Serial.println("--goPot--");
//servo3 UpDown
if (32 > readUpDown){
  for (int j = 32; j>=readUpDown; j--){
    up_down.write(j);
    delay(stepSpeed);
  }
}
else if (readUpDown > 32){
  for (int j = 32; j<=readUpDown; j++){
    up_down.write(j);
    delay(stepSpeed);
  }
}
else{
  up_down.write(readUpDown);
  delay(stepSpeed);
}

//servo6 ForwardBackward  
if (80 > readForwardBackward){
  for (int j = 80; j>=readForwardBackward; j--){
    forward_backward.write(j);
    delay(stepSpeed);
  }
}
else if (80 < readForwardBackward){
  for (int j = 80; j<=readForwardBackward; j++){
    forward_backward.write(j);
    delay(stepSpeed);
  }
}
else{
  forward_backward.write(readForwardBackward);
  delay(stepSpeed);
}

//servo2 Rotate
if (0 > readRotate){
  for (int j = 0; j>=readRotate; j--){   
    rotate.write(j);
    delay(stepSpeed);
//    Serial.print("goPot_readRotate=");
//    Serial.println(j);
  }
}
else if (readRotate > 0){
  for (int j = 0; j<=readRotate; j++){
    rotate.write(j);
    delay(stepSpeed);
  }
}
else{
  rotate.write(readRotate);
}

//servo1 Grab 
if (readGrab > 180){
  for (int j = 180; j<=readGrab; j++){
    grab.write(j);
    delay(stepSpeed);
  }
}
else if (readGrab < 180){
  for (int j = 180; j>=readGrab; j--){
    grab.write(j);
    delay(stepSpeed);
  }
}
else{
  grab.write(readGrab);
}

//เพิ่มเซอร์โว4,5
//servo4 UpDown4
if (32 > readUpDown4){
  for (int j = 32; j>=readUpDown4; j--){
    up_down4.write(j);
    delay(stepSpeed);
  }
}
else if (readUpDown4 > 32){
  for (int j = 32; j<=readUpDown4; j++){
    up_down4.write(j);
    delay(stepSpeed);
    //25620910เพิ่มหน่วยเวลา
//    delay(stepSpeed);
  }
}
else{
  up_down4.write(readUpDown4); 
  //25620910เพิ่มหน่วยเวลา
    delay(stepSpeed);
//    delay(stepSpeed);
}
//servo5 UpDown5
if (32 > readUpDown5){
  for (int j = 32; j>=readUpDown5; j--){
    up_down5.write(j);
    //25620910เพิ่มหน่วยเวลา
    delay(stepSpeed);
//    delay(stepSpeed);
  }
}
else if (readUpDown5 > 32){
  for (int j = 32; j<=readUpDown5; j++){
    up_down5.write(j);  
    //25620910เพิ่มหน่วยเวลา
    delay(stepSpeed);
  }
}
else{
  up_down5.write(readUpDown5);
   //25620910เพิ่มหน่วยเวลา
    delay(stepSpeed);
 }
 Serial.println("--end goPot--");
}type or paste code here
1 Like

ขอบคุณที่แชร์ครับ มีเวลาเอารูปใช้งานจริงมาชมกันหน่อยนะครับ :grin:

การทดสอบใช้งานจริง เชื่อฟังดี ไม่มีปัญหา เชื่อง รอดูความเสถียรระยะยาว ครับ
กำลังหาเซ็นเซอร์ใส่เพื่อให้ตามจับสิ่งของที่มือและส่งไปวางที่กำหนดใว้
และกำหนดแรงบีบจับสิ่งของ จะทำให้หุ่นยนต์มีชีวิตชีวากับคนเล่น
ใช้เซ็นเซอร์อะไรดีครับ

1 Like

เห็นแล้วน่าสนุกนะครับ เอาไปประยุกค์ได้มากมายเลยทีเดียว