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

🕹 Controller
1

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

15684485227581458309638
156844852275814583096382976×3968 3.07 MB

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

ปัญหา

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

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

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

1 Likes
4

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

5

เซอร์โว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);
  
 }
}
6

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

7

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

8

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

9

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

10

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

11

ใช้แขนกลประยุกต์ใช้งานบนโต๊ะเหมือน 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
12

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

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

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

14

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

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

image

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

15

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

image
image776×800 41.6 KB

16

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

17

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

18

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

19

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

20

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

21

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

1 Likes
22

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