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文件
资料内容详见：6自由度并联拉线写字机器人-写字
