ทำแขนกล Robot ARM ด้วย Arduino แถมยังบันทึกตำแหน่งแบบง่ายๆ

เห็น Robot arm ของท่านนี้แล้วนับว่าน่าสนใจมากๆ สามารถกำหนดตำแหน่งแบบง่ายๆ ไม่ต้องเขียนโปรแกรมใหม่ ใช้หมุนแล้วกดปุ่มเก็บค่าพิกัดที่อยู่ตรงนั้น จำไว้เป็ยสเต็ปต่อเนื่องไปเรื่อยๆ สุดท้ายก็กดรันยาวๆ ไป โอโห้คิดได้ไงครัช แถมยังมีไฟล์ solidwork และ code arduino มาให้ใจดีจริงๆ ว่างๆ คงต้องมาลองเล่นกันดูละครับ แต่ปัญหาคือ ปริ้นเตอร์ 3D นี้สิ หึหึ

Solidworks files: https://grabcad.com/library/robot-arm-43

Arduino code:
https://drive.google.com/drive/folders/0B6M3VFjMuPx4bG1rWE1sRnVwS2c

ใครลองแล้วบอกผมด้วยนะครับ อิอิ

1 Likes

อยากรู้ว่าในกล่องคอนโทลสีดำในคลิปนะครับ ประกอบไปด้วยอะไรบ้าง
คือผมอยากทำแต่ไม่ค่อยรู้เรื่องเลยครับ รบกสนหน่อยนะครับ @Remy_Martin

2 Likes

โค้ดเต็ม Robot_Arm_V2.1 เดียวมีเวลามาช่วยพี่แกะโค้ดกันนะครับ แล้วเราจะใช้เว็บ IDE sim ผลออกมาดูกัน

///////////////////////////////////////////////////////
/* 
   Robot movement is based on the principle of inverse kinematics
   
   OPERATION
     Format:
     1. Any coordinate:
     {x,  y,  z}
     * -10 <= x <= 10
     * 1 <= y <= 10
     * 0 <= z <= 6

     2. Tiempo de espera:
       {0.001,  0.001,  T}
     * 0.001: reserved for a wait time.
     * "T" is the wait time, in milliseconds

     3. Open and close GRIPPER
     {0.002,  0.002,  G}
     * 0.002: reserved for open/close gripper.
     * 60 <= G <= 125 (G: degree sexagesimal)
      
     
  EXAMPLE:
  
  {-5,  0,  6.5},         // initial position
  {0.001,  0.001,  200},  // wait 200 milliseconds
  {-5,  0,  -1},          // nex position
  {0.001,  0.001,  200},  // wait 200 milliseconds
  {-8.5,  0,  -1},        // nex position
  {0.001,  0.001,  1000}, // wait 1000 milliseconds (1 second)
  {0.002,  0.002,  35},   // close gripper
  {0.001,  0.001,  1000}, // wait 1000 milliseconds (1 second)
  {-5,  0,  0},           // nex position
  {0.001,  0.001,  200},  // wait 200 milliseconds
  {-5,  0,  6.5},         // nex position
  {0.001,  0.001,  200},  // wait 200 milliseconds
  {5,  0,  6.5},          // nex position
  {0.001,  0.001,  200},  // wait 200 milliseconds
  {8.5,  0,  6.5},        // nex position
  {0.001,  0.001,  1000}, // wait 1000 milliseconds (1 second)
  {0.002,  0.002,  90},   // open gripper
  {0.001,  0.001,  2000}, // wait 2000 milliseconds (2 second)
  {5,  0,  6.5},          // nex position
  {0.001,  0.001,  200},  // wait 200 milliseconds
  {-5,  0,  6.5},         // initial position (It is going to the starting point to start a new cycle)

  AUTOR
   Fernandez Villanueva David
   fernandezv.david@gmail.com
   National University of Engineering LIMA-PERU
   Version 2.0

*/
///////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////

#include <Servo.h>
Servo miServo1; Servo miServo2; Servo miServo3; Servo miServo4;

void setup()
{
  Serial.begin(9600);
  miServo1.attach(5);  // AXIS Y
  miServo2.attach(6);  // AXIS Z
  miServo3.attach(9);  // GRIPPER
  miServo4.attach(10); // X
}

double X[] = {5, 0, -5};
double Y[] = {5, 5, 5};
double Z[] = {2, 2, 2};

//{00.00,  08.05,  02.00},
int speed_mov = 5;
const int num_xyz = 21;

///////////////////////////////////////////////////////////////////
double coordinates[num_xyz][3] = { 
//HEAR SHOULD WRITE THE COORDINATES
  {-5,  0,  6.5},
  {0.001,  0.001,  200},
  {-5,  0,  -1},
  {0.001,  0.001,  200},
  {-8.5,  0,  -1},
  {0.001,  0.001,  1000},
  {0.002,  0.002,  35},
  {0.001,  0.001,  1000},
  {-5,  0,  0},
  {0.001,  0.001,  200},
  {-5,  0,  6.5},
  {0.001,  0.001,  200},
  {5,  0,  6.5},
  {0.001,  0.001,  200},
  {8.5,  0,  6.5},
  {0.001,  0.001,  1000},
  {0.002,  0.002,  90},
  {0.001,  0.001,  2000},
  {5,  0,  6.5},
  {0.001,  0.001,  200},
  {-5,  0,  6.5},
  
 // END COORDINATES
};
/////////////////////////////////////////////////////////////////

const int mum_X = sizeof(X) / 4;

//double theta1[mum_X];
//double theta2[mum_X];
//double theta3[mum_X];

double theta1[num_xyz];
double theta2[num_xyz];
double theta3[num_xyz];

int alpha1, alpha2, alpha3, alpha4;
int vect_with_coord[num_xyz];



int c_theta1_2_3[num_xyz][4];

//int next_point_rate[11][6];
float next_point_rate[num_xyz][7];

const float pi = 3.14;

double L1 = 0;
double L2 = 6;
double L3 = 5.5;
double x, y, z;
double q11, q22, q33;
float h, c, alpha, ganma, q1, q2, q3;
int cont = 0;

double num_prueba = -10.9;
double num_abs;

int num_coord = 0;

//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
void loop()
{
  inv_kinematics();
  rate_add();
  delay(1000);
  while (1)
  {
    path_fin();
  }
}
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////

double inv_kinematics()
{
  for (int i = 0; i < num_xyz; i++) // coordinates[0][0]
  {
    if ((coordinates[i][0] == coordinates[i][1]) && (coordinates[i][0] == 0.001))
    {
      c_theta1_2_3[i][0] = 1;
      c_theta1_2_3[i][1] = (int)(coordinates[i][2]);
      c_theta1_2_3[i][2] = (int)(coordinates[i][2]);
      c_theta1_2_3[i][3] = (int)(coordinates[i][2]);
    }
    else if ((coordinates[i][0] == coordinates[i][1]) && (coordinates[i][0] == 0.002))
    {
      c_theta1_2_3[i][0] = 2;
      c_theta1_2_3[i][1] = (int)(coordinates[i][2]);
      c_theta1_2_3[i][2] = (int)(coordinates[i][2]);
      c_theta1_2_3[i][3] = (int)(coordinates[i][2]);
    }
    else
    {
      x = coordinates[i][0];
      y = coordinates[i][1];
      z = coordinates[i][2];

      h = sqrt(pow(x, 2) + pow(y, 2));
      c = sqrt(pow(x, 2) + pow(y, 2) + pow((z - L1), 2));
      alpha = acos((pow(L2, 2) + pow(c, 2) - pow(L3, 2)) / (2 * L2 * c));
      ganma = atan((z - L1) / (x));

      q1 = atan(y / x) * 180 / pi;
      q2 = (atan((z - L1) / (h)) + alpha) * 180 / pi;
      q3 = (acos((pow(c, 2) - pow(L2, 2) - pow(L3, 2)) / (2 * L2 * L3))) * 180 / pi;

      if (x < 0)
      {
        q11 = 180 + (int)(q1); //(int)(q1);
      }
      else
      {
        q11 = (int)(q1); //(int)(q1);
      }
      q22 = (int)(q2); //(int)(q2);
      q33 = (int)(q3); //(int)(q3);

      c_theta1_2_3[i][0] = 0;
      c_theta1_2_3[i][1] = q11;
      c_theta1_2_3[i][2] = q22;
      c_theta1_2_3[i][3] = q33;


      theta1[i] = q11;
      theta2[i] = q22;
      theta3[i] = q33;
      cont = cont + 1;
    }
  }
  //comprueva_array2();

}


double absoluto(double num1)
{
  if (num1 < 0)
  {
    return (-1) * num1;
  }
  else
  {
    return num1;
  }

}



void rate_add()
{ //int next_point_rate[11][6];
  int cont_rate_add = 0;
  int first_coord = 0;

  //int vect_with_coord[11];
  int num_no_coord = 0;
  //int num_coord=0;
  for (int j = 0; j < num_xyz; j++)
  {
    if (c_theta1_2_3[j][0] == 0)
    {
      vect_with_coord[num_coord] = j;
      num_coord = 1 + num_coord; // número de coordenadas "0".
    }
    else
    {
      //vect_with_coord[10 - num_no_coord] = 99;
      vect_with_coord[(num_xyz-1) - num_no_coord] = 99;
      num_no_coord = num_no_coord + 1;
    }
  }

  // rate calculate
  int cont_num_coord = 0;
  for (int i = 0; i < num_xyz; i++)
  {
    if (c_theta1_2_3[i][0] == 0)
    {
      if (i == vect_with_coord[num_coord - 1])
      {
        int k0 = vect_with_coord[cont_num_coord];
        int k1 = vect_with_coord[0];
        next_point_rate[i][0] = c_theta1_2_3[k1][1] - c_theta1_2_3[k0][1];
        next_point_rate[i][1] = c_theta1_2_3[k1][2] - c_theta1_2_3[k0][2];
        next_point_rate[i][2] = c_theta1_2_3[k1][3] - c_theta1_2_3[k0][3];

        int maxim_tree_val = max_tree(next_point_rate[i][0], next_point_rate[i][1], next_point_rate[i][2]);

        next_point_rate[i][3] = next_point_rate[i][0] / maxim_tree_val;
        next_point_rate[i][4] = next_point_rate[i][1] / maxim_tree_val;
        next_point_rate[i][5] = next_point_rate[i][2] / maxim_tree_val;
        next_point_rate[i][6] = maxim_tree_val;

        cont_num_coord = cont_num_coord + 1;
      }
      else
      {
        int k0 = vect_with_coord[cont_num_coord];
        int k1 = vect_with_coord[cont_num_coord + 1];
        next_point_rate[i][0] = c_theta1_2_3[k1][1] - c_theta1_2_3[k0][1];
        next_point_rate[i][1] = c_theta1_2_3[k1][2] - c_theta1_2_3[k0][2];
        next_point_rate[i][2] = c_theta1_2_3[k1][3] - c_theta1_2_3[k0][3];

        int maxim_tree_val = max_tree(next_point_rate[i][0], next_point_rate[i][1], next_point_rate[i][2]);

        next_point_rate[i][3] = next_point_rate[i][0] / maxim_tree_val;
        next_point_rate[i][4] = next_point_rate[i][1] / maxim_tree_val;
        next_point_rate[i][5] = next_point_rate[i][2] / maxim_tree_val;
        next_point_rate[i][6] = maxim_tree_val;

        cont_num_coord = cont_num_coord + 1;
      }
    }
  }
}







int max_tree(int num1, int num2, int num3)
{
  if (num1 < 0) {
    num1 = num1 * (-1);
  }
  if (num2 < 0) {
    num2 = num2 * (-1);
  }
  if (num3 < 0) {
    num3 = num3 * (-1);
  }

  int borra = max(num1, num2);
  int maxim_tree = max(borra, num3);

  return maxim_tree;
}


void path_fin()
{
  int inicio = 0;
  for (int i = 0; i < num_xyz; i++)
  { ///////////


    if (c_theta1_2_3[i][0] == 0)
    {
      if (inicio == 0) //first loop
      {
        delay(speed_mov);
        alpha1 = (int)(c_theta1_2_3[i][1]);
        alpha2 = (int)(c_theta1_2_3[i][2]);
        alpha3 = (int)(c_theta1_2_3[i][3]);
        miServo4.write(43+alpha2);             
        miServo1.write(55+(alpha3-alpha2));
        miServo2.write(alpha1);              

      }
      else
      {
        int netxc = vect_with_coord[inicio - 1];
        for (int j = 1; j <= next_point_rate[netxc][6]; j++)
        {
          delay(speed_mov);
          alpha1 = (int)(c_theta1_2_3[netxc][1] + j * next_point_rate[netxc][3]);
          alpha2 = (int)(c_theta1_2_3[netxc][2] + j * next_point_rate[netxc][4]);
          alpha3 = (int)(c_theta1_2_3[netxc][3] + j * next_point_rate[netxc][5]);
          miServo4.write(43+alpha2);              
          miServo1.write(55+(alpha3-alpha2));  
          miServo2.write(alpha1);                 
        }
      }
      inicio = inicio + 1;
    }

    else if (c_theta1_2_3[i][0] == 1)// retardo
    {
      delay(c_theta1_2_3[i][2]);
      //Serial.println(c_theta1_2_3[i][2]);
      //delay(500);
    }
    else                            
    {
      int gripper_g = (int)(c_theta1_2_3[i][2]);
      miServo3.write(gripper_g);
      //int miServo3();
    }

    //
  }// end for




}
1 Likes

รบกวนหน่อยนะครับแจ้งผลด้วยนะครับพี่ ขอบคุณครับ

1 Likes

ช่วยแนะนำการต่อวงจรแขนกลหน่อยครับ:+1::+1::+1::+1::pray::pray::pray::pray:

2 Likes

เรียนสาขาอะไรครับ แล้วมีความรู้เรื่องอะไรบ้าง

1 Likes

เรียนอิเล็กทรอนิกส์ครับ ผวา ตอนนี้กำลังศึกษาเรื่องArduinoครับ

2 Likes

โดยเบื่้องต้นลองศึกษาข้อมูลเพิ่มเติ่มก่อนก็ได้ครับ ผมมีโครงการจะทำแขนกลอยู่เหมือนกันรอติดตามละกันนะครับ
หรือถ้ายังไงลองซื้อโมดูลราคาไม่แพงมาทดสอบดูก่อน แล้วถ้าติดปัญหาอะไรในส่วนไหนเอามาคุยกันในนี้ได้เลยครับ :grin:

1 Likes

พี่ครับ ผมอยาก รู้เกี่ยวกับวิธีการทำด้วยครับ อยากพี่ทำเป็นคลิปโชว์เลยได้ไหม55

2 Likes

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

ส่วนของกล่องควบคุมการทำงาน

ส่วนของโค้ด

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

#include <Servo.h>

//Create servo objects to control servo motors
Servo up_down;  
Servo forward_backward;
Servo rotate;
Servo grab;

//Constants
const int startBT = 7;
const int teachBT = 8;
const int buzzer = 9;
const int potForwardBackward = A3;
const int potUpDown = A2;
const int potRotate = A1;
const int potGrab = A0;
//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;
//-------------------------------------//
//Variables
int readUpDown,readForwardBackward,readRotate,readGrab,readTeach,readStart;
int teachUpDown[100],teachForwardBackward[100],teachRotate[100],teachGrab[100];
boolean started =false;
int index = 1;
int stepSpeed = 20; //Change this to fo faster!

void setup() {
  //Attach Servo motors
  forward_backward.attach(3);
  up_down.attach(4);
  rotate.attach(5); //portokali xontro
  grab.attach(6);  
  //Inputs-Outputs
  pinMode(teachBT, INPUT_PULLUP);
  pinMode(startBT, INPUT_PULLUP);
  pinMode(buzzer, OUTPUT);
  //Do a smooth movement on startup, from home potion to pot position:
  readInputs();
  goHome();
  goPot();
}

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(){
  //Read potentiometers
  readUpDown = analogRead(potUpDown);
  readUpDown = map(readUpDown,0,1023,minUpDown,maxUpDown);
  readForwardBackward = analogRead(potForwardBackward);
  readForwardBackward = map(readForwardBackward,0,1023,minForwardBackward,maxForwardBackward);
  readRotate = analogRead(potRotate);
  readRotate = map(readRotate,0,1023,minRotate,maxRotate);
  readGrab = analogRead(potGrab);
  readGrab = map(readGrab,0,1023,minGrab,maxGrab);
  //Read buttons
  readTeach = digitalRead(teachBT);
  readStart = digitalRead(startBT);
  delay(50);
}
void moveServos(){
  up_down.write(readUpDown);
  forward_backward.write(readForwardBackward);
  rotate.write(readRotate);
  grab.write(readGrab);
}

void savePosition(){
  teachUpDown[index] = readUpDown;
  teachForwardBackward[index] = readForwardBackward;
  teachRotate[index] = readRotate;
  teachGrab[index] = readGrab;
  index++;
}

void runTeach(){
  for (int i=0; i<index-1; i++){
if (teachRotate[i] < teachRotate[i+1]){
  for (int j = teachRotate[i]; j<= teachRotate[i+1]; j++){
    rotate.write(j);
    delay(stepSpeed);
  }
}
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]);
}
if (teachGrab[i] < teachGrab[i+1]){
  for (int j = teachGrab[i]; j<= teachGrab[i+1]; j++){
    grab.write(j);
    delay(stepSpeed);
  }
}
else if (teachGrab[i] > teachGrab[i+1]){
  for (int j = teachGrab[i]; j>= teachGrab[i+1]; j--){
    grab.write(j);
    delay(stepSpeed);
  } 
}
else{
  grab.write(teachGrab[i]);
}
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]);
}
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]);
}
  }
  started=false;
}

//Change values if it's necessary...
void goHome(){
  if (readForwardBackward < 80){
  for (int j = readForwardBackward; j<=80; j++){
    forward_backward.write(j);
    delay(stepSpeed);
  }
}
else if (readForwardBackward > 80){
  for (int j = readForwardBackward; j>=80; j--){
    forward_backward.write(j);
    delay(stepSpeed);
  }
}
else{
  forward_backward.write(80);
}
if (readUpDown < 32){
  for (int j = readUpDown; j<=32; j++){
    up_down.write(j);
    delay(stepSpeed);
  }
}
else if (readUpDown > 32){
  for (int j = readUpDown; j>=32; j--){
    up_down.write(j);
    delay(stepSpeed);
  }
}
else{
  up_down.write(32);
}
if (readRotate < 0){
  for (int j = readRotate; j<=0; j++){
    rotate.write(j);
    delay(stepSpeed);
  }
}
else if (readRotate > 0){
  for (int j = readRotate; j>=0; j--){
    rotate.write(j);
    delay(stepSpeed);
  }
}
else{
  rotate.write(0);
}
if (readGrab < 148){
  for (int j = readGrab; j<=148; j++){
    grab.write(j);
    delay(stepSpeed);
  }
}
else if (readGrab > 148){
  for (int j = readGrab; j>=148; j--){
    grab.write(j);
    delay(stepSpeed);
  }
}
else{
  grab.write(148);
}
//Always start from home position
teachForwardBackward[0]= 80;
teachUpDown[0]=32;
teachRotate[0]=0;
teachGrab[0]=148;
}

void goPot(){
if (0 > readRotate){
  for (int j = 0; j>=readRotate; j--){
    rotate.write(j);
    delay(stepSpeed);
  }
}
else if (readRotate > 0){
  for (int j = 0; j<=readRotate; j++){
    rotate.write(j);
    delay(stepSpeed);
  }
}
else{
  rotate.write(readRotate);
}
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);
}
  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);
}
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);
}
}
1 Likes

ชุดแขนกลรุ่นนี้สั่งซื้อได้ที่ไหน

1 Likes

ไม่มีขายครับแต่มีไฟล์ 3D เอาไปปริ้นด้วย 3D ออกมาได้ ถ้าไม่มีอาจต้องไปจ้างเขาปริ้นให้ครับ

สนใจครับบอรด์arduinoรุ่นไหนไม่ชัดเจน แนะนำด้วยครับ ตัวแรกเป็น uno แต่ตัวหลังตัวที่จำตำแหน่งการเคลื่อนที่และมีกล่องดำควบคุมอยู่ใต้ฐานดูไม่ชัดครับ

ผมตอบไปแล้วนะครับ

ขอบคุณครับ

มีใครทำแขนกลเองบ้างครับ

กำลังจะทำครับ 555

อยากทำครับ แต่คงต้องศึกษาอีกนาน ถ้าแอดทำเสร็จเอามาสอนเป็นวิทยาทานด้วยครับ

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

1 Likes

ช่วงนี้จะหายหน้าหายตาไปสักนิด ติดโปรเจคอื่นอยู่นะครับ 555