极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 1706|回复: 0

6自由度并联拉线写字机器人实现写字功能

[复制链接]
发表于 2023-5-18 09:33:33 | 显示全部楼层 |阅读模式
本帖最后由 机器谱 于 2023-5-18 09:33 编辑

1. 功能说明
       本文示例将实现R287样机6自由度并联拉线写字机器人写字(机器时代)的功能。

       该机器人有两部分:绘图机构、走纸机构。绘图机构由6个舵机模块近似正六边形位置分布,共同控制位于中心的画笔;还具备一个走纸机构,走纸机构是由一个大圆周舵机驱动的。

2.  6自由度并联拉线写字机器人逆解算法
       该6自由度并联拉线写字机器人的运动控制采用逆运动更容易一些,下面我们将对其逆运动算法进行介绍。我们先确定该6自由度并联拉线写字机器人的位置,通过建立坐标系的方法确定位置。这里我们选择在6自由度并联拉线写字机器人外一点建立一个直角坐标系,Z轴范围——笔架上下接线间距60,坐标系Z轴0点为7X11平板平面,这里面我们需要求解出每个舵机转动角度与画笔位置的关系:
       各舵机坐标(注意这里面的Z轴坐标是以实际作用到舵机上的为准)
           1(97,55,-10)
           2(25,200,50)
           3(97,345,-10)
           4(302,345,50)
           5(375,200,-10)
           6(302,55,50)
       中心点(x,y,z);目标点(xt,yt,zt);舵机半径 radius——24.0

中心点到每个舵机的距离:


目标点到每个舵机的距离 :


中心点到目标点舵机需要转动的角度(弧长公式):


       其中90.0为笔在中心时舵机初始角度(这个很重要,舵机角度安装一定要注意),M_PI=3.1415926,0.5用于五入(为了补充运动过程中无法避免的损耗产生的运动误差)。

3. 电子硬件

      本实验中采用了以下硬件:

主控板
Basra主控板(兼容Arduino Uno)
扩展板
Bigfish2.1扩展板
电池
7.4V锂电池
其它
画笔、画笔套管、连线、纸张等

电路连接说明: 舵机连接:按圆盘顺时针方向,舵机位置依次对应Bigfish扩展板的D4, D7, D11,D3, D8, D12


4. 功能实现
编程环境:Arduino 1.8.19
下面提供一个6自由度并联拉线写字机器人写字(机器时代)的参考例程(servo_writing.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-04-20 https://www.robotway.com/

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

  7. /*

  8. * 舵机连接,按圆盘顺时针舵机位置依次对应Bigfish:4, 7, 11, 3, 8, 12

  9. * 书写范围:dx --> 50; dy --> 40

  10. */

  11. #include <Arduino.h>

  12. #include <avr/pgmspace.h>

  13. #include "model.h"

  14. #include "words.h"

  15. #include <Servo.h>


  16. #define SQU 0

  17. #define JI 1

  18. #define QI 2

  19. #define SHI 3

  20. #define DAI 4


  21. Servo myServo[SERVO_NUM];     //笔筒控制舵机数组

  22. Servo myServo1;               //走纸舵机

  23. Servo myServo2;               //压纸舵机

  24. int servo_port[SERVO_NUM] = {4,7,11,3,8,12};   //笔筒控制舵机引脚   

  25. int words_num[10] = {};   


  26. Point squ[] = {};              //字体

  27. Point ji[] = {};

  28. Point qi[] = {};

  29. Point shi[] = {};

  30. Point dai[] = {};   


  31. int pos_x = 0, pos_y = 1, pos_z = 2;


  32. boolean pos_test = false;      //位置测试, 真为测试


  33. void setup() {

  34.   Serial.begin(9600);

  35.   myServo1.attach(16);         //走纸舵机引脚

  36.   myServo2.attach(17);         //压纸舵机引脚

  37.   myServo1.write(88);          //走纸停

  38.   pressServoDown();            //压纸



  39.   if(pos_test) posTest();      //坐标位置测试

  40.   wordsArrayLength();          //计算flash中存储的字体数组长度

  41.   delay(1000);

  42. }


  43. void loop() {                  //写字

  44. //   writing(SQU);

  45.   writing(JI);                 //机

  46.   writing(QI);                 //器

  47.   writing(SHI);                //时

  48.   writing(DAI);                //代

  49.   while(1){};

  50. }


  51. void setPos(Point pos) {                              

  52.   static const float _basic_dists[kActuatorCount] = {

  53.     distance(kInitialPoint, kActuatorOrigins[0]),

  54.     distance(kInitialPoint, kActuatorOrigins[1]),

  55.     distance(kInitialPoint, kActuatorOrigins[2]),

  56.     distance(kInitialPoint, kActuatorOrigins[3]),

  57.     distance(kInitialPoint, kActuatorOrigins[4]),

  58.     distance(kInitialPoint, kActuatorOrigins[5])

  59.   };

  60.   int degree[kActuatorCount] = {};

  61.   for (int i = 0; i < kActuatorCount; ++i)

  62.   {

  63.     float dist = distance(pos, kActuatorOrigins[i]);

  64.     float deg = 90.0 + (dist - _basic_dists[i]) /

  65.                    kActuatorRadius / M_PI * 180.0;

  66.     degree[i] = floor(deg+0.5);

  67.   }

  68.   for (int i = 0; i < kActuatorCount; ++i)

  69.   {

  70.     ServoGo(i, map(degree[i], 0, 180, 700, 2100));

  71.   }

  72. }


  73. void writeLine(Point a, Point b, unsigned long t) {

  74.   static const int dt = 50;

  75.   unsigned long k = t / dt;

  76.   float dx = (b.x - a.x) / (float)k;

  77.   float dy = (b.y - a.y) / (float)k;

  78.   Point p = a;

  79.   for (int i = 0; i < k; ++i)

  80.   {

  81.     setPos(p);

  82.     delay(dt);

  83.     p.x += dx;

  84.     p.y += dy;

  85.   }

  86. }


  87. void ServoStart(int which)

  88. {

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

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

  91. }



  92. void ServoStop(int which)

  93. {

  94.   myServo[which].detach();

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

  96. }


  97. void ServoGo(int which , int where)

  98. {

  99.     ServoStart(which);

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

  101. }


  102. void posTest(){

  103. //    setPos({175, 185, 20});   //最小坐标

  104. //    delay(1000);

  105.     setPos({200, 200, 50});   //中间坐标,确定笔的位置时可将位置设置为中间坐标,打开开关然后机械调整舵机角度直到笔筒在中间位置即可

  106.     delay(1000);

  107. //    setPos({225, 225, 20});   //最大坐标

  108. //    delay(1000);


  109.     while(1){delay(10);};

  110. }


  111. void wordsArrayLength(){

  112.    words_num[0] = sizeof(squArray) / sizeof(squArray[0]) / 3;

  113.    words_num[1] = sizeof(jiArray) / sizeof(jiArray[0]) / 3;             //机

  114.    words_num[2] = sizeof(qiArray) / sizeof(qiArray[0]) / 3;             //器

  115.    words_num[3] = sizeof(shiArray) / sizeof(shiArray[0]) / 3;           //时

  116.    words_num[4] = sizeof(daiArray) / sizeof(daiArray[0]) / 3;           //代

  117. }


  118. void readProgmem(int p, Point a, Point b, int ary[]){

  119.     a.x = pgm_read_word_near( ary + p * 3 + pos_x) + 175;

  120.     a.y = pgm_read_word_near( ary + p * 3 + pos_y) + 185;

  121.     a.z = pgm_read_word_near( ary + p * 3 + pos_z);

  122.    

  123.     b.x = pgm_read_word_near( ary + (p + 1) * 3 + pos_x) + 175;

  124.     b.y = pgm_read_word_near( ary + (p + 1) * 3 + pos_y) + 185;

  125.     b.z = pgm_read_word_near( ary + (p + 1) * 3 + pos_z);

  126.    

  127.     writeLine(a, b, 500);


  128. //    Serial.print(a.x);  

  129. //    Serial.print("_");

  130. //    Serial.print(a.y);

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

  132. //    Serial.print(b.x);  

  133. //    Serial.print("_");

  134. //    Serial.println(b.y);

  135. }


  136. void writing(int which){

  137.   for(int i=0;i<words_num[which] - 1;i++){

  138.     switch(which){

  139.         case 0:

  140.           readProgmem(i, squ[i], squ[i+1], squArray);

  141.         break;

  142.         case 1:

  143.           readProgmem(i, ji[i], ji[i+1], jiArray);

  144.         break;

  145.         case 2:

  146.           readProgmem(i, qi[i], qi[i+1], qiArray);

  147.         break;

  148.         case 3:

  149.           readProgmem(i, shi[i], shi[i+1], shiArray);

  150.         break;

  151.         case 4:

  152.           readProgmem(i, dai[i], dai[i+1], daiArray);

  153.         break;

  154.     }

  155.     pos_x = 0;

  156.     pos_y = 1;

  157.     pos_z = 2;

  158.   }

  159.   setPos({200, 200, 50});

  160.   delay(1000);

  161.   pressServoUp();

  162.   delay(100);

  163.   positionSwitch();

  164.   pressServoDown();

  165. }


  166. void positionSwitch(){                //走纸函数

  167.     myServo1.write(80);

  168.     delay(500);

  169.     myServo1.write(88);

  170.     delay(100);

  171. }


  172. void pressServoDown(){                //压杆落

  173.   myServo2.write(70);  

  174. }


  175. void pressServoUp(){                  //压杆抬

  176.   myServo2.write(75);  

  177. }
复制代码

5. 资料下载
资料内容:
​①写字-例程源代码
​②写字-样机3D文件
资料下载地址:https://www.robotway.com/h-col-211.html

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

回复

使用道具 举报

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

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

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 09:23 , Processed in 0.047403 second(s), 17 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

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