本帖最后由 机器谱 于 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): - /*------------------------------------------------------------------------------------
- 版权说明:Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
- Distributed under MIT license.See file LICENSE for detail or copy at
- https://opensource.org/licenses/MIT
- by 机器谱 2023-04-20 https://www.robotway.com/
- ------------------------------*/
- /*
- * 舵机连接,按圆盘顺时针舵机位置依次对应Bigfish:4, 7, 11, 3, 8, 12
- * 书写范围:dx --> 50; dy --> 40
- */
- #include <Arduino.h>
- #include <avr/pgmspace.h>
- #include "model.h"
- #include "words.h"
- #include <Servo.h>
- #define SQU 0
- #define JI 1
- #define QI 2
- #define SHI 3
- #define DAI 4
- Servo myServo[SERVO_NUM]; //笔筒控制舵机数组
- Servo myServo1; //走纸舵机
- Servo myServo2; //压纸舵机
- int servo_port[SERVO_NUM] = {4,7,11,3,8,12}; //笔筒控制舵机引脚
- int words_num[10] = {};
- Point squ[] = {}; //字体
- Point ji[] = {};
- Point qi[] = {};
- Point shi[] = {};
- Point dai[] = {};
- int pos_x = 0, pos_y = 1, pos_z = 2;
- boolean pos_test = false; //位置测试, 真为测试
- void setup() {
- Serial.begin(9600);
- myServo1.attach(16); //走纸舵机引脚
- myServo2.attach(17); //压纸舵机引脚
- myServo1.write(88); //走纸停
- pressServoDown(); //压纸
-
- if(pos_test) posTest(); //坐标位置测试
- wordsArrayLength(); //计算flash中存储的字体数组长度
- delay(1000);
- }
- void loop() { //写字
- // writing(SQU);
- writing(JI); //机
- writing(QI); //器
- writing(SHI); //时
- writing(DAI); //代
- while(1){};
- }
- void setPos(Point pos) {
- static const float _basic_dists[kActuatorCount] = {
- distance(kInitialPoint, kActuatorOrigins[0]),
- distance(kInitialPoint, kActuatorOrigins[1]),
- distance(kInitialPoint, kActuatorOrigins[2]),
- distance(kInitialPoint, kActuatorOrigins[3]),
- distance(kInitialPoint, kActuatorOrigins[4]),
- distance(kInitialPoint, kActuatorOrigins[5])
- };
- int degree[kActuatorCount] = {};
- for (int i = 0; i < kActuatorCount; ++i)
- {
- float dist = distance(pos, kActuatorOrigins[i]);
- float deg = 90.0 + (dist - _basic_dists[i]) /
- kActuatorRadius / M_PI * 180.0;
- degree[i] = floor(deg+0.5);
- }
- for (int i = 0; i < kActuatorCount; ++i)
- {
- ServoGo(i, map(degree[i], 0, 180, 700, 2100));
- }
- }
- void writeLine(Point a, Point b, unsigned long t) {
- static const int dt = 50;
- unsigned long k = t / dt;
- float dx = (b.x - a.x) / (float)k;
- float dy = (b.y - a.y) / (float)k;
- Point p = a;
- for (int i = 0; i < k; ++i)
- {
- setPos(p);
- delay(dt);
- p.x += dx;
- p.y += dy;
- }
- }
- void ServoStart(int which)
- {
- if(!myServo[which].attached())myServo[which].attach(servo_port[which]);
- pinMode(servo_port[which], OUTPUT);
- }
- void ServoStop(int which)
- {
- myServo[which].detach();
- digitalWrite(servo_port[which],LOW);
- }
- void ServoGo(int which , int where)
- {
- ServoStart(which);
- myServo[which].writeMicroseconds(where);
- }
- void posTest(){
- // setPos({175, 185, 20}); //最小坐标
- // delay(1000);
- setPos({200, 200, 50}); //中间坐标,确定笔的位置时可将位置设置为中间坐标,打开开关然后机械调整舵机角度直到笔筒在中间位置即可
- delay(1000);
- // setPos({225, 225, 20}); //最大坐标
- // delay(1000);
- while(1){delay(10);};
- }
- void wordsArrayLength(){
- words_num[0] = sizeof(squArray) / sizeof(squArray[0]) / 3;
- words_num[1] = sizeof(jiArray) / sizeof(jiArray[0]) / 3; //机
- words_num[2] = sizeof(qiArray) / sizeof(qiArray[0]) / 3; //器
- words_num[3] = sizeof(shiArray) / sizeof(shiArray[0]) / 3; //时
- words_num[4] = sizeof(daiArray) / sizeof(daiArray[0]) / 3; //代
- }
- void readProgmem(int p, Point a, Point b, int ary[]){
- a.x = pgm_read_word_near( ary + p * 3 + pos_x) + 175;
- a.y = pgm_read_word_near( ary + p * 3 + pos_y) + 185;
- a.z = pgm_read_word_near( ary + p * 3 + pos_z);
-
- b.x = pgm_read_word_near( ary + (p + 1) * 3 + pos_x) + 175;
- b.y = pgm_read_word_near( ary + (p + 1) * 3 + pos_y) + 185;
- b.z = pgm_read_word_near( ary + (p + 1) * 3 + pos_z);
-
- writeLine(a, b, 500);
- // Serial.print(a.x);
- // Serial.print("_");
- // Serial.print(a.y);
- // Serial.print(" ");
- // Serial.print(b.x);
- // Serial.print("_");
- // Serial.println(b.y);
- }
- void writing(int which){
- for(int i=0;i<words_num[which] - 1;i++){
- switch(which){
- case 0:
- readProgmem(i, squ[i], squ[i+1], squArray);
- break;
- case 1:
- readProgmem(i, ji[i], ji[i+1], jiArray);
- break;
- case 2:
- readProgmem(i, qi[i], qi[i+1], qiArray);
- break;
- case 3:
- readProgmem(i, shi[i], shi[i+1], shiArray);
- break;
- case 4:
- readProgmem(i, dai[i], dai[i+1], daiArray);
- break;
- }
- pos_x = 0;
- pos_y = 1;
- pos_z = 2;
- }
- setPos({200, 200, 50});
- delay(1000);
- pressServoUp();
- delay(100);
- positionSwitch();
- pressServoDown();
- }
- void positionSwitch(){ //走纸函数
- myServo1.write(80);
- delay(500);
- myServo1.write(88);
- delay(100);
- }
- void pressServoDown(){ //压杆落
- myServo2.write(70);
- }
- void pressServoUp(){ //压杆抬
- myServo2.write(75);
- }
复制代码
5. 资料下载
资料内容:
①写字-例程源代码
②写字-样机3D文件
资料下载地址:https://www.robotway.com/h-col-211.html
想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com
|