代码收藏家 Robot Arm 机械臂源码解析
Robot Arm 机械臂源码解析
说明：
Robot Arm是我复刻，也是玩的第一款机械臂。用的是三自由度的结构，你可以理解为了三个电机，三轴有自己的一些缺陷。相比于六轴机械臂而言因为结构的缺陷，不能达到空间内的一些点，这些点又叫做奇异点。但是问题不大，完成一些基础的操作是完全没有问题的。
国外大佬20sffactory开源项目。就免去了我们设计，编程代码这一系列从头再来的繁琐。具体的话到github上面去搜索就行；
写这篇小博文，主要是想分享一下，学习的心得与体会。
关于这个机械臂，我做了一个小的思维导图：
理解的话其实内置的基础算法实现也没有那么难，官网自带的资料我觉得就很好，前提是你要有一定的英文功底；当然，翻译也行
源码可以实现基本的码垛机器人，配合马林固件实现绘画机器人，
还可以结合视觉进行二次开发。都可以直接修改。
通过串口与机械臂进行交互，如下是定义的一些指令：类似于Gcode，和数控加工，数控雕刻差不多。
翻译一下，如图：
如思维导图所说：
我用的是Arduino uno板，做基础的功能都是能实现的，有些地方需要取舍末端执行器换成SG90舵机，风扇不能随时控制，，
用uno板结合GRBL固件是可以实现写字机的绘图与写字功能的，但修改用来控制机械臂就会：
如之前所说，限制还是比较大；
这篇博客，主要是想说一下代码解析部分；也是我比较熟悉的一部分，毕竟别硬件的实现都有现成的；
主控为了节省成本，我用的是 Arduino uno ，用 C++ 来编写的，然后我之前有C的基础，所以理解起来还是没什么问题的，但是因为没有系统的学习过C++的相关知识，所以在细节上面难免会有一些解释不到位的地方，欢迎各位大佬指出；
Arduino 主函数里面分为 void setup（）和 void loop（）函数。
前一个看名字就知道是初始化一些配置，第二个就相当于51或者stm32里面的这个while（1）循环函数。
在 void setup（）和 void loop（）函数之前还会有一个引脚声明，类似于 51 或者 stm32 中的 sbit，在 pinout.h 中有详细的定义；
机械臂是通过控制不同方位的电机角度，进而控制大臂，小臂的姿态，让末端执行器达到指定位置；在代码中大臂用 LOW_SHANK 表示，小臂用 HIGH_SHANK 表示
首先解释一下机械臂的：
运动学正解（Forward kinematics）
通过各个关节的角度和臂长算出L2末端的坐标，这叫正解
运动学逆解（Inverse kinematics）
通过末端的坐标，计算出各个关节的角度，这叫逆解
可以通过几何分析来进行FK和IK的解算（当然解算的方式有很多种，比如DH法等等，这里用简单的几何法讲解）
我们这里只用到了运动学逆解，简而言之就是输入目标位置的坐标，然后逆解转换成角度值，然后将角度值转换成步进电机的电流信号。驱动电机转动，机械臂就实现了从起始点到目标点的转动。
几何分析
将机械臂看做两臂在二维平面中，分析可以看到：
将竖直向上的轴看作Z轴的正半轴，向右的方向看作Y轴的正半轴
分析清楚了以后再加上旋转底座，来改变X轴的坐标；此时在Y-O-Z平面中仅rrot发生了改变，别的值是没有发生改变的哦
我们从俯视图上看到机械臂在XOY平面的运动情况
注意，机械臂的坐标是末端执行器相对于原点而言的。
代码中用到的几何算法如下（运动学逆解）：
void RobotGeometry::calculateGrad() {
float rrot_ee = hypot(xmm, ymm); //hypot函数是取三角形斜边的函数（在几何模型里面有的）
float rrot = rrot_ee - ee_offset; //这里的xmm，ymm以这个末端执行器中心为标准的，所以结算的时候要减去ee_offset，得到上臂轴承到原点的距离
float rside = hypot(rrot, zmm); //得到三维空间中原点到上臂轴承的最短距离
//sq（）为math.h里面的函数，功能是计算一个数的平方
//这里得到构建的以rside，low，high为三边的三角形的边长平方，方便直接带入使用余弦定理
float rside_2 = sq(rside);
float low_2 = sq(low_shank_length);
float high_2 = sq(high_shank_length);
rot = asin(xmm / rrot_ee);//反三角函数得到转盘与y轴的夹角
high = PI - acos((low_2 + high_2 - rside_2) / (2 * low_shank_length * high_shank_length));
//求讲义里面的角2
//Angle of Lower Stepper Motor (asin()=Angle To Gripper)
if (zmm > 0) {
low = acos(zmm / rside) - acos((low_2 - high_2 + rside_2) / (2 * low_shank_length * rside));
//讲义里面角1的余角
} else {
low = PI - asin(rrot / rside) - acos((low_2 - high_2 + rside_2) / (2 * low_shank_length * rside));
//在z小于0的时候，求解与z大于0的方法相同，只是要保证acos(zmm / rside)的zmm大于0，这里用的正弦的反三角函数，因为rrot永远为正
}
high = high + low;//得到的是小臂与大臂小臂关节处的垂直线右上方的夹角
}
最后对应构建好的坐标系，大概是这个样子的：（注意一点是，坐标原点是齿轮中心；而非底座）
归位操作
现在已经基本掌握了坐标如何准换成角度，那么只要我们将机械臂移动到对应的位置，然后开机（接通电源），并用 G92 指令设置当前的位置，坐标系就被我们构建好了，随后机械臂就能按照我们的意愿，在我们建立的坐标系里面移动了。
但是问题是，我们手动设定的位置是否精准，在笛卡尔坐标系里面，测量末端执行器到大齿轮中心的位置，实在是不怎么方便，零件有一定厚度，有些位置无法测量，，，而且如果你每次使用都需要测量的话，那么实在是太麻烦了。所以就有G28归位这个指令，将初始的rot，low，high三个角都定义好。然后只测此时的坐标即可。在每个轴的末端我们用一个限位开关来标记，当大臂或小臂以及转盘碰到限位开关以后，限位开关电平发生变化（这个在config中可以根据自己的需求设定限位开关触发以后的逻辑值是0还是1），关于电平的定义，如下图所示，在 uno板的资料中也有提到；
同时设计出限位开关转动到指定位置的角度数随后转化为步进电机的步数，驱动步进电机运转即可。
在主函数的 loop 可以看到作者为 uno 写的专用的归位函数 homeSequence_UNO：
void homeSequence_UNO(){
Logger::logINFO("PLEASE WAIT A SEC");
if (HOME_Y_STEPPER && HOME_X_STEPPER){
while (!endstopY.state() || !endstopX.state()){
endstopY.oneStepToEndstop(INVERSE_Y_STEPPER);
endstopX.oneStepToEndstop(INVERSE_X_STEPPER);
}
endstopY.homeOffset(INVERSE_Y_STEPPER);
endstopX.homeOffset(INVERSE_X_STEPPER);
} else {
setStepperEnable(true);
endstopY.homeOffset(!INVERSE_Y_STEPPER);
endstopX.homeOffset(!INVERSE_X_STEPPER);
}
if (HOME_Z_STEPPER){
endstopZ.home(!INVERSE_Z_STEPPER); //INDICATE STEPPER HOMING DIRECDTION//
}
interpolator.setInterpolation(INITIAL_X, INITIAL_Y, INITIAL_Z, INITIAL_E0, INITIAL_X, INITIAL_Y, INITIAL_Z, INITIAL_E0);
//将归位后的位置信息传入插点函数中，初始末尾位置一致，
Logger::logINFO("HOMING COMPLETE");
}
这里我之前尝试进行修改将大臂小臂一个一个进行归位，但是会出现 BUG 不行，原因是在某些特定的位置，不管是大臂先归位还是小臂先归位，都会卡住。
所以为了避免大臂小臂在特殊位置卡住所以这里选择两者一起复位的方式，注意，由于结构特点即使先复位小臂，大臂在某些特殊位置还是会因为连杆卡住复位；例如先复位小臂，但是大臂小臂之间的夹角大于135度的时候；
另外，传入的参数 INVERSE_Y_STEPPER 为逻辑值：1或0，控制归位时候的电机转动方向；开源代码直接烧入的话，电机转动方向和我们期望的方向相反（至少我烧入的时候是这样）；所以就需要更改，通过加减非：！符号进行调试；
while (!endstopY.state() || !endstopX.state()){
endstopY.oneStepToEndstop(INVERSE_Y_STEPPER);
endstopX.oneStepToEndstop(INVERSE_X_STEPPER);
}
homeSequence_UNO 这个函数大概做了四件事，这里我们一个一个来说：
1，将转轴运动到限位开关处
2，从限位开关位置为起点，运动config文件中定义的指定步数，到达初始位置
3，将初始位置的坐标作为起点以及目标点传入插补函数（setInterpolation），相当于设定归位后姿态的坐标
4，串口返回"HOMING COMPLETE"，表示已经执行了上述操作。
其中的 INITIAL_X , INITIAL_Y , INITIAL_Z，也是在config文件中提前定义好的，图示如下：
INITIAL_Y是小臂的长度加上轴承到末端执行器的长度。
值得一提的是，源文件主函数并没有用 RampsStepper.c 文件里面的电机驱动函数。而是独立创建了一个 endstop.c 文件来执行操作。如果使用 RampsStepper 函数，需要将stepperStepTargetPosition 与 stepperStepPosition 进行比较得到差值，才能驱动电机进行转动，用于后半段替代 .homeOffset的功能是完全可行的。同时需要测量来设置转动一个较大的角度来替代oneStepToEndstop 实现到达限位开关，实现前半段的功能。.oneStepToEndstop这个操作的要求是向限位开关的方向一直转动且每转一步就对限位开关进行扫描，判断开关的电平是否发生改变。在主函数loop中用写一个if（）判断语句，时刻检测限位开关的电平是否变化，后控制RampsStepper中的电机也行；理论都是可行的，可能设置有些复杂；
但是需要注意的一点是：RampsStepper中的update()函数并没有延时功能，在endstop中的三个函数每控制电机转动一步就会有delayMicroseconds(home_dwell);延时1000us=1ms来约束电机转动。所以RampsStepper没有延时，可能会过快的驱动电机导致损坏。
与此同时，我想用endstop.c来进行一个整体的封装使的结构清晰。让RampsStepper.c中的函数仅仅配合插补函数控制电机向着规定路径，速度，加速度线性移动。而互不干扰，
因为没有调用RampsStepper.c，为了保持角度坐标一致，将归位的角度值也设置到RampsStepper中的目标与实际角度（即当前角度）中去，
stepperHigher.setPositionRad(PI / 2.0); // 90°大臂与小臂角度
stepperLower.setPositionRad(0); // 0°大臂与Z轴
stepperRotate.setPositionRad(0); // 0°转盘
在homeSequence_UNO函数里面你也看到有.home，.homeOffset，.oneStepToEndstop这三个函数，来实现，节选如下。
void Endstop::home(bool dir) {//包含的有回位和复位2个功能
digitalWrite(en_pin, LOW);
delayMicroseconds(5);
if (dir==1){
digitalWrite(dir_pin, HIGH);
} else {
digitalWrite(dir_pin, LOW);
}
delayMicroseconds(5);
bState = !(digitalRead(min_pin) ^ switch_input);
while (!bState) {
digitalWrite(step_pin, HIGH);
digitalWrite(step_pin, LOW);
delayMicroseconds(home_dwell);
bState = !(digitalRead(min_pin) ^ switch_input);
}
homeOffset(dir);
}
void Endstop::homeOffset(bool dir){
if (dir==1){
digitalWrite(dir_pin, LOW);
}
else{
digitalWrite(dir_pin, HIGH);
}
delayMicroseconds(5);
for (int i = 1; i <= step_offset; i++) {//循环走step_offset步，传入的参数是在config里面定义好的X，Y，Z_HOME_STEPS这些
digitalWrite(step_pin, HIGH);
digitalWrite(step_pin, LOW);
delayMicroseconds(home_dwell); //Home_Dwell也是在config里面定义好的参数，用作延时us相当于控制电机转速
}
}
void Endstop::oneStepToEndstop(bool dir){//函数的作用仅仅是向endstop运动，并检测endstop的状态，不能控制到达后停止
digitalWrite(en_pin, LOW);
delayMicroseconds(5);
if (dir==1){
digitalWrite(dir_pin, HIGH);
} else {
digitalWrite(dir_pin, LOW);
}
delayMicroseconds(5);
bState = !(digitalRead(min_pin) ^ switch_input);
if (!bState) {
digitalWrite(step_pin, HIGH);
digitalWrite(step_pin, LOW);
delayMicroseconds(home_dwell);
}
bState = !(digitalRead(min_pin) ^ switch_input);
}
电机运动
既然已经说道了电机运动，就一并说一说RampsStepper的update函数：
核心内容就是stepperStepTargetPosition与stepperStepPosition的设定，你可以理解为起始位置和目标位置之间的夹角是两条射线绕同一个点的角度。然后经过变换将两个位置之间的角度差变成步进数差。通过判断stepperStepTargetPosition与stepperStepPosition的相对大小来确定转动方向，通过stepperStepTargetPosition与stepperStepPosition的差值得到转动步数。
细心的你发现，为什么这里不像endstop函数里面一样有延时函数，来控制步进电机的速度，不要着急，在下面说道插点函数（路径规划）的时候会讲到。
void RampsStepper::update() {
while (stepperStepTargetPosition < stepperStepPosition) {
digitalWrite(dirPin, inverse);//设置坐标判断后的转动方向（可以改变XYZ轴的正方方向，改了以后要将最大限一并修改）
digitalWrite(stepPin, HIGH);//通过交替输出高低电平驱动电机一步一步的运动
digitalWrite(stepPin, LOW);
stepperStepPosition--;
}
while (stepperStepTargetPosition > stepperStepPosition) {
digitalWrite(dirPin, !inverse);//设置坐标判断后的转动方向（可以改变XYZ轴的正方方向，改了以后要将最大限一并修改）
digitalWrite(stepPin, HIGH);
digitalWrite(stepPin, LOW);
stepperStepPosition++;
}
}
将两个位置之间的角度差变成步进数差，还需要涉及到步进电机导轮与传动齿轮之间的换算；
RampsStepper函数开头就设置了radToStepFactor（弧度制角度–>步进步数的转化系数）
在主函数中传入参数：
setReductionRatio(MAIN_GEAR_TEETH / MOTOR_GEAR_TEETH, MICROSTEPS * STEPS_PER_REV);
void RampsStepper::setReductionRatio(float gearRatio, int stepsPerRev) {//
radToStepFactor = gearRatio * stepsPerRev / 2 / PI;
};
//将（2*PI）换成（360.0）可以把弧度制改为角度制
void RampsStepper::stepToPositionRad(float rad) {//输入角度（绝对），得到目标位置的步进位置
stepperStepTargetPosition = rad * radToStepFactor;
}
第一个参数：
gearRatio=MAIN_GEAR_TEETH / MOTOR_GEAR_TEETH；MAIN_GEAR_TEETH 是主要齿轮的齿数，MOTOR_GEAR_TEETH是电机导轮的齿数。
在config文件有对MAIN_GEAR_TEETH 与 MOTOR_GEAR_TEETH的初始化，分别是90.0与20.0，取决于实际的3d打印模型
那么gearRatio的含义就很容易理解了，即让大轮转动一圈，电机导轮需要转动的圈数。
第二个参数：
stepsPerRev=MICROSTEPS * STEPS_PER_REV ；MICROSTEPS是步进模式的设置，STEPS_PER_REV是每转所要走的步数。
在config文件中将STEPS_PER_REV设置为200，每转1圈需要200步，这样算下来每一步也就是1.8度。MICROSTEPS设置的是1/16，说白了也就是将原来的一步分成16步来走。（注意驱动模块的引脚设置）这样一来，每转1圈需要16*200步，也就是一步0.1125度。
因为设置的rad参数是弧度制，所以在最后除以（2*PI）
这样一来就实现了，弧度制下的角度到步进电机驱动大轮转动步数之间的转换；
模式选择
刚刚的 RampsStepper 只是一个小插曲；
接着刚刚说道的，用G28归位完成了以后，就可以进行线性移动的操作了。在主函数里面，你大概可以看到这样一条函数，处理G0/G1指令的switch，case语句
if (cmd.id == 'G') {
switch (cmd.num) {
case 0:
case 1:
fan.enable(true);
Point posoffset;
posoffset = interpolator.getPosOffset();//传入绝对模式下的原点坐标(初始化默认的PosOffset都为0。)
cmdMove(cmd, interpolator.getPosmm(), posoffset, command.isRelativeCoord);//绝对相对模式的分类，并输出目标位置坐标
interpolator.setInterpolation(cmd.valueX, cmd.valueY, cmd.valueZ, cmd.valueE, cmd.valueF);//输入目标值，进行插点计算，循环操作。
Logger::logINFO("LINEAR MOVE: X" + String(cmd.valueX-posoffset.xmm) + " Y" + String(cmd.valueY-posoffset.ymm) + " Z" + String(cmd.valueZ-posoffset.zmm) + " E" + String(cmd.valueE-posoffset.emm));
break;
我们一句一句看，
posoffset = interpolator.getPosOffset();
是传入绝对模式下的原点坐标(初始化默认的PosOffset都为0)，简而言之就是机械臂相对于哪个点建立坐标系；在最后的串口交互里面看的很清楚最后要减去-posoffset.x，y，zmm，表示的就是相对于这些点进行的线性移动。
Logger::logINFO("LINEAR MOVE: X" + String(cmd.valueX-posoffset.xmm) + " Y" + String(cmd.valueY-posoffset.ymm) + " Z" + String(cmd.valueZ-posoffset.zmm) + " E" + String(cmd.valueE-posoffset.emm));
其次：
cmdMove这个函数在command.c中，作用就是将输入的坐标指令进行处理，判断设置的是相对模式还是绝对模式，并对输入的数据进行处理。用isnan()函数用于判断数据是否为NAN，这是因为在每次串口接收数据的时候，都会将cmd.valueX，Y，Z置为NAN，这样，如果对应的方向没有新的指令，那么依旧是原来的坐标位置。这里用的？：：条件运算符，类似于if（）判断语句。
cmdMove(cmd, interpolator.getPosmm(), posoffset, command.isRelativeCoord);
void cmdMove(Cmd(&cmd), Point pos, Point pos_offset, bool isRelativeCoord){
if(isRelativeCoord == true){//相对模式
cmd.valueX = isnan(cmd.valueX) ? pos.xmm : cmd.valueX + pos.xmm;//isnan()函数用于判断数据是否为NAN
cmd.valueY = isnan(cmd.valueY) ? pos.ymm : cmd.valueY + pos.ymm;
cmd.valueZ = isnan(cmd.valueZ) ? pos.zmm : cmd.valueZ + pos.zmm;
cmd.valueE = isnan(cmd.valueE) ? pos.emm : cmd.valueE + pos.emm;
} else {
cmd.valueX = isnan(cmd.valueX) ? pos.xmm : cmd.valueX + pos_offset.xmm;//绝对模式是相对于pos_offset的，即相对于x=0，y=0，z=0
cmd.valueY = isnan(cmd.valueY) ? pos.ymm : cmd.valueY + pos_offset.ymm;
cmd.valueZ = isnan(cmd.valueZ) ? pos.zmm : cmd.valueZ + pos_offset.zmm;
cmd.valueE = isnan(cmd.valueE) ? pos.emm : cmd.valueE + pos_offset.emm;
}
}
最后：
interpolator.setInterpolation(cmd.valueX, cmd.valueY, cmd.valueZ, cmd.valueE, cmd.valueF);//输入目标值，进行插点计算，循环操作。
插补算法
在interpolator文件中原作者用了很多.setInterpolation的同名函数，使用的时候C++会根据函数参数个数的不同进行一一配对。所以不用担心错用
//G0，G1的操作
void Interpolation::setInterpolation(float px, float py, float pz, float pe, float v) {
Point p;
p.xmm = px;
p.ymm = py;
p.zmm = pz;
p.emm = pe;
setInterpolation(p, v);
}
void Interpolation::setInterpolation(Point p1, float v) {
Point p0;
p0.xmm = xStartmm + xDelta;
p0.ymm = yStartmm + yDelta;
p0.zmm = zStartmm + zDelta;
p0.emm = eStartmm + eDelta;
setInterpolation(p0, p1, v);
}
void Interpolation::setInterpolation(Point p0, Point p1, float av) {
v = av; //mm/s
float a = (p1.xmm - p0.xmm);
float b = (p1.ymm - p0.ymm);
float c = (p1.zmm - p0.zmm);
float e = abs(p1.emm - p0.emm);
float dist = sqrt(a*a + b*b + c*c);//计算三维坐标系下的两点距离
if (dist < e) {
dist = e;
}
if (v < 5) { //includes 0 = default value//首次传入的参数v是0
v = sqrt(dist) * 10; //set a good value for v//这里是取决于这个目标位置与
}
if (v < 5) {
v = 5;
}
tmul = v / dist;// 得到用时的倒数：1/t，从而限定(t*tmul)的范围在（0,1）之间
xStartmm = p0.xmm;//设置起点位置
yStartmm = p0.ymm;
zStartmm = p0.zmm;
eStartmm = p0.emm;
xDelta = (p1.xmm - p0.xmm);//设置增量
yDelta = (p1.ymm - p0.ymm);
zDelta = (p1.zmm - p0.zmm);
eDelta = (p1.emm - p0.emm);
state = 0;//表示状态正常
startTime = micros();//标记当前的时间节点
}
Interpolation::setInterpolation(Point p0, Point p1, float av)本语句的核心，主要做了怎么几件事：
1，将初始坐标分别设置为x，y，zStartmm
2，将目标坐标与初始坐标分别相减，得到对应的过程值，x，y，zDelta
3，计算在三维空间中，起始坐标到终点坐标的直线距离dist，并设置一个合理的速度sqrt(dist) * 10
4，计算1/t，从而限定后面用到的(t*tmul)的范围在（0,1）之间
5，记录此时的时间，为运动开始的时间。
至此，switch中的指令执行完成，但是此时线性运动并没有完成：回到主函数中的loop函数中，这些语句将会循环执行。下面我们一句一句来看；
interpolator.updateActualPosition();//获得实时位置（获得当前插点坐标的值）
geometry.set(interpolator.getXPosmm(), interpolator.getYPosmm(), interpolator.getZPosmm());//获取XYZ的位置，后进行解算，得到相应的角度
stepperRotate.stepToPositionRad(geometry.getRotRad());//解算获取转盘与Y轴正半轴的夹角
stepperLower.stepToPositionRad(geometry.getLowRad());//解算获取下臂与Z轴正半轴夹角
stepperHigher.stepToPositionRad(geometry.getHighRad());//解算获取上臂夹角信息
#if RAIL
stepperRail.stepToPositionMM(interpolator.getEPosmm(), STEPS_PER_MM_RAIL);
#endif
stepperRotate.update();//转动转盘
stepperLower.update();//转动下臂
stepperHigher.update();//转动上臂
什么是插补？
由于我们用的是步进电机，运动的时候是一步一步的，并不是平滑的，这里假设在一个平面上面，一个x步进电机，一个y步进电机。那么放大它们所走的轨迹就会是阶梯状的。而它们也只能沿着网格运动。相当于有了一个最小的分辨率，这个分辨率就是一步。如下图所示，如果x，y电机同时走就能走出45度的斜线。在三维空间里面运动也是如此，你应该可以想象的出来；
首先，是updateActualPosition，用来时时更新此时的差补位置，得到的是一个路径与时间 t 的函数（其中路径的范围为0，1，时间范围为0，dist/v）
void Interpolation::updateActualPosition() {
if (state != 0) {
return;
}
long microsek = micros();//执行到这一步时候的时间节点
float t = (microsek - startTime) / 1000000.0; //从下发命令到此次插点运算的时间
float progress;
switch (SPEED_PROFILE){
// FLAT SPEED CURVE
case 0:
progress = t * tmul;//注意！！！这里的t已经除以1000000.0，所以progress可以认为是一个小量
if (progress >= 1.0){
progress = 1.0;
state = 1;//表示状态出错
}
break;
// ARCTAN APPROX
case 1:
progress = atan((PI * t * tmul) - (PI * 0.5)) * 0.5 + 0.5;//case 1:case 2:改变的都是在不同时间process的大小，从而实现在不同时间节点，运动的快慢不同，具体的变化趋势要视不同的函数而定
if (progress >= 1.0) {//看清楚这里是progress哦，不是t
progress = 1.0;
state = 1;
}
break;
// COSIN APPROX
case 2:
progress = -cos(t * tmul * PI) * 0.5 + 0.5;
if ((t * tmul) >= 1.0) {
progress = 1.0;
state = 1;
}
break;
}
这里可能会用到机械臂路径规划的问题：
有这么几种术语：
插补函数
显然呢switch (SPEED_PROFILE)这个函数呢就是用来选择机械臂移动的时候的路径规划情况：
case0为绿色，case1位蓝色，case2为红色；
因为之前提到过变量 tmul 将函数的范围限制在（0,1）之间，而且 tmul 函数中已经包含了运动时间的信息，所以t*tmul就是一个关于运动时间的函数
将3个函数分别求导，那么就得到了下面这幅图：同样范围也是（0,1）之间
可以看到，我们所求的导数与开源中机械臂在运动过程中的加速度示意图保持一致；
接下来的部分：
pos_tracker[X_AXIS] = xStartmm + progress * xDelta;//设置当前追踪位置
pos_tracker[Y_AXIS] = yStartmm + progress * yDelta;
pos_tracker[Z_AXIS] = zStartmm + progress * zDelta;
pos_tracker[E_AXIS] = eStartmm + progress * eDelta;
pos_tracker[X_AXIS] = xStartmm + progress * xDelta;
tracker，这个变量起的名字也比较贴切—追踪器；最后就是机械臂在一个一个插补后的坐标之间连续运动，实现从起始位置到目标位置。因为我们先前已经将progress限制在（0,1]，所以最后progress=1的时候，也就到达了目标位置。
限位判断
然后就涉及到了一个是否可以到达目标位置的判断，之前说过SCARA机械臂由于自由度有限，所以不是空间中的每一个位置都可以到达。
其次，存在一种情况就是，起始坐标和终止坐标都可以到达，但是在运行的过程中，有一些位置是不能到达的，那么这种情况单独靠判断目标位置就不行。则需要将插补的每一个坐标点都进行判断，然后再执行。如果有一点不满足，那么就会回到原始位置。
if(isAllowedPosition(pos_tracker)){//判断位置是否超出限度，超出后进行else语句，全部清零
xPosmm = pos_tracker[X_AXIS];
yPosmm = pos_tracker[Y_AXIS];
zPosmm = pos_tracker[Z_AXIS];
ePosmm = pos_tracker[E_AXIS];
} else {
pos_tracker[X_AXIS] = xPosmm;//ERROR超过限度：新坐标依旧等于上次的位置
pos_tracker[Y_AXIS] = yPosmm;
pos_tracker[Z_AXIS] = zPosmm;
pos_tracker[E_AXIS] = ePosmm;
state = 1;
progress = 1.0;
xStartmm = xPosmm;
yStartmm = yPosmm;
zStartmm = zPosmm;
eStartmm = ePosmm;
xDelta = 0;
yDelta = 0;
zDelta = 0;
eDelta = 0;
}
}
限位判断函数：
bool Interpolation::isAllowedPosition(float pos_tracker[4]) { //LIMIT REACHED 限位判断
float rrot_ee = hypot(pos_tracker[X_AXIS], pos_tracker[Y_AXIS]);
float rrot = rrot_ee - END_EFFECTOR_OFFSET; //得到x-y平面内，轴承到原点的距离
float rrot_x = rrot * (pos_tracker[Y_AXIS] / rrot_ee); //得到得到x-y平面内，轴承的x坐标
float rrot_y = rrot * (pos_tracker[X_AXIS] / rrot_ee); //得到得到x-y平面内，轴承的y坐标
float squaredPositionModule = sq(rrot_x) + sq(rrot_y) + sq(pos_tracker[Z_AXIS]); //在空间三维坐标中，原点到大臂轴承的位置
bool retVal = (
squaredPositionModule <= sq(R_MAX) //以这个机械臂的轴承为原点，就极大程度的方便了限位。
&& squaredPositionModule >= sq(R_MIN) //这里的这个R_MAX，R_MIN就是在保证大臂和小臂齿轮不
&& pos_tracker[Z_AXIS] >= Z_MIN
&& pos_tracker[Z_AXIS] <= Z_MAX
&& pos_tracker[E_AXIS] <= RAIL_LENGTH
);
if(!retVal) {
Logger::logERROR("LIMIT REACHED: [X:" + String(pos_tracker[X_AXIS]) + " Y:" + String(pos_tracker[Y_AXIS]) + " Z:" + String(pos_tracker[Z_AXIS]) + " E:" + String(pos_tracker[E_AXIS]) + "]");
}
return retVal;//存在一个不符合条件的话，就范围逻辑0，用&&去判断的
}
在config文件中可以自己定义运动空间范围：
//MOVE LIMIT PARAMETERS //移动限制参数
#define Z_MIN -140.0 //MINIMUM Z HEIGHT OF TOOLHEAD TOUCHING GROUND //刀头接触地面的最小 Z 高度
#define Z_MAX (LOW_SHANK_LENGTH+30.0) //SHANK_LENGTH ADDING ARBITUARY NUMBER FOR Z_MAX //下臂长度+30得到最大 Z 高度
#define SHANKS_MIN_ANGLE_COS 0.791436948 //45度
#define SHANKS_MAX_ANGLE_COS -0.774944489 //135度
//R_MIN与R_MAM的求法就是以两臂为边，45或135度为定角的三角形，绕原点转动，末端轴承经过的路径可以看做是部分圆，而三角形第三边的长度分别就是R_MIN与R_MAM
#define R_MIN (sqrt((sq(LOW_SHANK_LENGTH) + sq(HIGH_SHANK_LENGTH)) - (2*LOW_SHANK_LENGTH*HIGH_SHANK_LENGTH*SHANKS_MIN_ANGLE_COS) ))//可以理解为Y轴的最大最小距离
#define R_MAX (sqrt((sq(LOW_SHANK_LENGTH) + sq(HIGH_SHANK_LENGTH)) - (2*LOW_SHANK_LENGTH*HIGH_SHANK_LENGTH*SHANKS_MAX_ANGLE_COS) ))//用余弦定理求得
最长与最短运动边界的计算方法是：在Y-O-Z平面内，大臂小臂分别成45度和135度。以原点为圆心画弧，结合Z_MAX和Z_MIN确定高度，这样基本可以得到在Y-O-Z平面内的运动范围。分析一下，这个大臂小臂所成的角度是有范围的也就是说45度和135度不是我们随便选的，因为机械结构问题，角度并不能达到每一个所期望的角度。而在可以达到的角度内，45度,135度是可以达到运动范围最大的两个极限角度值。所以我们这里将臂长作为三角形的两条边来进行限位来判断：
用CAD作图如下：
空间的轨迹形成图如下：
在Y-O-Z平面，以及X-O-Y平面内的运动范围如下所示：
运动的空间可以这么描述，即以原点为球心的部分球壳，球壳厚度为(R_MAX-R_MIN)
循环更新目标值
最后，确认坐标一切无误以后：传入RobotGeometry中进行几何结算，得到high，rot，low等角度
geometry.set(interpolator.getXPosmm(), interpolator.getYPosmm(), interpolator.getZPosmm());//获取XYZ的位置，后进行解算，得到相应的角度
再将结算出来的high，rot，low等角度带入转换成步进电机步数
stepperRotate.stepToPositionRad(geometry.getRotRad());//解算获取转盘与Y轴正半轴的夹角
stepperLower.stepToPositionRad(geometry.getLowRad());//解算获取下臂与Z轴正半轴夹角
stepperHigher.stepToPositionRad(geometry.getHighRad());//解算获取上臂夹角信息
然后驱动电机转动对应步数就行；
stepperRotate.update();//转动转盘
stepperLower.update();//转动下臂
stepperHigher.update();//转动上臂
到这里你明白了为什么RampsStepper::update()中不需要延时了吗，我们定义的float t = (microsek – startTime) / 1000000.0; t有6位小数，可以说这个插补的精确度到达单片机最好的程度了，所以在loop循环中每次只是达下一个插补坐标的位置，步进长度很小，自然不需要延时啦。
串口通讯
完成这一切操作都需要一个大前提，就是用电脑与Arduino进行串口通讯：
完成这一操作涉及到main.c，command.c，以及queue.h
在setup函数中：
Serial.begin(BAUD);//设置通讯的波特率为：115200
在loop函数中：
主要分为三个部分：
1，命令接受：
if (!queue.isFull()) {
if (command.handleGcode()) //处理Gcode，对"回车"和"换行"进行判断。然后整理接受到的数据
{
queue.push(command.getCmd());//将结构体new_command加在数列后面
}
}
2，命令处理：
if ((!queue.isEmpty()) && interpolator.isFinished()) {//如果列不为空，而且插点算法处于完成(空闲)状态的时候
executeCommand(queue.pop());//进行指令解析、选择
if (PRINT_REPLY) {
Serial.println(PRINT_REPLY_MSG);
}
}
3，状态显示（代表执行到了这一条语句，uno板中没有此led的引脚）
if (millis() % 500 < 250) {//millis()函数用于返回的Arduino板开始运行当前程序时的毫秒数。这个数字在大约50天后溢出，即回到零。
led.cmdOn();
}
else {
led.cmdOff();
}
}
先看到命令接受：在command.c函数中handleGcode();
bool Command::handleGcode() {
if (Serial.available()) {//判断串口有无数据
char c = Serial.read();//读取一个字节（1Byte）的数据，根据双重if判断可以实现连续读写的效果
if (c == '\n') {//是否为NL（换行（LF）：一个让输出设备切换到下一行的控制字符）
return false;
}
if (c == '\r') {//是否为CR(回车（CR）：是一个让输出回到该行文字开头的控制字符)
bool b = processMessage(message);
message = "";//将字符串message清空，然后准备接受下一个字符串
return b; //返回值为true或false;
}
else {
message += c; //如果收到的数据既不是'\n'也不是'\r',那么将收集到的数据加和到字符串message中 ，这时handleGcode()返回值为flase
}
}
return false;
}
这里涉及到一些控制字符的知识，主要是’\n’以及’\r’，切换到下一行并将光标移动到文字开头：
这里用CR来判断一条指令是否结束，没有结束，那么就将指令都收集到字符串message中，因为这里接受到的指令可能是大小写字母，数字，空格，这些元素，所以用字符串来收集再好不过了。
如果接受结束，那么就进入处理信息的函数processMessage(message)，返回值为是否处理完成，用于判断指令的开头是否为“G”或“M”；
指令处理加工
在command.c中，processMessage函数如下：
分为3部分来看：
1，对接受来的数据进行基本处理：将字母全部变为大写，将字符串中的空格取消
2，判定代码能否识别，并将指令存入new_command.num中；
3，将各个方向轴的数据带入value_segment中进行储存
bool Command::processMessage(String msg){
new_command.valueX = NAN; //NAN是未定义的意思
new_command.valueY = NAN;
new_command.valueZ = NAN;
new_command.valueE = NAN;
new_command.valueF = 0;
new_command.valueS = 0;
msg.toUpperCase(); //将字符串里面的字母都换成大写的
msg.replace(" ", "");//取消字符串之间的空格
int active_index = 0;
new_command.id = msg[active_index];
if((new_command.id != 'G') && (new_command.id != 'M')){
printErr();
return false;
}
active_index++;
int temp_index = active_index;//此时序数都为：1
while (temp_index<msg.length() && !isAlpha(msg[temp_index])){//isAlpha（）用于判断是否为英文字母，若是则返回1，不是的话返回0
temp_index++; //注意这里的while()是先判断后执行，所以temp_index刚好是到达下一个字母的前一位置
}
new_command.num = msg.substring(active_index, temp_index).toInt();//返回一个包含从 active_index 到最后（不包含 active_index ）的子字符串的字符串。
active_index = temp_index; //这个函数的作用主要是将G代码的序号提取出来
temp_index++;
while (temp_index<msg.length()){//循环3次，如果是有XYZ轴的话，循环4次，如果是有XYZE轴的话
while (!isAlpha(msg[temp_index]) || msg[temp_index]=='.'){//将temp_index的序号移动到下一个字母上，"||"是或符号，整个都不成立则不成立，先判断前面的
temp_index++;
if (temp_index == msg.length()){
break;
}
}
value_segment(msg.substring(active_index, temp_index));//依次将XYZ的新坐标分别储存
active_index = temp_index;
temp_index++;
}
return true;
}
随后带入value_segment中
void Command::value_segment(String msg_segment){
float msg_value = msg_segment.substring(1).toFloat();//截取索引为1以及后面的所有元素并转换成float型：例如X20中的20.000000
switch (msg_segment[0]){
case 'X': new_command.valueX = msg_value; break;
case 'Y': new_command.valueY = msg_value; break;
case 'Z': new_command.valueZ = msg_value; break;
case 'E': new_command.valueE = msg_value; break;
case 'F': new_command.valueF = msg_value; break;
case 'S': new_command.valueS = msg_value; break;
}
}
至此一次基本的数据处理就完成了，但是我们不可能仅仅发送一次数据是吗，为了避免一次性发送很多数据在Arduino的串口缓存区丢失，我们需要将标准化的指令（经过processMessage函数处理过的信息）进行储存。
如果单看一次处理的话，将指令储存后又将指令取出实在是小题大做，但是插补算法以及电机驱动都是需要一定时间来实现的，为了保证机械臂运行的连贯以及自动性。这个做法实有必要。
有个小问题就是你可能会在想，之前储存在new_command中的变量怎么到cmd中了。那是因为在command.h文件中有如下定义
class Command {
public:
Command();
bool handleGcode();
bool processMessage(String msg);
void value_segment(String msg_segment);
Cmd getCmd() const;
void cmdGetPosition(Point pos, Point pos_offset, float highRad, float lowRad, float rotRad);
void cmdToRelative();
void cmdToAbsolute();
bool isRelativeCoord;
Cmd new_command;//这里将new_command定义为与Cmd一致的结构体
private:
String message;
};
回到我们的第一步：
if (!queue.isFull()) {
if (command.handleGcode()) //处理Gcode，对"回车"和"换行"进行判断。然后整理接受到的数据
{
queue.push(command.getCmd());//将结构体new_command加在数列后面
}
}
command.getCmd()返回值为结构体new_command
template <typename Element>
bool Queue<Element>::push(Element elem) { //push() 在队尾插入一个元素
data[(start + count++) % len] = elem; //求余运算，如果被除数小于除数，那么返回值依旧是被除数
}
如果是第一次，那么很显然将结构体new_command储存到data[0]中去。len是在config文件中定义的，用于确认数据数列最大长度。
然后进入第二步
if ((!queue.isEmpty()) && interpolator.isFinished()) {//如果列不为空，而且插点算法处于完成(空闲)状态的时候
executeCommand(queue.pop());//进行指令解析、选择
if (PRINT_REPLY) {
Serial.println(PRINT_REPLY_MSG);
}
}
因为与第一步并不是if（）的嵌套结构，为了避免初始的时候第一步未完成（没有数据传入data）而进行了第二步的指令解析，所以在这里需要判断data中是否有数据；
template <typename Element>//判断空间是否为空
bool Queue<Element>::isEmpty() const {
return count <= 0;
}
其次如果上次的插补还么有完成，那么必然是不能进行下一次的指令解析的。
bool Interpolation::isFinished() const {
return state != 0;
}
随后在主函数的executeCommand就是主函数中那个大的switch，case语句中进行判断cmd.id，cmd.num
需要用到queue中的pop函数来弹出start位置处的元素，总的元素数量减一，将后面一个位置作为start点。
又因为有 % len 所以每隔 len 个数据就会进行一次覆盖，保证可以持续发送指令；
template <typename Element>
Element Queue<Element>::pop() { //pop() 弹出队列start位置处的元素，总的元素数量减一，将后面一个位置作为start点
count--;
int s = start;
start = (start + 1) % len;
return data[(s) % len];
}
但是同时也有一个问题就是，最好不要一次性发送超过 len 个数据，假设在这期间Arduino只接受不处理数据，那么序数为0的数据就会被序数为（len+1）的数据覆盖。造成数据丢失。
可以适当在config文件中增 len（QUEUE_SIZE）的数值大小，来确保队列可以一次性接受更多的数据。
至此，从串口传入机械臂的指令通过解析，计算，实现操作才算真正完成；
此外还有fancontrol.c与logger.c分别用于控制风扇的打开，延时关闭；和串口监视器的信息交互；
末端执行器的拓展
最后补充一点的是末端执行器的部分；
常见的拓展就是28yj步进电机的夹持器，或者SG90舵机的夹持器，真空吸盘，以及激光雕刻的末端执行器都是没问题的。前提是了解驱动的方式。
以及将28yj步进电机拓展为第四轴，控制末端执行器旋转，然后SG90舵机作为加持器的操作；
官方源码里面有用28yj步进电机来控制夹持器的，或者用SG90舵机来控制夹持器
以舵机为例：
设置好信号线，以及抓取角度，在每次操作完以后及时将线程分离
#include "servo_gripper.h"
#include <Arduino.h>
#include <Servo.h>
Servo_Gripper::Servo_Gripper(int pin, float grip_degree, float ungrip_degree){//之前在config文件中定义的12号引脚为舵机引脚
servo_pin = pin;
servo_grip_deg = grip_degree;
servo_ungrip_deg = ungrip_degree;
Servo servo_motor;
}
void Servo_Gripper::cmdOn(){
servo_motor.attach(servo_pin);
servo_motor.write(servo_grip_deg);//抓取设置角度：90.0
delay(300);
servo_motor.detach();//线程分离
}
void Servo_Gripper::cmdOff(){
servo_motor.attach(servo_pin);
servo_motor.write(servo_ungrip_deg);//放开设置角度：00.0
delay(300);
servo_motor.detach();
}
除此之外，源码还预留了一个equipment函数，方便我们拓展别的设备，函数已经封装好了，只需要在pinout中定义设备引脚就行；
#include "equipment.h"
#include <Arduino.h>
Equipment::Equipment(int equipment_pin){//输入设备端口
pin = equipment_pin;
pinMode(pin, OUTPUT);//将端口设置为输出模式
}
void Equipment::cmdOn(){
digitalWrite(pin, HIGH);//指令开启，将端口设置为高电平
}
void Equipment::cmdOff(){
digitalWrite(pin, LOW);//指令关闭，将端口设置为低电平
}
源码涉及到的子文件比较多，所以难免会解释的有些模糊，建议还是多看几遍源码，并结合自己的理解来；
