本帖最后由 机器谱 于 2023-4-23 09:49 编辑
1. 功能说明
本文示例将实现R305b样机3自由度并联绘图机器人写字的功能。本实验使用的样机是用探索者兼容零件制作的。
2. 电子硬件
在这个示例中,采用了以下硬件,请大家参考:
主控板 | Basra主控板(兼容Arduino Uno) | 扩展板 | Bigfish2.1扩展板 | 传感器 | 触碰传感器 | 电池 | 7.4V锂电池 |
3. 功能实现
3.1建立坐标系
我们需要先对3自由度并联绘图机器人进行极限位置调节,下图中为左极限位置,右极限位置与其对称。3自由度并联绘图机器人绘图主要由图中所示两个伺服电机控制。
3.2 硬件连接
① 触碰传感器连接Bigfish扩展板A0端口
② 左侧舵机连接Bigfish扩展板D4号引脚
③ 右侧舵机连接Bigfish扩展板D7号引脚
④ 控制底部连杆的舵机连接到Bigfish扩展板D11号引脚
3.3 示例程序
编程环境:Arduino 1.8.19
实现思路:接通电源后,等待3秒,按下触碰传感器,3自由度并联绘图机器人开始依次写出“王”、“云”、“飞”三个汉字。
将参考例程(ThreeServo_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-03 https://www.robotway.com/
- ------------------------------*/
- /*******************************************************************************************************
- 实现功能:
- 三自由度舵机组成的机械臂实现写字;
- 实现思路:
- 接通电源后,等待3秒,按下触碰传感器,3自由度并联绘图机器人开始依次写出“王”、“云”、“飞”三个汉字
- 实验接线:
- 触碰传感器接到A0;
- 控制左边连杆的舵机接到D4;
- 控制右边连杆的舵机接到D7;
- 控制底部连杆的舵机接到D11;
- *******************************************************************************************************/
- #include <Servo.h> //驱动舵机需要的函数库
- #define SERVOLEFTNULL 500 //左侧舵机转动到0度的值
- #define SERVORIGHTNULL 500 //右侧舵机转动到0度的值
- #define SERVOFAKTORLEFT 603 //左侧舵机转动到0度右侧舵机转动到90度时的值
- #define SERVOFAKTORRIGHT 610 //左侧舵机转动到90度右侧舵机转动到180度时的值
- #define LIFT0 1500 //落笔写字时舵机转动的值
- #define LIFT1 1800 //一笔写完换笔时舵机转动的值
- #define LIFT2 1400 //初始及完成写字后抬笔时舵机的值
- #define SERVOPINLEFT 4 //定义左边舵机要连接的引脚;
- #define SERVOPINRIGHT 7 //定义右边舵机要连接的引脚;
- #define SERVOPINLIFT 11 //定义底部舵机要链接的引脚;
- #define TouchSensor A0 //触碰传感器连接的引脚
- #define LIFTSPEED 1500 //定义抬笔落笔时舵机转动的速度
- #define L1 90 //定义与舵机相连杆的长度 mm
- #define L2 117.6 //定义与舵机相连杆的端点铰接处中心到笔中心的距离 mm
- #define L3 31.6 //定义中间杆顶端铰接处中心到笔的距离 mm
- #define X1 40.0 //定义左侧舵机中心轴到原点在X方向上的距离
- #define Y1 -42 //定义左侧舵机中心轴到X轴的垂直距离
- #define X2 111.4 //定义右侧舵机中心轴到原点在X方向上的距离
- #define Y2 -42 //定义右侧舵机中心轴到到X轴的垂直距离
- //坐标系建立第一象限,X轴方向为140mm,Y轴方向为120mm,选取坐标点时Y值建议大于70
- int coAry[10][10][2] = {
- //汉字 王
- {{25,110},{0,0},{51,110},{1,1}},
- {{26,98},{0,0},{49,98},{1,1}},
- {{25,84},{0,0},{51,84},{1,1}},
- {{37,110},{0,0},{37,84},{1,1}},
- //汉字 云
- {{61,110},{0,0},{81,110},{1,1}},
- {{58,100},{0,0},{85,100},{1,1}},
- {{68,100},{0,0},{62,85},{86,85},{1,1}},
- {{80,96},{0,0},{91,82},{1,1}},
-
- //汉字 飞
- {{99,110},{0,0},{114,110},{114,98},{115,90},{115,86},{118,84},{121,82},{126,88},{1,1}},
- {{121,103},{0,0},{116,98},{122,94},{1,1}}
-
- };
- int servoLift = LIFT2;
- Servo servo1; //声明舵机1
- Servo servo2; //声明舵机2
- Servo servo3; //声明舵机3
- volatile double lastX = 25; //笔初始坐标值
- volatile double lastY = 110;
- void setup() {
- Serial.begin(9600); //开启串口通信
- servo1.attach(SERVOPINLEFT);
- servo2.attach(SERVOPINRIGHT);
- servo3.attach(SERVOPINLIFT); //声明舵机引脚;
- servo3.writeMicroseconds(LIFT0+500); //初始化机械身躯的高度;
- drawTo(lastX,lastY); //舵机初始角度;
- delay(3000);
- }
- void Waiting() //直到触碰传感器触发,程序开始运行
- {
- while(digitalRead(TouchSensor)){
- delay(2);
- }
- delay(1000);
- }
- void loop() {
- Waiting();//直到触碰传感器触发,程序开始运行
- if(1)
- {
- if(!servo1.attached()) servo1.attach(SERVOPINLEFT);
- if(!servo2.attached()) servo2.attach(SERVOPINRIGHT);
- if(!servo3.attached()) servo3.attach(SERVOPINLIFT);
- lift(2);
- //汉字坐标处理
- for(int i=0;i<10;i++)
- {
- for(int j=0;j<10;j++)
- {
- int x = coAry[i][j][0];
- int y = coAry[i][j][1];
- if(x == 0 && y == 0)
- {
- lift(0);
- continue;
- }
- else if(x == 1 && y == 1)
- {
- lift(1);
- break;
- }
- drawTo(x,y);
- }
- }
- lift(2);
- servo1.detach();
- servo2.detach();
- servo3.detach();
- }
- delay(1/0); //程序一直处于等待状态;(类似于死循环)
- }
- //抬笔舵机转动函数,三个动作,落笔、换笔、起笔, 0 ,1, 2
- void lift(char lift)
- {
- switch(lift)
- {
- case 0:
- Serial.println("lift0");
- if(servoLift >= LIFT0)
- {
- while(servoLift >= LIFT0)
- {
- servoLift--;
- servo3.writeMicroseconds(servoLift);
- delayMicroseconds(LIFTSPEED);
- }
- }
- else
- {
- while(servoLift <= LIFT0)
- {
- servoLift++;
- servo3.writeMicroseconds(servoLift);
- delayMicroseconds(LIFTSPEED);
- }
- }
- break;
- case 1:
- Serial.println("lift1");
- if(servoLift >= LIFT1)
- {
- while(servoLift >= LIFT1)
- {
- servoLift--;
- servo3.writeMicroseconds(servoLift);
- delayMicroseconds(LIFTSPEED);
- }
- }
- else
- {
- while(servoLift <= LIFT1)
- {
- servoLift++;
- servo3.writeMicroseconds(servoLift);
- delayMicroseconds(LIFTSPEED);
- }
- }
- break;
- case 2:
- Serial.println("lift2");
- if(servoLift >= LIFT2)
- {
- while(servoLift >= LIFT2)
- {
- servoLift--;
- servo3.writeMicroseconds(servoLift);
- delayMicroseconds(LIFTSPEED);
- }
- }
- else
- {
- while(servoLift <= LIFT2)
- {
- servoLift++;
- servo3.writeMicroseconds(servoLift);
- delayMicroseconds(LIFTSPEED);
- }
- }
- break;
- default:break;
- }
- }
- //笔移动函数,参数为点坐标值,计算两点坐标距离来控制笔移动
- void drawTo(double pX, double pY) {
- double dx, dy, c;
- int i;
-
- Serial.println(pX);
- Serial.println(pY);
- // dx dy of new point
- dx = pX - lastX;
- dy = pY - lastY;
- //path lenght in mm, times 4 equals 4 steps per mm
- c = floor(10 * sqrt(dx * dx + dy * dy));
- if (c < 1) c = 1;
- for (i = 0; i <= c; i++) {
- // draw line point by point
- set_XY(lastX + (i * dx / c), lastY + (i * dy / c));
- }
- lastX = pX;
- lastY = pY;
- }
- double return_angle(double a, double b, double c) {
- // cosine rule for angle between c and a
- return acos((a * a + c * c - b * b) / (2 * a * c));
- }
- //用各种三角函数把位置坐标换算成舵机的角度,如何计算,请参考
- //http://www.thingiverse.com/thing:248009/
- void set_XY(double Tx, double Ty)
- {
- delay(1);
- double dx, dy, c, a1, a2, Hx, Hy;
- // calculate triangle between pen, servoLeft and arm joint
- // cartesian dx/dy
- dx = Tx - X1;
- dy = Ty - Y1;
- // polar lemgth (c) and angle (a1)
- c = sqrt(dx * dx + dy * dy); //
- a1 = atan2(dy, dx); //
- a2 = return_angle(L1, L2, c);
-
- //Serial.println(floor(((M_PI-a1 - a2) * SERVOFAKTORLEFT) + SERVOLEFTNULL));
- servo1.writeMicroseconds(floor(((M_PI-a1 - a2) * SERVOFAKTORLEFT) + SERVOLEFTNULL));
- // calculate joinr arm point for triangle of the right servo arm
- a2 = return_angle(L2, L1, c);
- Hx = Tx + L3 * cos((a1 - a2 + 0.621) + M_PI); //36,5掳
- Hy = Ty + L3 * sin((a1 - a2 + 0.621) + M_PI);
- // calculate triangle between pen joint, servoRight and arm joint
- dx = Hx - X2;
- dy = Hy - Y2;
- c = sqrt(dx * dx + dy * dy);
- a1 = atan2(dy, dx);
- a2 = return_angle(L1, (L2 - L3), c);
-
- //Serial.println(floor(((M_PI - a1 + a2) * SERVOFAKTORRIGHT) + SERVORIGHTNULL));
- servo2.writeMicroseconds(floor(((M_PI - a1 + a2) * SERVOFAKTORRIGHT) + SERVORIGHTNULL));
- }
复制代码
4. 资料下载
资料内容:
①写字2-例程源代码
②写字2-样机3D文件
资料下载地址:https://www.robotway.com/h-col-205.html
想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com
|