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

ตัวอย่างการเคลื่อนที่รุ่นนี้ใช้บอร์ด nano v3 ใช้โค้ดเดียวกันได้

ผังการต่อสายอุปกรณ์ ลองจัดขาใหม่จะดูสวยขึ้นครับ

โค้ด

/*
 *  https://www.ardumotive.com/roboticarmgrab-en.html
 *  Arduino Robotic Arm with Grab
 *  More info: http://www.ardumotive.com/
 *  arduino pro mini 328 broad 5v 16 Khz 
 *  Dev: Michalis Vasilakis Data: 12/12/2016 Ver: 1.1 
 *  
 *  การเชื่อมขาต่างๆของ arduino pro mini
 *  forward_backward.attach(3);//servo 6 pin 3
 *  up_down.attach(4);//servo 3 pin 4
 *  rotate.attach(5); //servo 2 pin 5
 *  grab.attach(6);//servo 1 pin 6
 *  up_down4.attach(2);//servo 4 pin 2
 *  up_down5.attach(9);//servo 5 pin 9
 *  startBT = 7;// PRESS BUTTOM กดทำงานรอบเดียว
 *  teachBT = 8;// PRESS BUTTOM กดบันทึกการสอนทีละขั้น
 *  autoBT = 11;// PRESS BUTTOM กดเพื่อทำงานมากกว่า1รอบ
 *  buzzer = 10; //สร้างเสียง
 *  
 *  
 *  ฟังก์ชันต่างๆในโค้ด
 *  setup();
 *  loop();
 *  readInputs();//อ่านค่าจากตัวต้านทานปรับค่า6ตัว
 *  goHome();//ไปตำแหน่งเริ่มต้น
 *  goPot();//ไปตำแหน่งตามค่าตัวต้านทานปรับค่าใว้ตอนเปิด   
 *  moveServos();//เซอโวทำงานช่วงเริ่มต้น
 *  readEeprom();//เพิ่มอ่านข้อมูลจากEEPROM มาใช้งาน 
 *  savePosition();//บันทึกข้อมูลใน index[],EEPROM manual
 *  autorunsametime();//ทำงานวนซ้ำหลายรอบโค้ดต่างกัน
 *  autorun();//ทำงานวนซ้ำหลายรอบโค้ดต่างกัน
 *  runTeach();//สอนการทำงาน
 *  
 */

#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;// PRESS BUTTOM กดทำงานรอบเดียว
const int teachBT = 8;// PRESS BUTTOM กดบันทึกการสอนทีละขั้น
const int autoBT = 11;// PRESS BUTTOM กดเพื่อทำงานมากกว่า1รอบ
//2562-09-06 const int buzzer = 9;
const int buzzer = 10; //สร้างเสียง
//สร้างตัวแปรเก็บค่าอ่านได้จากขา A0-A5 ค่าของตัวต้านทานปรับค่าได้
const int potForwardBackward = A3;//servo6
const int potUpDown = A2;//servo3
const int potRotate = A1;//servo2
const int potGrab = A0;//servo1

//เพิ่มเซอร์โว2ตัว
  const int potUpDown4 = A4;//servo4
  const int potUpDown5 = A5;//servo5
//สร้างตัวแปรเก็บค่าองศาการเคลื่อนที่ของเชอโว
/*
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;
*/
//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=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 = 20; //25620910 20 to 50  ช้าลง
  int stepSpeed2 = 30; //25620920 
  int stepSpeed3 = 40; //25620920

//สร้างตัวแปรเพิ่อควบคุมแบบ multitask
unsigned long last_time_servo3_UpDown = 0;
unsigned long last_time_servo6_ForwardBackward = 0;
unsigned long last_time_servo2_Rotate = 0;
unsigned long last_time_servo1_Grab = 0;
unsigned long last_time_servo4_UpDown4 = 0;
unsigned long last_time_servo5_UpDown5 = 0;
unsigned long period_servo3_UpDown = 10;//100,250,500
unsigned long period_servo6_ForwardBackward = 15;//25,50,150,300
unsigned long period_servo2_Rotate = 20;//100,200,400
unsigned long period_servo1_Grab = 25;//150,250,500
unsigned long period_servo4_UpDown4 = 30;//50,150,300
unsigned long period_servo5_UpDown5 = 35;//100,200,400
//สร้างตัวแปรเพิ่อควบคุมแบบ multitask

//-----start setup-----------------------//   
void setup() {
  Serial.begin(9600);
//Attach Servo motors การต่อขาเซอร์โวกับบอร์ด arduino nano V3
  forward_backward.attach(3);//servo 6 pin D3
  up_down.attach(4);//servo 3 pin D4
  rotate.attach(5); //servo 2 pin D5
  grab.attach(6);//servo 1 pin D6
   
  //เพิ่มเซอร์โว4,5
    up_down4.attach(2);//servo 4 pin D2
    up_down5.attach(9);//servo 5 pin D9
   
  //Inputs-Outputs
  
  pinMode(startBT, INPUT_PULLUP);// pin D7
  pinMode(teachBT, INPUT_PULLUP);// pin D8
  pinMode(autoBT, INPUT_PULLUP);// pin D11
  pinMode(buzzer, OUTPUT);// pin D10
  
  //Do a smooth movement on startup, from home potion to pot position:
  
  indexend = EEPROM.read(0);//อ่านค่าใน eeppos มาเก็บ
//readEeprom();//เพิ่มอ่านข้อมูลจากEEPROM มาใช้งาน
  readInputs();
//  goHome();
//  goPot();

  
  Serial.println(indexend);
// Serial.println("--end-setup()--");
  eeppos = 0;
}

//----146------strat loop----- 
void loop() {
  Serial.println("..25621024_5_41..");

//เมื่อเปิดสวิทช์ แขนกลจะเคลื่อนมาตำแหน่งเริ่มต้น
if (!started){

  readInputs();
  moveServos();
  readEeprom();//1.อับโหลดครั้งแรกปิด 2.สอนการเคลื่อนที่ครั้งแรกและอับโหลดครั้งที่สองให้เปิดเพื่ออ่านข้อมูลจากEEPROM มาใช้งาน 
  delay(100); 

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

//2.Manualเมื่อกดปุ่มสตาร์ททำงาน 1 ครั้ง
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();
  autorunsametime();
//  autorun();

}

  Serial.println("--end loop--");
}
//-----199-356-----start- autorunsametime()-----

void autorunsametime(){
/*  
      Serial.print("runauto = ");
      Serial.println(runauto);
    if (runauto <= 2){
      runTeach();
      delay(500); 
      runauto++;      
      Serial.print("runauto = ");
      Serial.println(runauto);      
    }
*/
Serial.println("--Startautorunsametime--");
   //ลำดับการทำงานของเซอร์โวเปลี่ยนลำดับการทำงานเซอรืโว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
//i คือindexสุดท้าย
//เริ่มต้นteachUpDown[i]-ปลายทางteachUpDown[i+1]

if (teachUpDown[i] < teachUpDown[i+1]){
  for (int j = teachUpDown[i]; j<= teachUpDown[i+1]; j++){
//runทีละขั้น 
    if (j <= teachUpDown[i+1]){

if(millis() - last_time_servo3_UpDown >= period_servo3_UpDown )// waits for the servo to get there
  {
    last_time_servo3_UpDown = millis();    
    up_down.write(j);
  }
    
 }

//runทีละขั้น    
//    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--){

if(millis() - last_time_servo3_UpDown >= period_servo3_UpDown )// waits for the servo to get there
  {
    last_time_servo3_UpDown = millis();    
    up_down.write(j);
  }

//    delay(stepSpeed3);
 
  }
}
else{

if(millis() - last_time_servo3_UpDown >= period_servo3_UpDown )// waits for the servo to get there
  {
    last_time_servo3_UpDown = millis();  
  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++){

if(millis() - last_time_servo6_ForwardBackward >= period_servo6_ForwardBackward )// waits for the servo to get there
  {
    last_time_servo6_ForwardBackward = millis();    
    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--){

if(millis() - last_time_servo6_ForwardBackward >= period_servo6_ForwardBackward )// waits for the servo to get there
  {
    last_time_servo6_ForwardBackward = millis();    
    
    forward_backward.write(j);
  }

//    delay(stepSpeed);
  }
}
else{

if(millis() - last_time_servo6_ForwardBackward >= period_servo6_ForwardBackward )// waits for the servo to get there
  {
    last_time_servo6_ForwardBackward = millis();    
  
  forward_backward.write(teachForwardBackward[i]);
  }

}

//servo2 Rotate
if (teachRotate[i] < teachRotate[i+1]){
  for (int j = teachRotate[i]; j<= teachRotate[i+1]; j++)
  {

if(millis() - last_time_servo2_Rotate >= period_servo2_Rotate )// waits for the servo to get there
  {
    last_time_servo2_Rotate = millis();    

    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--){

if(millis() - last_time_servo2_Rotate >= period_servo2_Rotate )// waits for the servo to get there
  {
    last_time_servo2_Rotate = millis();       
    rotate.write(j);
  }
//    delay(stepSpeed);
  }  
}
else{

if(millis() - last_time_servo2_Rotate >= period_servo2_Rotate )// waits for the servo to get there
  {
    last_time_servo2_Rotate = millis();     
    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(stepSpeed2);

if(millis() - last_time_servo1_Grab >= period_servo1_Grab )// waits for the servo to get there
  {
    last_time_servo1_Grab = millis();       
    grab.write(j);   
  }

//    delay(100);
/*
    Serial.print("teachGrab1 = ");
    Serial.println(j);   
*/
  }
}
else if (teachGrab[i] > teachGrab[i+1]){
  for (int j = teachGrab[i]; j>= teachGrab[i+1]; j--){
//    delay(stepSpeed2);

if(millis() - last_time_servo1_Grab >= period_servo1_Grab )// waits for the servo to get there
  {
    last_time_servo1_Grab = millis();       

    grab.write(j);    
  }
//    delay(stepSpeed2);
  } 
}
else{
//  delay(stepSpeed2);

if(millis() - last_time_servo1_Grab >= period_servo1_Grab )// waits for the servo to get there
  {
    last_time_servo1_Grab = millis();       
  
  grab.write(teachGrab[i]);
  }
  
//  delay(stepSpeed2);  
}
//servo4 UpDown4
if (teachUpDown4[i] < teachUpDown4[i+1]){
  for (int j = teachUpDown4[i]; j<= teachUpDown4[i+1]; j++){

if(millis() - last_time_servo4_UpDown4 >= period_servo4_UpDown4 )// waits for the servo to get there
  {
    last_time_servo4_UpDown4 = millis();    
    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--){

if(millis() - last_time_servo4_UpDown4 >= period_servo4_UpDown4 )// waits for the servo to get there
  {
    last_time_servo4_UpDown4 = millis();      
    up_down4.write(j);
  }
//    delay(stepSpeed);
  }    
}
else{

if(millis() - last_time_servo4_UpDown4 >= period_servo4_UpDown4 )// waits for the servo to get there
  {
    last_time_servo4_UpDown4 = millis();  
  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++){

if(millis() - last_time_servo5_UpDown5 >= period_servo5_UpDown5 )// waits for the servo to get there
  {
    last_time_servo5_UpDown5 = millis();  
    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--){

if(millis() - last_time_servo5_UpDown5 >= period_servo5_UpDown5 )// waits for the servo to get there
  {
    last_time_servo5_UpDown5 = millis();  
    up_down5.write(j);
  }

//    delay(stepSpeed3);
  }
}
else{

if(millis() - last_time_servo5_UpDown5 >= period_servo5_UpDown5 )// waits for the servo to get there
  {
    last_time_servo5_UpDown5 = millis();  
  up_down5.write(teachUpDown5[i]);
  }

//  delay(stepSpeed3);
  }


}  
  started=false; 
//  Serial.println("--endrunTeach--");
  Serial.println("----25640415_04-----endautorunsametime--");   
}
//-----199-372---end  autorunsametime()-----

//-----374-397--autorun()-----

void autorun(){
//1
Serial.println("--startAutoRun--");
goHome();
    runTeach();
/*
    Serial.print("runauto = ");
    Serial.println(runauto);      
*/
//2
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--");
}
//----359-397---autorun()-----

//-----401-----readInputs()-----
void readInputs(){
//1.เก็บข้อมูลในตัวแปร เป็นค่าเริ่มต้นจาก MAX,MIN
//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5 เนื่องจากระบบอ่านโค้ดตามลำดับ
//Read potentiometers of servo1-servo6 อ่านค่าปัจจุบันของตัวต้านทานปรับค่าได้  
//servo3
  readUpDown = analogRead(potUpDown);//ขา A5 ต่อเซอร์โว5
//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("--25640415_04---end-readInput-----");
  
}

//-----455-----start moveServos()-----
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("-----end moveServos--");
}

//-----488-----start savePosition()-------

//บันทึกข้อมูลเมื่อมีการให้เคลื่อนที่ทีละตำแหน่ง เข้า 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
  
 
// เซอโว UpDown
  eeppos = eeppos+1;//ตำแหน่งเริ่มต้นบันทึกค่า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]);

// เซอโว ForwardBackward
  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]);

// เซอโว Rotate  
  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]);

// เซอโว Grab  
  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 UpDown4
  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]);

// เพิ่มเซอร์โว5 UpDown5  
  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);//บันทึกข้อมูลสุดท้ายเก็บที่ EEPROM(0)
  
/*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 
*/   
}
//-----485-591----savePosition()-------

//-----593-----start readEeprom()-----
//4.นำข้อมูลมาใช้งาน อ่านค่าจาก eeppos ไปเก็บใน index ที่ 1 ถึง indexที่ eeppos/6
//-----------------------


//-----598-----start readEeprom()-----
void readEeprom(){
//  ลำดับการทำงานของเซอร์โว
//  25620930//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
//  eeppos = 0;//เริ่มที่0
//  index  = 1;//เริ่มที่1
//  Serial.println("indexend = "); 
//  Serial.println(indexend); 
//  เริ่ม count = 1
  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
*/
  }
    Serial.println("end readEeprom");
}
//-----682-----end readEeprom()-----

//-----684-----start runTeach()-----

//ดึงข้อมูลจาก 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++){
//แบ่งการทำงานของเซอร์โวด้วยเวลา
if(millis() - last_time_servo3_UpDown >= period_servo3_UpDown )// waits for the servo to get there
  {
    last_time_servo3_UpDown = millis();
    
    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--){

if(millis() - last_time_servo3_UpDown >= period_servo3_UpDown )// waits for the servo to get there
  {
    last_time_servo3_UpDown = millis();    
    up_down.write(j);
  }
    
//    delay(stepSpeed3);

  }
}
else{

if(millis() - last_time_servo3_UpDown >= period_servo3_UpDown )// waits for the servo to get there
  {
    last_time_servo3_UpDown = millis();  
  up_down.write(teachUpDown[i]);

  }
  
//  delay(stepSpeed3);
}
//-----servo3 UpDown-----

//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(stepSpeed2);
    grab.write(j);   
    delay(100);
/*
    Serial.print("teachGrab1 = ");
    Serial.println(j);   
*/
  }
}
else if (teachGrab[i] > teachGrab[i+1]){
  for (int j = teachGrab[i]; j>= teachGrab[i+1]; j--){
    delay(stepSpeed2);
    grab.write(j);    
    delay(stepSpeed2);
  } 
}
else{
  delay(stepSpeed2);
  grab.write(teachGrab[i]);
  delay(stepSpeed2);  
}
//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("----25640415_04----endrunTeach--");
}

//-----start goHome()-----
//Change values if it's necessary...

void goHome(){
//ลำดับการทำงานของเซอร์โว
//ลำดับการทำงานของเซอร์โว3,6,2,1,4,5
//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
//25640418 ลำดับการทำงานของเซอร์โว6,5,4,3,2,1

//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);
}
//servo3 UpDown 5-100


//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(stepSpeed2);
    grab.write(j);
    delay(stepSpeed2);
  }
}
else if (readGrab > 150){
  for (int j = readGrab; j>=150; j--){
    delay(stepSpeed2);
    grab.write(j);
    delay(stepSpeed2);
  }
}
else{
  delay(stepSpeed2);
  grab.write(150);
  delay(stepSpeed2);
} 
//servo1 Grab 180-126

//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);
}
//servo4 UpDown4 5-100

//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);
}
//servo5 UpDown5 5-100



//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--");
}
3 Likes

โฮ้ละเอียดมาครับ ตัวนิดเดียว ทำงานใหญ่ได้สบายเลย

โค้ดนี้จะรวม โค้ดจริง โค้ดทดลอง คำอธิบาย การแสดงค่าทดสอบต่างๆตามขั้นตอนออกทาง serial monitor มากๆทำให้การทำงาน สะดุด และยังมีปัญหาหน่วยความจำเต็มเกิน 80% เริ่มมีเตือน จึงอยากเปลื่ยนใช้ บอรด์รุ่นอื่น ที่มี ความเร็ว หน่วยความจำ รองรับเทคโนโลยีใหม่ มีที่เลือกใว้ 3 บอร์ด ท่านใดมีประะสบการณ์ใช้รุ่นไหนดี จบในบอร์ดเดียว
ผมมีความรู้ด้าน controler น้อย แต่สนใจมาตั้งแต่ตอนทำโปรเจ็คจบ
แนะนำได้เลยครับ
1.Rasperry pi 4b วงการโปรเจ็คนักศึกษาใช้ Rasperry กันเยอะ สร้างหุ่นยนต์ ปัจจุบันไม่แน่ใจว่าเปลี่ยนหรือยัง
2.Arduino due 32bit
3.Arduino mega 2560 v3
แผนพัฒนาต่อ
1.ใช้ Blynk ควบคุมหรือแสดงค่าต่างๆที่ทำได้ แนะนำ widget box ใดใช้ได้บ้าง
2.จะใช้ c++ หรือ Python ขึ้นอยู่กับโค้ดตัวอย่าง ลอกมาดัดแปลงครับ ขอศึกษาจากความสำเร็จผู้อื่นลงสู่พื้นฐาน 555

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