极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 2968|回复: 0

3自由度串联机械臂实现电磁铁搬运功能

[复制链接]
发表于 2023-3-14 10:32:24 | 显示全部楼层 |阅读模式
本帖最后由 机器谱 于 2023-3-14 10:32 编辑

1、功能描述
      R308样机是一款拥有3自由度的串联机械臂。本文提供的示例所实现的功能为:在3自由度串联机械臂样机上安装电磁铁,实现电磁铁搬运物品的功能。

2、电子硬件
在这个示例中,我们采用了以下硬件,请大家参考:

主控板Basra(兼容Arduino Uno)
扩展板Bigfish2.1
舵机270°伺服电机
电池7.4V锂电池
其它
电磁铁、USB线
电路连接说明:
① 270°伺服电机连接在Bigfish扩展板D4 . GND . VCC接口上
② 270°伺服电机连接在Bigfish扩展板D7 . GND . VCC接口上
③ 270°伺服电机连接在Bigfish扩展板D11 . GND . VCC接口上
电磁铁连接在Bigfish扩展板D9,D10接口上

3、运动控制
上位机:Controller 1.0
下位机编程环境:Arduino 1.8.19

3.1初始位置的设定
① 将Controller下位机程序servo_bigfish.ino直接下载到主控板。这段代码供Controller上位机与主控板通信,并允许调试舵机。代码如下:
  1. /*------------------------------------------------------------------------------------

  2.   版权说明:Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

  3.            Distributed under MIT license.See file LICENSE for detail or copy at

  4.            https://opensource.org/licenses/MIT

  5.            by 机器谱 2023-01-31 https://www.robotway.com/

  6.   ------------------------------

  7. /*

  8. * Bigfish扩展板舵机口; 4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19

  9. * 使用软件调节舵机时请拖拽对应序号的控制块

  10. */

  11. #include <Servo.h>


  12. #define ANGLE_VALUE_MIN 0

  13. #define ANGLE_VALUE_MAX 180

  14. #define PWM_VALUE_MIN 500

  15. #define PWM_VALUE_MAX 2500


  16. #define SERVO_NUM 12


  17. Servo myServo[SERVO_NUM];


  18. int data_array[2] = {0,0};   //servo_pin: data_array[0], servo_value: data_array[1];

  19. int servo_port[SERVO_NUM] = {4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19};

  20. int servo_value[SERVO_NUM] = {};


  21. String data = "";


  22. boolean dataComplete = false;


  23. void setup() {

  24.   Serial.begin(9600);



  25. }


  26. void loop() {



  27.   while(Serial.available())

  28.   {

  29.     int B_flag, P_flag, T_flag;

  30.     data = Serial.readStringUntil('\n');

  31.     data.trim();

  32.     for(int i=0;i<data.length();i++)

  33.     {

  34.       //Serial.println(data[i]);

  35.       switch(data[i])

  36.       {

  37.         case '#':

  38.           B_flag = i;  

  39.         break;

  40.         case 'P':

  41.         {

  42.           String pin = "";

  43.           P_flag = i;

  44.           for(int i=B_flag+1;i<P_flag;i++)

  45.           {

  46.             pin+=data[i];

  47.           }

  48.           data_array[0] = pin.toInt();

  49.         }

  50.         break;

  51.         case 'T':

  52.         {

  53.           String angle = "";

  54.           T_flag = i;

  55.           for(int i=P_flag+1;i<T_flag;i++)

  56.           {

  57.             angle += data[i];

  58.           }

  59.           data_array[1] = angle.toInt();

  60.           servo_value[pin2index(data_array[0])] = data_array[1];

  61.         }

  62.         break;

  63.         default: break;

  64.       }     

  65.     }

  66.    

  67.     /*

  68.     Serial.println(B_flag);

  69.     Serial.println(P_flag);

  70.     Serial.println(T_flag);

  71.    

  72.     for(int i=0;i<2;i++)

  73.     {

  74.       Serial.println(data_array[i]);

  75.     }

  76.     */

  77.    

  78.     dataComplete = true;

  79.   }



  80.   if(dataComplete)

  81.   {

  82.     for(int i=0;i<SERVO_NUM;i++)

  83.     {

  84.       ServoGo(i, servo_value[i]);

  85.       /*********************************串口查看输出***********************************/

  86. //      Serial.print(servo_value[i]);

  87. //      Serial.print(" ");

  88.     }

  89. //    Serial.println();

  90.       /*********************************++++++++++++***********************************/


  91.     dataComplete = false;

  92.   }




  93. }


  94. void ServoStart(int which){

  95.   if(!myServo[which].attached() && (servo_value[which] != 0))myServo[which].attach(servo_port[which]);

  96.       else return;

  97.   pinMode(servo_port[which], OUTPUT);

  98. }


  99. void ServoStop(int which){

  100.   myServo[which].detach();

  101.   digitalWrite(servo_port[which],LOW);

  102. }


  103. void ServoGo(int which , int where){

  104.   ServoStart(which);

  105.   if(where >= ANGLE_VALUE_MIN && where <= ANGLE_VALUE_MAX)

  106.   {

  107.     myServo[which].write(where);

  108.   }

  109.   else if(where >= PWM_VALUE_MIN && where <= PWM_VALUE_MAX)

  110.   {

  111.     myServo[which].writeMicroseconds(where);

  112.   }

  113. }


  114. int pin2index(int _pin){

  115.   int index;

  116.   switch(_pin)

  117.   {

  118.     case 4: index = 0; break;

  119.     case 7: index = 1; break;

  120.     case 11: index = 2; break;

  121.     case 3: index = 3; break;

  122.     case 8: index = 4; break;

  123.     case 12: index = 5; break;

  124.     case 14: index = 6; break;

  125.     case 15: index = 7; break;

  126.     case 16: index = 8; break;

  127.     case 17: index = 9; break;

  128.     case 18: index = 10; break;

  129.     case 19: index = 11; break;

  130.   }

  131.   return index;

  132. }
复制代码

下载完成后,保持主控板和电脑的USB连接,以便利用上位机进行调试。
② 双击打开Controller 1.0b.exe:




③ 界面左上角选择:设置-面板设置,弹出需要显示的调试块,可通过勾选隐藏不需要调试的舵机块:联机-选择主控板对应端口号以及波特率。

④ 拖动进度条,可以观察相应的舵机角度转动。写好对应的舵机调试角度,勾选左下角添加-转化,获得舵机调试的数组:



⑤ 将该数组直接复制到相应的Arduino程序中的get_coordinate()部分进行使用。

3.2调试好角度后将电磁铁搬运例程(calculate_angle_test.ino)下载到主控板:
  1. /*------------------------------------------------------------------------------------

  2.   版权说明:Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

  3.            Distributed under MIT license.See file LICENSE for detail or copy at

  4.            https://opensource.org/licenses/MIT

  5.            by 机器谱 2023-01-31 https://www.robotway.com/

  6.   ------------------------------*/

  7. #include <math.h>

  8. #include <Servo.h>


  9. #define SERVO_SPEED 3460                                        //定义舵机转动快慢的时间

  10. #define ACTION_DELAY 200                                        //定义所有舵机每个状态时间间隔


  11. #define L1 172

  12. #define L2 160

  13. #define L3 135


  14. Servo myServo[6];


  15. int f = 200;                                                    //定义舵机每个状态间转动的次数,以此来确定每个舵机每次转动的角度

  16. int servo_port[6] = {4,7,11,3,8,12};                            //定义舵机引脚

  17. int servo_num = sizeof(servo_port) / sizeof(servo_port[0]);     //定义舵机数量

  18. float value_init[6] = {1500, 1500, 1500, 0, 0, 0};              //定义舵机初始角度


  19. double theta[3] = {};

  20. float value_pwm[6] = {};

  21. float coordinate[3] = {};

  22. int data_num;


  23. boolean dataComplete = false;


  24. void setup() {

  25.   Serial.begin(9600);

  26.   pinMode(9, OUTPUT);

  27.   pinMode(10, OUTPUT);

  28.   for(int i=0;i<servo_num;i++){

  29.     ServoGo(i,value_init[i]);

  30.   }



  31.   get_coordinate(160, 0, 45);

  32.   delay(1000);

  33. }


  34. void loop() {

  35.   get_coordinate(125, 100, 45);

  36.   get_coordinate(125, 100, 20);

  37.   delay(1000);

  38.   put();

  39.   get_coordinate(125, 100, 45);

  40.   get_coordinate(125, 100, 20);

  41.   delay(500);

  42.   get();

  43.   get_coordinate(125, 100, 45);





  44.   get_coordinate(158, 5, 80);

  45.   get_coordinate(158, 5, 51);

  46.   delay(1000);

  47.   put();

  48.   get_coordinate(158, 5, 80);

  49.   get_coordinate(158, 5, 51);

  50.   delay(500);

  51.   get();

  52.   get_coordinate(110, 0, 158);



  53.   get_coordinate(100, -80, 116);

  54.   delay(1000);

  55.   put();

  56.   get_coordinate(100, -80, 140);

  57.   get_coordinate(100, -80, 116);

  58.   delay(500);

  59.   get();

  60.   get_coordinate(100, -80, 140);



  61.   get_coordinate(110, 0, 160);

  62.   get_coordinate(158, 5, 51);

  63.   delay(1000);

  64.   put();

  65.   get_coordinate(158, 5, 80);

  66.   get_coordinate(158, 5, 51);

  67.   delay(500);

  68.   get();

  69.   get_coordinate(160, 5, 80);

  70.   delay(200);

  71. }


  72. void get_coordinate(float x, float y, float z){

  73.   coordinate[0] = x;

  74.   coordinate[1] = y;

  75.   coordinate[2] = z;



  76.   angle();

  77. }


  78. void angle(){

  79.   calculate_position(coordinate);



  80.   for(byte i=0;i<3;i++){  

  81.     value_pwm[i] = map(theta[i], 0, 180, 500, 2500);

  82.   }



  83.   servo_move(value_pwm[0], value_pwm[1], value_pwm[2], 0, 0, 0);

  84.   dataComplete = false;


  85. }


  86. void calculate_position(float coordinate[3]){

  87.   float a, b, c, posX, posY, posZ;

  88.   double theta0, theta1, theta2;


  89.   a = L2;

  90.   b = L3;



  91.   posX = coordinate[0] == 0 ? 1 : coordinate[0];

  92.   posY = coordinate[1];

  93.   posZ = coordinate[2];


  94.   theta0 = atan(posX / posY);

  95.   c = sqrt(posX * posX / sq(sin(theta0)) + sq(posZ - L1));


  96.   theta2 = acos((a * a + b * b - c * c) / (2 * a * b));

  97.   theta1 = asin((posZ - L1) / c) + acos((a * a + c * c - b * b) / (2 * a * c));


  98.   if(theta0 >= 0){

  99.     theta[0] = theta0 * 180 / PI;

  100.   }

  101.   else

  102.   {

  103.     theta[0] = 180 + theta0 * 180 / PI;

  104.   }


  105.   theta[1] = 90 - theta1 * 180 / PI;

  106.   theta[2] = theta2 * 180 / PI;

  107. //   Serial.print("theta0 = ");

  108. //   Serial.println(theta[0]);

  109. //   Serial.print("theta1 = ");

  110. //   Serial.println(theta[1]);

  111. //   Serial.print("theta2 = ");

  112. //   Serial.println(theta[2]);

  113. //   Serial.println("-------------------------------------");


  114. }


  115. void ServoStart(int which)

  116. {

  117.   if(!myServo[which].attached())myServo[which].attach(servo_port[which]);

  118.   pinMode(servo_port[which], OUTPUT);

  119. }




  120. void ServoStop(int which)

  121. {

  122.   myServo[which].detach();

  123.   digitalWrite(servo_port[which],LOW);

  124. }


  125. void ServoGo(int which , int where)

  126. {

  127.   if(where!=200)

  128.   {

  129.     if(where==201) ServoStop(which);

  130.     else

  131.     {

  132.       ServoStart(which);

  133.       myServo[which].writeMicroseconds(where);

  134.     }

  135.   }

  136. }


  137. void servo_move(float value0, float value1, float value2, float value3, float value4, float value5)

  138. {



  139.   float value_arguments[] = {value0, value1, value2, value3, value4, value5};

  140.   float value_delta[servo_num];



  141.   for(int i=0;i<servo_num;i++)

  142.   {

  143.     value_delta[i] = (value_arguments[i] - value_init[i]) / f;

  144.   }



  145.   for(int i=0;i<f;i++)

  146.   {

  147.     for(int k=0;k<servo_num;k++)

  148.     {

  149.       value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];

  150.     }

  151.    

  152.     for(int j=0;j<servo_num;j++)

  153.     {

  154.       ServoGo(j,value_init[j]);

  155.     }

  156.     delayMicroseconds(SERVO_SPEED);

  157.   }





  158. }


  159. void get(){

  160.   digitalWrite(9, HIGH);

  161.   digitalWrite(10, LOW);

  162.   delay(1000);

  163. }


  164. void put(){

  165.   digitalWrite(9, LOW);

  166.   digitalWrite(10, LOW);

  167.   delay(1000);

  168. }
复制代码

4. 资料下载
资料内容:
R308-电磁铁搬运-程序源代码
R308-电磁铁搬运-样机3D文件
Controller1.0b资料包
​资料下载地址:https://www.robotway.com/h-col-191.html

想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-5-20 10:44 , Processed in 0.041701 second(s), 17 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表