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 athttps://opensource.org/licenses/MITby 机器谱 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 4Servo 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自由度并联拉线写字机器人-写字