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

ต่อจากกระทู้แขนกล

โค้คมีประโยชน์มากครับเหมาะกับการเริ่มต้น

ปัญหา

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

2.ไม่จำค่าที่สอนให้เคลื่อนไปตำแหน่งต่างๆเมื่อปิดสวิทช์ไฟและเปิดใหม่ข้อมูลที่สอนไว้หายต้องสอนจำตำแหน่งใหม่ถ้าจะให้บันทึกลงใน sd card เมื่อเปิดสวิทช์ดึงข้อมูลมาทำงานได้ทันที

มีแผนพัฒนาให้เสริพแก้วกาแฟได้ครับ

1 Likes

วางโค้ต ที่ใช้ล่าสุดตอนนี้หน่อยครับ

เซอร์โว1เริ่มที่มือจับไล่ลำดับถึงฐานเป็นเซอร์โวตัวที่6

  /*  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 <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;

//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 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 stepSpeed = 20; //Change this to fo faster!
  int stepSpeed = 20;//25620910 20 to 50  ช้าลง

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(buzzer, OUTPUT);//pin 10
  
  //Do a smooth movement on startup, from home potion to pot position:
  
  readInputs();
  goHome();
  goPot();
  Serial.begin(9600);
}

void loop() {
  if (!started){
readInputs();
moveServos();
if (readTeach==LOW){
  savePosition();
  tone(buzzer,500);
  delay(500);
  noTone(buzzer);
}
if (readStart==LOW){
  tone(buzzer,700);
  started=true;
  delay(1000);
  noTone(buzzer);
 }
}
  else{
goHome();
runTeach();
  }
}

void readInputs()
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5

{
  //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);
  
  //servo2
  readRotate = analogRead(potRotate);
  readRotate = map(readRotate,0,1023,minRotate,maxRotate);
  
  //servo1
  readGrab = analogRead(potGrab);
  readGrab = map(readGrab,0,1023,minGrab,maxGrab);
 
  //เพิ่มเซอร์โว4,5
  //servo4
  readUpDown4 = analogRead(potUpDown4);
  readUpDown4 = map(readUpDown4,0,1023,minUpDown4,maxUpDown4);
  
  //servo5
  readUpDown5 = analogRead(potUpDown5);
  readUpDown5 = map(readUpDown5,0,1023,minUpDown5,maxUpDown5);
  
  //Read buttons
  readTeach = digitalRead(teachBT);
 
  readStart = digitalRead(startBT);
  
}
void moveServos()
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5

{
  up_down.write(readUpDown);
  Serial.print("moveServos_up_down = ");
  Serial.println(readUpDown);
  
  forward_backward.write(readForwardBackward);
  rotate.write(readRotate);
  grab.write(readGrab);

  //เพิ่มเซอร์โว4,5
  up_down4.write(readUpDown4);  
  up_down5.write(readUpDown5);
}

void savePosition(){
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate 
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
  
  teachUpDown[index] = readUpDown;
  
  Serial.print("savePosition_index="); 
  Serial.println(index);
  Serial.println(readUpDown);
  
  teachForwardBackward[index] = readForwardBackward;
  
  teachRotate[index] = readRotate;
  
  teachGrab[index] = readGrab;
  
  //เพิ่มเซอร์โว4,5
  
  teachUpDown4[index] = readUpDown4;
  
  teachUpDown5[index] = readUpDown5;
  
  index++;
}

void runTeach(){
//ลำดับการทำงานของเซอร์โว
//servo2 Rotate 
//servo1 Grab
//servo6 ForwardBackward
//servo3 UpDown
//servo4 UpDown4
//servo5 UpDown5
  
  for (int i=0; i<index-1; 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("runteach_teachRotate=");
    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]);
}

//servo1 Grab

if (teachGrab[i] < teachGrab[i+1]){
  for (int j = teachGrab[i]; j<= teachGrab[i+1]; j++){
    
    delay(stepSpeed);
    grab.write(j);
    
    delay(100);
  }
}
else if (teachGrab[i] > teachGrab[i+1]){
  for (int j = teachGrab[i]; j>= teachGrab[i+1]; j--){
    delay(stepSpeed);
    grab.write(j);
    //delay(stepSpeed);
    delay(100);
  } 
}
else{
  delay(stepSpeed);
  grab.write(teachGrab[i]);
  delay(100);  
}

//servo6 ForwardBackward

if (teachForwardBackward[i] < teachForwardBackward[i+1]){
  for (int j = teachForwardBackward[i]; j<= teachForwardBackward[i+1]; j++){
    forward_backward.write(j);
    delay(stepSpeed);
  }
}
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]);
}

//servo3 UpDown

if (teachUpDown[i] < teachUpDown[i+1]){
  for (int j = teachUpDown[i]; j<= teachUpDown[i+1]; j++){
    up_down.write(j);
    delay(stepSpeed);
  }
}
else if (teachUpDown[i] > teachUpDown[i+1]){
  for (int j = teachUpDown[i]; j>= teachUpDown[i+1]; j--){
    up_down.write(j);
    delay(stepSpeed);
  }
}
else{
  up_down.write(teachUpDown[i]);
}

//servo4 UpDown4

if (teachUpDown4[i] < teachUpDown4[i+1]){
  for (int j = teachUpDown4[i]; j<= teachUpDown4[i+1]; j++){
    up_down4.write(j);
    delay(stepSpeed);
  }
}
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(stepSpeed);
  }
}
else if (teachUpDown5[i] > teachUpDown5[i+1]){
  for (int j = teachUpDown5[i]; j>= teachUpDown5[i+1]; j--){
    up_down5.write(j);
    delay(stepSpeed);
  }
}
else{
  up_down5.write(teachUpDown5[i]);
  delay(stepSpeed);
}

  }  
  started=false;
}

//Change values if it's necessary...

void goHome()
//ลำดับการทำงานของเซอร์โว
//servo6 ForwardBackward 160-80
//servo3 UpDown 5-100
//servo2 Rotate 0-100
//servo1 Grab 180-126
//servo4 UpDown4 5-100
//servo5 UpDown5 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);
  
}

//servo3 UpDown 5-100

if (readUpDown < 32){
  for (int j = readUpDown; j<=32; j++){
    //up_down.write(j);
    delay(stepSpeed);
     up_down.write(j);
    delay(stepSpeed);
  }
}
else if (readUpDown > 32){
  for (int j = readUpDown; j>=32; j--){
    //up_down.write(j);
    delay(stepSpeed);
     up_down.write(j);
    delay(stepSpeed);
  }
}
else{
 // up_down.write(32);
 delay(stepSpeed);
  up_down.write(32);
  delay(stepSpeed);
}

//servo2 Rotate 0-100

if (readRotate < 0){
  for (int j = readRotate; j<=0; j++){
    delay(stepSpeed);
    
    rotate.write(j);
    
    delay(stepSpeed);
  }
}
else if (readRotate > 0){
  for (int j = readRotate; j>=0; j--){
    
    delay(stepSpeed);
    
    rotate.write(j);
    
    delay(stepSpeed);
  }
}
else{
  delay(stepSpeed);
  
  rotate.write(0);
}

//servo1 Grab 180-126

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

//servo4 UpDown4 5-100

if (readUpDown4 < 32){
  for (int j = readUpDown4; j<=32; j++){
    delay(stepSpeed);
    up_down4.write(j);
    delay(stepSpeed);
  }
}
else if (readUpDown4 > 32){
  for (int j = readUpDown4; j>=32; j--){
    delay(stepSpeed);
    up_down4.write(j);
    delay(stepSpeed);
  }
}
else{
  up_down4.write(32);
  delay(stepSpeed);
  delay(stepSpeed);
}


//servo5 UpDown5 5-100

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

else{

  //up_down5.write(32);
  delay(stepSpeed);
  up_down5.write(32);//25620911
  delay(stepSpeed);
}

//Always start from home position จุดเริ่มต้นไปตำแหน่งใหม่

teachForwardBackward[0]= 80;
//delay(stepSpeed);
teachUpDown[0]=32;
//delay(stepSpeed);
teachRotate[0]=0;
//delay(stepSpeed);
teachGrab[0]=148;
delay(stepSpeed);
//เพิ่มเซอร์โว4,5
teachUpDown4[0]=32;
delay(stepSpeed);
teachUpDown4[0]=100;
//delay(stepSpeed);
teachUpDown5[0]=32;
//delay(stepSpeed);


 }


//ไปยังตำแหน่งที่ต้องการ

void goPot()
//ลำดับการทำงานของเซอร์โว
//servo2 Rotate
//servo1 Grab
//servo6 ForwardBackward
//servo3 UpDown
//servo4 UpDown4
//servo5 UpDown5

{

//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 > 148){
  for (int j = 148; j<=readGrab; j++){
    grab.write(j);
    delay(stepSpeed);
  }
}
else if (readGrab < 148){
  for (int j = 148; j>=readGrab; j--){
    grab.write(j);
    delay(stepSpeed);
  }
}
else{
  grab.write(readGrab);
}

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

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

//เพิ่มเซอร์โว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);
  
 }
}

มีสองทางคือ ใช้ sd card อย่างที่ว่า กับบีนทึกลง EEPROM ครับ เวลาเปิดใหม่ก็ให้ไปโหลดต่ำแหน่งจาก EEPROM ก่อนเลย

เดียวขอไล่โค้ตดูก่อน อาจจะต้อง กำหนด step flow การทำงานใหม่ ตรงไหนทำก่อน ตรงไหนทำหลัง ถ้าจะเอาง่ายตอนนี้เลยคือดูว่า มอเตอร์ชุดไหนทำก่อน จดไว้ จากนั้น เอาไปหาว่ามันอยู่ตรงโค้ตส่วนไหน จับมันย้ายขึ้นลงให้ถูก แต่ถ้ามันเป็น step array ก็ต้องลงลึกกันอีกทีหนึ่ง

จะลองพัฒนาต่อครับ

ติดตรงไหนเอามาปรึกษากันครับ

อันนี้จะเอาไปชงกาแฟสดหรอครับ อิอิ :coffee:

ใช้แขนกลประยุกต์ใช้งานบนโต๊ะเหมือน 7bot ตอนนี้กำลังทดสอบโค้ดEEPROM น่าจะง่่ายกว่า ใช้SD CARD กำลังหาจุดที่เอาเข้าไปแทรกในโค้ดแขนกล ตอนนี้ยังใช้งานไม่ได้แต่น่าจะสำเร็จ
ปัญหา การเคลื่อนที่ของเซอร์โวยังไม่นุ่มนวลเพราะเคลื่อนที่ไม่พร้อมกัน หรือใกล้เคียงกัน

#include <EEPROM.h>
//1.ประกาศตัวแปรและ เก็บข้อมูลในตัวแปร
int teachUpDown[100],teachForwardBackward[100];

//1.สมมติข้อมูล1,2,3,4,5,6 เก็บในตัวแปร
int readUpDown =1;
int readForwardBackward = 2;

int index = 1;
int eeppos = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("Writing...");

//2.บันทึกข้อมูลในEeprom เริ่มตำแหน่งที่2,3,4,5,6,7
  EEPROM.write(2,readUpDown+2);//3
  EEPROM.write(3,readForwardBackward+2);//4
 
//3.อ่านข้อมูลมาเก็บในตัวแปร 
  
  readUpDown = EEPROM.read(2);//2=3
  readForwardBackward = EEPROM.read(3);//3=4
 
  
//4.นำข้อมูลมาใช้งาน แสดงข้อมูล Serial Moniter
  
  Serial.println(readUpDown);//3
  Serial.println(readForwardBackward);//4
  
  Serial.println("end1------------------------");
 
}
void loop() {
  savePosition();
}  
void savePosition()
{

//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward

//  Serial.println(readUpDown);//3
//  Serial.println(readForwardBackward);//4
  
//2.บันทึกข้อมูลใน[index]และ EEPROM เริ่มตำแหน่งที่[1],0
  //eepposที่ตำแหน่ง 1
   int eeppos = 0;
  eeppos = eeppos+1;
  teachUpDown[index] = readUpDown+3;//1=6 
  EEPROM.write(eeppos,readUpDown+3);//1=6
  
  
  Serial.print("teachUpDown_index="); 
  Serial.println(index);
  Serial.println(teachUpDown[index]);//6

  eeppos = eeppos+1;//2
  teachForwardBackward[index] = readForwardBackward+3;//1=7
  EEPROM.write(eeppos,readForwardBackward+3);//2=7
  
  Serial.print("teachForwardBackward_index="); 
  Serial.println(index);
  Serial.println(teachForwardBackward[index]);//7
  

//3.อ่านข้อมูลเก็บในตัวแปร


  eeppos = 1;//1
  readUpDown = EEPROM.read(eeppos);//6
  
  Serial.print("eeppos_readUnpDown");
  Serial.print(eeppos);//1
  Serial.print("  = ");
  Serial.println(readUpDown);//6
  Serial.println("---readUpDown----------");
  
  
  eeppos = eeppos+1;//2
  readForwardBackward = EEPROM.read(eeppos);//7
  
  Serial.print("eeppos_readForwardBackward");//2
  Serial.print(eeppos);
  Serial.print(" = ");
  Serial.println(readForwardBackward);//7
  Serial.println("-----------readForwardBackward-----");
     
 index++;
 

}
1 Likes

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

  /*  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;

//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 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 eeppos = 0;

//int stepSpeed = 50; //Change this to fo faster!
  int stepSpeed = 50; //25620910 20 to 50  ช้าลง
  int stepSpeed2 = 50; //25620920 
  int stepSpeed3 = 50; //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(buzzer, OUTPUT);//pin 10
  
  //Do a smooth movement on startup, from home potion to pot position:
  
  readInputs();
//  readEeprom();
  goHome();
  goPot();
  Serial.begin(9600);
}

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

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

//เมื่อกดปุ่มสตาร์ท เลือกข้อมูลจาก EEPROM
if (readStart==LOW){
  tone(buzzer,700);
  started=true;
  delay(1000);
  noTone(buzzer);
//  readEeprom();
//  runTeach();
 }
}
  else
  {
    goHome();
    readEeprom();//อ่านข้อมูลมาใช้ เมื่อกดสตาร์ทครั้งแรก
    runTeach();
  }
}

void readInputs(){
//1.เก็บข้อมูลในตัวแปร
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
//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);
//teachUpDown[index] = readUpDown;
//  Serial.print(index);
// Serial.println(teachUpDown[index]);
     
  //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);
  
}
void moveServos(){
//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5

  up_down.write(readUpDown);
  Serial.print("moveServos_up_down = ");
  Serial.println(readUpDown);
//  Serial.println(up_down);
  
  forward_backward.write(readForwardBackward);
  Serial.print("moveServos_forward_backward = ");
  Serial.println(readForwardBackward);
//  Serial.println(forward_backward);
  
  
  rotate.write(readRotate);
  grab.write(readGrab);

  //เพิ่มเซอร์โว4,5
  up_down4.write(readUpDown4);  
  up_down5.write(readUpDown5);
}

//บันทึกข้อมูลเมื่อมีการให้เคลื่อนที่ทีละตำแหน่ง เข้า INDEX[],EEPROM()
void savePosition(){

//ลำดับการทำงานของเซอร์โว
//servo3 UpDown
//servo6 ForwardBackward
//servo2 Rotate 
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5

//2.บันทึกข้อมูลใน  index[1],EEPORM เริ่มตำแหน่งที่1
  eeppos = eeppos+1;
  teachUpDown[index] = readUpDown; 
  EEPROM.write(eeppos,readUpDown);
  
  Serial.print("savePosition_index="); 
  Serial.println(index);
  Serial.print("EEPROM = ");
  Serial.println(eeppos);
  Serial.println(readUpDown);

  eeppos = eeppos+1;
  teachForwardBackward[index] = readForwardBackward;
  EEPROM.write(eeppos,readForwardBackward);
  
  eeppos = eeppos+1;
  teachRotate[index] = readRotate;
  EEPROM.write(eeppos,readRotate);
  
  eeppos = eeppos+1;
  teachGrab[index] = readGrab;
  EEPROM.write(eeppos,readGrab);
  
  //เพิ่มเซอร์โว4,5
  
  eeppos = eeppos+1;
  teachUpDown4[index] = readUpDown4;
  EEPROM.write(eeppos,readUpDown4);
  
  eeppos = eeppos+1;
  teachUpDown5[index] = readUpDown5;
  EEPROM.write(eeppos,readUpDown5);
  
  index++;
  
  //eeppos++;
}
  /*
  3.อ่านข้อมูลเก็บในตัวแปร
  data1 = EEPROM.read(0);
  data2 = EEPROM.read(1);
  readUpDown = EEPROM.read(2);
  readForwardBackward = EEPROM.read(3);

  //4.นำข้อมูลมาใช้งาน แสดงข้อมูล Serial Moniter
  Serial.println(data1);.
  Serial.println(data2);
  Serial.println(readUpDown);
  Serial.println(readForwardBackward);
 */ 
 
//4.นำข้อมูลมาใช้งาน อ่านค่าจาก eeppos ไปเก็บใน index ที่ 1 ถึง indexที่ eeppos/6

// ดึงข้อมูลจาก EEPROM มาใช้

void readEeprom(){

//ลำดับการทำงานของเซอร์โว
//25620919
//servo6 ForwardBackward
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
//servo3 UpDown
//servo2 Rotate
  
  readForwardBackward = EEPROM.read(eeppos);
  
  readGrab = EEPROM.read(eeppos);
  
  readUpDown4 = EEPROM.read(eeppos);

  readUpDown5 = EEPROM.read(eeppos);

  readUpDown = EEPROM.read(eeppos);

  readRotate = EEPROM.read(eeppos);
  
  Serial.print("EEPROM_readUpDown = ");
  Serial.println(readUpDown);
 
  Serial.println(readForwardBackward);
  
  Serial.println(readRotate);

  Serial.println(readGrab);

  Serial.print("EEPROM_readUpDown4 = ");
  Serial.println(readUpDown4);

  Serial.print("EEPROM_readUpDown4 = ");
  Serial.println(readUpDown4); 

  eeppos++;

}

// ดึงข้อมูลจาก index[] มาใช้
void runTeach()
{
//ลำดับการทำงานของเซอร์โวเปลี่ยนลำดับการทำงานเซอรืโว25620918

//25620919
//servo6 ForwardBackward
//servo1 Grab
//servo4 UpDown4
//servo5 UpDown5
//servo3 UpDown
//servo2 Rotate
  
  for (int i=0; i<index-1; i++)
  {

    
//servo6 ForwardBackward

if (teachForwardBackward[i] < teachForwardBackward[i+1]){
  for (int j = teachForwardBackward[i]; j<= teachForwardBackward[i+1]; j++){
    forward_backward.write(j);
    delay(stepSpeed);
  }
}
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]);
}

//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);
  }
}
else if (teachGrab[i] > teachGrab[i+1]){
  for (int j = teachGrab[i]; j>= teachGrab[i+1]; j--){
    delay(stepSpeed2);
    grab.write(j);
    //delay(stepSpeed2);
    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);
  }
}
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);
  }
}
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);
}

  


//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("runteach_teachUpDown = ");
    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);
}


//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("runteach_teachRotate=");
    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);
}

}  
  started=false;
}

//Change values if it's necessary...

void goHome()
//ลำดับการทำงานของเซอร์โว
//servo6 ForwardBackward 160-80
//servo3 UpDown 5-100
//servo2 Rotate 0-100
//servo1 Grab 180-126
//servo4 UpDown4 5-100
//servo5 UpDown5 5-100

//25620923
//ลำดับการทำงานของเซอร์โว
//servo6 ForwardBackward 160-80
//servo3 UpDown 5-100
//servo2 Rotate 0-100
//servo1 Grab 180-126
//servo5 UpDown5 5-100
//servo4 UpDown4 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);
  
}

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

//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 < 148){
  for (int j = readGrab; j<=148; j++){
    delay(stepSpeed2);
    grab.write(j);
    delay(stepSpeed2);
  }
}
else if (readGrab > 148){
  for (int j = readGrab; j>=148; j--){
    delay(stepSpeed2);
    grab.write(j);
    delay(stepSpeed2);
  }
}
else{
  delay(stepSpeed2);
  grab.write(148);
  delay(stepSpeed2);
} 

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


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




//Always start from home position จุดเริ่มต้นไปตำแหน่งใหม่

teachForwardBackward[0]= 80;
//delay(stepSpeed);

teachUpDown[0]=32;
//delay(stepSpeed);

teachRotate[0]=0;
//delay(stepSpeed);

teachGrab[0]=148;
delay(stepSpeed);

//เพิ่มเซอร์โว4,5
teachUpDown4[0]=32;
delay(stepSpeed);

teachUpDown5[0]=32;
delay(stepSpeed);


 }


//ไปยังตำแหน่งที่ต้องการ

void goPot()
//ลำดับการทำงานของเซอร์โว
//servo2 Rotate
//servo1 Grab
//servo6 ForwardBackward
//servo3 UpDown
//servo4 UpDown4
//servo5 UpDown5

{

//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 > 148){
  for (int j = 148; j<=readGrab; j++){
    grab.write(j);
    delay(stepSpeed);
  }
}
else if (readGrab < 148){
  for (int j = 148; j>=readGrab; j--){
    grab.write(j);
    delay(stepSpeed);
  }
}
else{
  grab.write(readGrab);
}

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

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

//เพิ่มเซอร์โว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);
  
 }
}
1 Likes

มีชุดขอมูลอะไร ที่ต้องเก็บไว้ใน eeprom บ้างครับ เวลาบันถึงมันบันทึกเป็น ตัวแปรชนิดไหน

น่าจะเอาไว้ในฟังก์ชัน readInputs()

ไว้หลัง readUpDownX ทุกมอเตอร์

image

แล้วก็ point ไปที่ตำแหน่ง eeprom นั้นๆ

ผมดูจากใน ฟังก์ชัน goPot() มันน่าจะเครื่องที่ไปทีละแกนนะครับ ตอนนี้มันเคลื่อนไปพร้อมกันเลยหรอ

กำลังนำข้อมูลไปพัฒนาต่อครับใกล้ถึงเป้าหมาย

เยี่ยมไปเลยครับ รอชม… :v:

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

ยังไม่ได้มีวิธีีคำนวณค่ากระแสกะว่าจะติดตั้งแอมมิสเตอร์วัดค่าตอนทำงาน เคยลองใช้สวิชชิงเหมือนว่าจะมีสัณณานกวน ตอนนี้เลือกใช้อะแดปเตอร์ 12 VDCจ่ายกระแสสูงสุด5A ต่อผ่านตัวแปลงไฟแบบปรับโวลท์ได้ 7.2 Vเลือกจ่าย5A เพื่อใช้กับเซอร์โวทั้ง6ตัวมีสวิทช์ตัดไฟเข้าเชอร์โวเพื่อความปลอดภัยช่วงที่ทดสอบโค้ดอาจทำให้แขนเคลื่อนที่ผิดพลาดตีหัวหรือหนีบมือได้ ตอนนี้เซอร์โวตัวที่2,3เริ่มกระตุกเมื่อจ่ายไฟเข้า ยังหาสาเหตุไม่เจอ เมื่อใช้ Serial.print เยอะทำให้หน่วยความจำเต็มและทำงานช้า จะขยายอย่างไรหรืออาจต้องเปลี่ยน บอร์ดที่เร็วขึ้นดีขึ้น(พัฒนาขั้นต่อไปจะใช้DUE 32bit)

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

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

1 Likes

เยี่ยมไปเลยครับ ขอบคุณที่แชร์สิ่งดีๆ นะครับ