6自由度并联拉线写字机器人实现写字功能
本帖最后由 机器谱 于 2023-5-18 09:33 编辑1. 功能说明
本文示例将实现R287样机6自由度并联拉线写字机器人写字（机器时代）的功能。
https://28846868.s21i.faiusr.com/2/ABUIABACGAAgqbKIogYo4JrS0QIw3yg4jhs!600x600.jpg.webp
该机器人有两部分：绘图机构、走纸机构。绘图机构由6个舵机模块近似正六边形位置分布，共同控制位于中心的画笔；还具备一个走纸机构，走纸机构是由一个大圆周舵机驱动的。
2.6自由度并联拉线写字机器人逆解算法
该6自由度并联拉线写字机器人的运动控制采用逆运动更容易一些，下面我们将对其逆运动算法进行介绍。我们先确定该6自由度并联拉线写字机器人的位置，通过建立坐标系的方法确定位置。这里我们选择在6自由度并联拉线写字机器人外一点建立一个直角坐标系，Z轴范围——笔架上下接线间距60，坐标系Z轴0点为7X11平板平面，这里面我们需要求解出每个舵机转动角度与画笔位置的关系：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg2ruIogYopLbIvAcw3AM4ggM!450x450.png.webp 各舵机坐标（注意这里面的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
中心点到每个舵机的距离：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAggbyIogYoxbeywAQw5QI4MQ.png.webp
目标点到每个舵机的距离 ：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg97yIogYomI2a1gIwvwI4Jw.png.webp
中心点到目标点舵机需要转动的角度（弧长公式）：
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgqr2IogYo9qWGnwIw-gI4Ng.png.webp
其中90.0为笔在中心时舵机初始角度（这个很重要，舵机角度安装一定要注意），M_PI=3.1415926，0.5用于五入（为了补充运动过程中无法避免的损耗产生的运动误差）。
3. 电子硬件
本实验中采用了以下硬件：
主控板Basra主控板（兼容Arduino Uno）
扩展板Bigfish2.1扩展板
电池7.4V锂电池
其它
画笔、画笔套管、连线、纸张等
电路连接说明： 舵机连接：按圆盘顺时针方向，舵机位置依次对应Bigfish扩展板的D4, D7, D11,D3, D8, D12
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg776IogYoiveR-AEw-Ak4hwY!600x600.png.webp
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 myServo1; //走纸舵机
Servo myServo2; //压纸舵机
int servo_port = {4,7,11,3,8,12}; //笔筒控制舵机引脚
int words_num = {};
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 = {
distance(kInitialPoint, kActuatorOrigins),
distance(kInitialPoint, kActuatorOrigins),
distance(kInitialPoint, kActuatorOrigins),
distance(kInitialPoint, kActuatorOrigins),
distance(kInitialPoint, kActuatorOrigins),
distance(kInitialPoint, kActuatorOrigins)
};
int degree = {};
for (int i = 0; i < kActuatorCount; ++i)
{
float dist = distance(pos, kActuatorOrigins);
float deg = 90.0 + (dist - _basic_dists) /
kActuatorRadius / M_PI * 180.0;
degree = floor(deg+0.5);
}
for (int i = 0; i < kActuatorCount; ++i)
{
ServoGo(i, map(degree, 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.attached())myServo.attach(servo_port);
pinMode(servo_port, OUTPUT);
}
void ServoStop(int which)
{
myServo.detach();
digitalWrite(servo_port,LOW);
}
void ServoGo(int which , int where)
{
ServoStart(which);
myServo.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 = sizeof(squArray) / sizeof(squArray) / 3;
words_num = sizeof(jiArray) / sizeof(jiArray) / 3; //机
words_num = sizeof(qiArray) / sizeof(qiArray) / 3; //器
words_num = sizeof(shiArray) / sizeof(shiArray) / 3; //时
words_num = sizeof(daiArray) / sizeof(daiArray) / 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 - 1;i++){
switch(which){
case 0:
readProgmem(i, squ, squ, squArray);
break;
case 1:
readProgmem(i, ji, ji, jiArray);
break;
case 2:
readProgmem(i, qi, qi, qiArray);
break;
case 3:
readProgmem(i, shi, shi, shiArray);
break;
case 4:
readProgmem(i, dai, dai, 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
页:
[1]