grbl控制3轴机械臂 原理 实现 （四） 之GRBL源码修改驱动三轴机械臂
往期回顾：
第一篇:grbl控制3轴机械臂 原理 实现 （一） 之2D机械臂模拟及实现
第二篇:grbl控制3轴机械臂 原理 实现 （二） 之3D机械臂模拟及实现
第三篇:grbl控制3轴机械臂 原理 实现 （三） 之如何通过步进电机控制机械臂、插补算法
通过前面的知识了解后，接下来实现GRBL的源码修改，让其支持机械臂。
我们先找到GRBL的源码下载下来 下载地址：github源码地址
我用的是grbl0.9版本，主要文件有这些：
（robot_arm.c这个文件是我们新增的）
接线来看看，要改这里面哪些文件
先打开setting.c文件，由于机械臂需要初始化，我们通过GRBL的$命令来传初始化参数，所以新增几个命令
找到settings_store_global_setting函数
在 case 27: settings.homing_pulloff = value; break;后面加上
//ROBOT_ARM
//$100=0 like x = 0;change current position value
case 30:
sys.position[X_AXIS] = lroundf(value*settings.steps_per_mm[X_AXIS]);
plan_sync_position();//同步平台总步长,传入大臂角度，算出总步长
return(STATUS_OK); //直接返回 不储存到flash
case 31:
sys.position[Y_AXIS] = lroundf(value*settings.steps_per_mm[Y_AXIS]);
plan_sync_position();//同步平台总步长,传入小臂角度，算出总步长
return(STATUS_OK); //直接返回 不储存到flash
case 32:
sys.position[Z_AXIS] = lroundf(value*settings.steps_per_mm[Z_AXIS]);
plan_sync_position();//同步平台总步长,传入旋转角度，算出总步长
return(STATUS_OK); //直接返回 不储存到flash
case 33:
SCARA_LINKAGE_1 = value; //传入机械臂大臂臂长
return(STATUS_OK); //直接返回 不储存到flash
case 34:
SCARA_LINKAGE_2 = value; //传入机械臂小臂臂长
return(STATUS_OK); //直接返回 不储存到flash打开config.h 定义一下机械臂的宏定义，随意地方加上
//定义robotarm
#define ROBOT_ARM 1打开system.c文件，找到system_convert_array_steps_to_mpos函数将里面的内容替换成
uint8_t idx;
#if defined (IS_ROBOT_ARM)
float position_scara[3];
for (idx=0; idx打开limits.c文件，找到函数limits_go_home 在里面的 } while (n_cycle-- > 0);前面加上
#if defined (ROBOT_ARM)
scara_report_positions();
#endif在其 } while (n_cycle-- > 0); 后面更改为
for (idx=0; idx0?(int32_t)((settings.max_travel[idx]+settings.homing_pulloff)*settings.steps_per_mm[idx]+0.5):(int32_t)((settings.max_travel[idx]+settings.homing_pulloff)*settings.steps_per_mm[idx]-0.5);
} else {
set_axis_position = (-settings.homing_pulloff*settings.steps_per_mm[idx])>0?(int32_t)(-settings.homing_pulloff*settings.steps_per_mm[idx]+0.5):(int32_t)(-settings.homing_pulloff*settings.steps_per_mm[idx]-0.5);
}
#endif 打开planner.c,找到函数 plan_buffer_line 为其添加变量
int32_t target_steps[N_AXIS];
float unit_vec[N_AXIS], delta_mm;
uint8_t idx;
float inverse_unit_vec_value;
float inverse_millimeters; // Inverse millimeters to remove multiple float divides
float junction_cos_theta;
float sin_theta_d2;
#if defined (IS_ROBOT_ARM)
float target_float[N_AXIS];
int32_t position_steps[N_AXIS];
#endif // Prepare and initialize new block
plan_block_t *block = &block_buffer[block_buffer_head];
block->step_event_count = 0;
block->millimeters = 0;
block->direction_bits = 0;
block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later
#ifdef USE_LINE_NUMBERS
block->line_number = line_number;
#endif
// Copy position data based on type of motion being planned.
//根据计划的运动类型复制位置数据。
#if defined (IS_ROBOT_ARM)
memcpy(position_steps, pl.position, sizeof(pl.position));
#endif其下方继续添加
#if defined (IS_ROBOT_ARM)
//通过逆运动学，将目标坐标target换算成机械臂三轴的角度target_float
//fixed small big arm no belong
inverse_kinematics_ROBOT(target,target_float);
#endif
其下面的for循环改成
for (idx=0; idx0?(int32_t)(target[idx]*settings.steps_per_mm[idx]+0.5):(int32_t)(target[idx]*settings.steps_per_mm[idx]-0.5);
block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
}
block->step_event_count = max(block->step_event_count, block->steps[idx]);
if (idx == A_MOTOR) {
delta_mm = ((target_steps[X_AXIS]-pl.position[X_AXIS]) + (target_steps[Y_AXIS]-pl.position[Y_AXIS]))/settings.steps_per_mm[idx];
} else if (idx == B_MOTOR) {
delta_mm = ((target_steps[X_AXIS]-pl.position[X_AXIS]) - (target_steps[Y_AXIS]-pl.position[Y_AXIS]))/settings.steps_per_mm[idx];
} else {
delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
}
#elif defined (IS_ROBOT_ARM)
//target是轴移动的距离，单位是毫米，系统设定了steps_per_mm值，也就是每毫米代表的轴移动步数，直接换算
//得到轴移动步数target_steps
//公式： 此轴总共要走的脉冲数 = 角度 * 多少脉冲/1度
target_steps[idx] = lroundf(target_float[idx]*settings.steps_per_mm[idx]);
//target表示轴从原点移动到终点的总距离，所以当前线段的移动步数需要用target减去之前所有线段移动的总步数
//得到相对移动的脉冲数
block->steps[idx] = abs(target_steps[idx]-position_steps[idx]);
//获得三个轴里移动距离最远的轴移动的距离，后面DDA直线插补时会用到这个值。关于DDA插补算法的方法后面会介绍
block->step_event_count = max(block->step_event_count, block->steps[idx]);
//根据步数换算出真实移动的距离，保存在unit_vec中，后面计算两条线段夹角时会用到
//公式： 移动的相对角度 = （轴移动总脉冲 - 之前移动脉冲的和）即相对脉冲数 / 多少脉冲/1度
delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx];
#else
target_steps[idx] = (target[idx]*settings.steps_per_mm[idx])>0?(int32_t)(target[idx]*settings.steps_per_mm[idx]+0.5):(int32_t)(target[idx]*settings.steps_per_mm[idx]-0.5);
block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
block->step_event_count = max(block->step_event_count, block->steps[idx]);
delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
#endif
unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. 打开gcode.c文件，gc_execute_line函数里面，在case 61下面添加
case 61:
word_bit = MODAL_GROUP_G13;
if (mantissa != 0) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G61.1 not supported]
// gc_block.modal.control = CONTROL_MODE_EXACT_PATH; // G61
break;
#if defined (IS_ROBOT_ARM)
case 95:
angle_mode=!angle_mode;
break;
#endif新建robot_arm.h 和 robot_arm.c文件
robot_arm.h
#ifndef ROBOT_ARM_h
#define ROBOT_ARM_h
#include "grbl.h"
#ifdef ROBOT_ARM
#define IS_ROBOT_ARM 1
#endif
// Length of inner and outer support arms. Measure arm lengths precisely.
//内外支撑臂的长度。精确测量臂长。
//#define SCARA_LINKAGE_1 200.0f //mm 大臂
//#define SCARA_LINKAGE_2 200.0f //mm 小臂 最外侧末端
// SCARA tower offset (position of Tower relative to bed zero position)
// This needs to be reasonably accurate as it defines the printbed position in the SCARA space.
//SCARA塔偏移（塔相对于床零位的位置）
//这需要相当准确，因为它定义了SCARA空间中打印床的位置。
//robotarm
#define SCARA_OFFSET_X 0 //mm 200
#define SCARA_OFFSET_Y 0 //mm
#define SCARA_OFFSET_Z 0 //mm
#define MANUAL_X_HOME_POS -45.43f
#define MANUAL_Y_HOME_POS 95.67f
#define MANUAL_Z_HOME_POS 0.0f
#define RADIANS(d) ((d)*(float)M_PI/180.0f)
#define DEGREES(r) ((r)*180.0f/(float)M_PI)
#define sq(x) x*x
extern volatile float SCARA_LINKAGE_1; //mm 大臂
extern volatile float SCARA_LINKAGE_2; //mm 小臂 最外侧末端
extern volatile float X_error_correction; //输入的长度/实际走长度
extern volatile float Y_error_correction; //
extern volatile float Z_error_correction; //
extern bool angle_mode;
extern float ManualHomePos[3];
void scara_report_positions(void) ;
//fixed small big arm no belong
void inverse_kinematics_ROBOT(float const *cartesian, float *f_scara);
void forward_kinematics_ROBOT(float const *f_scara, float *cartesian);
#endif
robot_arm.c
#include "grbl.h"
#include "robot_arm.h"
volatile float SCARA_LINKAGE_1 = 200.0f; //mm 大臂
volatile float SCARA_LINKAGE_2 = 200.0f; //mm 小臂 最外侧末端
volatile float X_error_correction = 1; //输入的长度/实际走长度
volatile float Y_error_correction = 1;
volatile float Z_error_correction = 1;
#define L1 SCARA_LINKAGE_1
#define L2 SCARA_LINKAGE_2
#define L1_2 sq(SCARA_LINKAGE_1)
#define L2_2 sq(SCARA_LINKAGE_2)
//float const L1 = SCARA_LINKAGE_1,
// L2 = SCARA_LINKAGE_2,
// L1_2 = sq(SCARA_LINKAGE_1),
// L2_2 = sq(SCARA_LINKAGE_2);
bool angle_mode=false;
float ManualHomePos[3]={MANUAL_X_HOME_POS,MANUAL_Y_HOME_POS,MANUAL_Z_HOME_POS};
//此正向运动时根据大臂,小臂都是独立角度，即大臂移动后，小臂角度对应平面不变
//传入值 f_scara是大臂和小臂的角度，cartesian是最终求得的坐标(这里输入的小臂角度是，平面垂线到小臂的角度)
//输入角度 返回坐标
void forward_kinematics_ROBOT(float const *f_scara, float *cartesian)
{
/***********************robot arm****************************/
float X,Y,Z;
float project3D;
//f_scara里面的0,1,2代表：大臂角度，小臂角度，旋转角度
// z + /y
// | /
// |/
// +-----+x
//2d机械臂朝向的方向
//Y = cos(RADIANS(f_scara[X_AXIS])) * L1 + sin(RADIANS(f_scara[Y_AXIS]) + RADIANS(90) - RADIANS(f_scara[X_AXIS])) * L2;
//3D 两臂投影到平台平面长度,通过三角函数转化为坐标轴方向长度
//cos(a) * L1 + sin(b) * L2
project3D = cos(RADIANS(f_scara[X_AXIS])) * L1 + sin(RADIANS(f_scara[Y_AXIS])) * L2;
Y = cos(RADIANS(f_scara[Z_AXIS])) * project3D;
//垂直向上的方向
//sin(a) * L1 + cos(b) * L2
Z = sin(RADIANS(f_scara[X_AXIS])) * L1 + cos(RADIANS(f_scara[Y_AXIS])) * L2;
//2d站机械臂后方看向它，它的右边方向
//X = sin(RADIANS(f_scara[Z_AXIS])) * cos(RADIANS(f_scara[X_AXIS])) * L1 + sin(RADIANS(f_scara[Y_AXIS]) + RADIANS(90) - RADIANS(f_scara[X_AXIS])) * L2;
//3D 站机械臂后方看向它，它的右边方向
X = sin(RADIANS(f_scara[Z_AXIS])) * project3D;
cartesian[X_AXIS] = X + SCARA_OFFSET_X; //求得用户坐标系下X值
cartesian[Y_AXIS] = Y + SCARA_OFFSET_Y; //求得用户坐标系下Y值
cartesian[Z_AXIS] = Z + SCARA_OFFSET_Z;
/***********************robot arm****************************/
}
//此正向运动时根据大臂,小臂都是独立角度，即大臂移动后，小臂角度对应平面不变
// f_scara是大臂和小臂的角度,和整体旋转角度，cartesian是坐标(这里的小臂角度是，平面垂线到小臂的角度)
//输入坐标 返回角度
void inverse_kinematics_ROBOT(float const *cartesian, float *f_scara)
{
/***********************robot arm****************************/
// z + /y
// | /
// |/
// +-----+x
static float ROBOTARM_alpha, ROBOTARM_beta, ROBOTARM_cta ,ROBOTARM_alphapsi, projectxyLength, X, Y, X_2,Y_2,sqrtx_2ay_2;
//实际测试机械臂的世界坐标下Y轴有误差，输入长度/实际要走长度 比值 0.76
//如果不是角度模式
if(!angle_mode)
{
//实际要走路程
f_scara[X_AXIS] = f_scara[X_AXIS] / X_error_correction;
f_scara[Y_AXIS] = f_scara[Y_AXIS] / Y_error_correction;
f_scara[Z_AXIS] = f_scara[Z_AXIS] / Z_error_correction;
}
//首先求得目标点 到 原点的距离
//获取机械臂投射到xy平面的长度
//length = sqrt(x*x + y*y)
projectxyLength = sqrtf(sq(cartesian[X_AXIS]) + sq(cartesian[Y_AXIS]));
//将3d机械臂变量变为2d机械臂的变量
// projectxyLength长度为2d机械臂的X
X = projectxyLength;
X_2 = sq(X);
Y = cartesian[Z_AXIS];
Y_2 = sq(Y);
sqrtx_2ay_2 = sqrtf(X_2 + Y_2);
//求得机械臂所在yz旋转平面的alphapsi角度
ROBOTARM_alphapsi = acosf(X / sqrtx_2ay_2);
//如果坐标在平面以下，将alphapsi取反
if (Y < 0)
{
ROBOTARM_alphapsi = -ROBOTARM_alphapsi;
}
//求得机械臂所在yz旋转平面的alpha角度,，即大臂到xy平面的角度(实际是弧度)
ROBOTARM_alpha = acosf((L1_2 + X_2 + Y_2 - L2_2) / (2 * L1 * sqrtx_2ay_2) ) + ROBOTARM_alphapsi;
//求得小臂的角度(实际是弧度)
ROBOTARM_beta = acosf((X_2 + Y_2 - L1_2 - L2_2) / (2 * L1 * L2) );
//由于小臂是相对大臂不动的，换算成小臂相对平面垂线的角度
ROBOTARM_beta = RADIANS(90) - ROBOTARM_alpha + ROBOTARM_beta;
//求得整体机械臂的旋转角度(实际是弧度)
ROBOTARM_cta = atan2f(cartesian[X_AXIS] , cartesian[Y_AXIS]);
//如果不是角度模式
if(!angle_mode)
{
f_scara[X_AXIS] = DEGREES(ROBOTARM_alpha); //大臂旋转弧度转换为角度
f_scara[Y_AXIS] = DEGREES(ROBOTARM_beta); //小臂旋转弧度转换为角度
f_scara[Z_AXIS] = DEGREES(ROBOTARM_cta);
//test robot_arm
printPgmString("\r\n IK degrees ");
for (int idx=0; idx< N_AXIS; idx++) {
printFloat_CoordValue(f_scara[idx]);
if (idx < (N_AXIS-1)) { printPgmString(","); }
}
}
//否则是角度模式
else
{
f_scara[X_AXIS] = cartesian[X_AXIS];
f_scara[Y_AXIS] = cartesian[Y_AXIS];
f_scara[Z_AXIS] = cartesian[Z_AXIS];
}
/***********************robot arm****************************/
}
void scara_report_positions()
{
u8 idx;
static float position_scara[3];
for (idx=0; idx打开grbl.h添加robot_arm.h头文件
#if defined (ROBOT_ARM)
#include "robot_arm.h"
#endif至此文件修改完毕，接下来编译，烧录到芯片中就行了。步进电机接线时注意电机方向，反了对调A+ A-线就行了。
接下来我们试试用机械臂来写个字，这里用到软件有inkscape和grbl controler，这里提供这两款打包好的下载链接：软件
首先我们生成要写的字的G代码，打开inkscape软件。
点击文件->文档属性
我们将单位改成mm，长宽都设为100
点击左侧的文字编辑
然后在画布上打上你要的字体
点击这个字体选中它
点击菜单的 路径->对象转化成路径
然后，点击菜单的 扩展->Laserengraver->Laser
点击perferences，设置好输出文件路径
到Laser调好速度，输出文件名字，点击应用
能够看到生成了字体笔画轨迹
这样就会得到一个文件picture.nc,我们用记事本打开它
我们把所有的M05字符都换成 G1 Z5，即z方向向上移动5mm，作为抬笔动作
将M03 替换成 G1 Z0 ，作为落笔动作。
保存退出
把我们的机械臂，大臂小臂摆好后再上好电，最好这样摆
我们可以用手机的水平仪量出a和b的角度，量出大臂和小臂的长度
我们打开GRBLcontroller软件，连接上串口
连上后发初始化指令，校准机械臂的位置。这里的值根据你自己测量相应填入
$30=85 即大臂a角度
$31=130 即小臂b角度
$32=0 即旋转角度
$33=200 即大臂长度
$34=200 即小臂长度
然后选好步距，通过这几个箭头，慢慢调整机械臂的笔头到要写字的平面附近
调整好后，点击设为零坐标
现在就可以开始写字了，点击选择文件，选刚刚的picture.nc文件，点击开始，机械臂就开始动起来了。
写出来的字体效果
我们可以看到，左边字体效果良好，而右边写的不好，这是因为机械臂的结构原因，导致臂之间连接是有个松动范围的，用弹簧把各个机械臂往任意一侧拉扯住，就不会造成臂的滑动了，当时我是用手直接掰着它把它给固定住的。
前期测试，其他效果不好的图
