Arduino 智能学习简易版 机械臂

一 总体思路

1 功能原理

• 实现功能：使用电位器控制每一个舵机运动，进而操作机械臂；设置学习模式，打开后可以记录手动操作的路径，并且能复位执行记录好的路径，能够路径信息打印出来调用。
• 原理：电位器操纵舵机，使用Servo库自带的map映射，实现控制效果非常容易；
  学习模式：本项目设计了一种方法，记录每个舵机一次运动中的开始角度与结束角度，加上舵机编号存入一个结构体数组中，具体实现详见代码部分。这种方法的优势在于，节省空间，支持机械臂复杂操作，可以流畅复位，理论上结构体数组范围可以开多大，就可以支持多少次的舵机运动，代码部分详细介绍，该方法仅涉及c/c++基本语法结构。

简单说3种模式：1 纯电位器操控，2 学习模式，可记录操作的路径，3 复位模式，不可操作，用来执行记录的路径。

2 硬件准备

• NANO版
• SG90伺服舵机
• 触控板
• 电位器
• 3d打印机械臂结构（某宝）
• 三色灯板

3 Arduino库

• 仅需要Servo库，其余为简单代码

本项目除了记录舵机路径的方法之外，其余部分皆为基础。逻辑清晰，有c++基础的小白也比较容易实现。

---

二 代码设计

源代码较长，该部分分块进行介绍：舵机，电位器，路径记录法，触控板与三色灯板。

1 舵机部分

该部分主要包含：舵机复位方法init_pos，一个固定的抓取路径catchs(度数)，和一些舵机基础全局变量。该部分为基础设定。此处有疑问可以参考我另一篇颜色识别机械臂博客中，有详细介绍。

再次强调：实现机械臂的前提一定先搞清楚自己手里舵机的角度范围和旋转方向，舵机参数都了解清楚后，才开始编码，这是最基础也是最重要的一步。本项目舵机引脚与角度范围值仅做参考，以自己实际情况为主。

#include <Servo.h>

//1 舵机
Servo myservo1,myservo2,myservo3,myservo4;
int pos1 = 0;  //每个舵机的角度值，12345分别对应：底座，大臂，小臂，尖端，爪子舵机
int pos2 = 0;
int pos3 = 130; 
int pos4 = 80;
int speed = 7;  //机械臂运动速度
//setup里记得写连接舵机初始引脚，还有write初始角度pos变量

/*
读取舵机角度参数，控制舵机流畅的回到初始位置
*/
void init_pos(){
  pos1=myservo1.read();  //当前各舵机角度值
  pos2=myservo2.read();
  pos3=myservo3.read();
  pos4=myservo4.read();

  for(int i=pos1;i>=0;i--){  //1号舵机，归位0
    myservo1.write(i);
    delay(speed);
  }myservo1.write(0);

  for(int i=pos3;i<=130;i++){    //3号舵机，归位130
    myservo3.write(i);
    delay(speed);
  }myservo3.write(130);

  for(int i=pos2;i>=0;i--){   //2号舵机，归位0
    myservo2.write(i);
    delay(speed);
  }myservo2.write(0);
  
  for(int i=pos4;i<=80;i++){  //4号舵机，归位80°
    myservo4.write(i);
    delay(speed);
  }myservo4.write(80);

  pos1=0;//更新pos
  pos2=0;
  pos3=130;
  pos4=80;

}

/*
一个固定路径的机械臂抓取函数，可设置旋转度数spin。
我把这个设置成了开机动画，开机一动不动像木头一样太无趣了
*/
void catchs(int spin) {
  
 //先下放小臂
 for (pos3 = 130; pos3 >= 30; pos3 -= 1) {
    myservo3.write(pos3);
    delay(speed);
 }
  //打开爪子
 for (pos4 = 80; pos4 >= 0; pos4 -= 1) {
    myservo4.write(pos4);
    delay(speed);
 }
 //放大臂
 for (pos2 = 0; pos2 <= 70; pos2 += 1) {
    myservo2.write(pos2);
    delay(speed);
 }
 //夹取物品
 for (pos4 = 0; pos4 <= 80; pos4 += 1) {
    myservo4.write(pos4);
    delay(speed);
 }
   //抬起大臂
 for (pos2 = 70; pos2 >= 0; pos2 -= 1) {
    myservo2.write(pos2);
    delay(speed);
 }
  //转向
  for(int i=0;i<spin;i++){   //转动spin度，spin不同颜色可以手动设置转动区间
    myservo1.write(pos1++);
    delay(speed);
  } 
 //大臂放下
 for (pos2 = 0; pos2 <= 15; pos2 += 1) {
    myservo2.write(pos2);
    delay(speed);
 }
 //打开爪子
 for (pos4 = 80; pos4 >= 0; pos4 -= 1) {
    myservo4.write(pos4);
    delay(speed);
 }
  init_pos();//机械臂流畅复位
}

2 电位器

加入电位器的目的是，可以通过手操电位器来控制舵机，也就是控制机械臂关节移动的效果。本项目用了4个SG90伺服舵机，故使用4个电位器操纵。在Arduino下载Servo库后，可以查看knob的示例文件，学习map映射操作舵机的方法。本处代码为，电位器控舵机部分，该部分很简单。

//2 电位器
int potpin1=A0; //potentiometer pin:电位器引脚
int potpin2=A1; //这里开个变量存引脚，是为了代码容易阅读
int potpin3=A2; //若直接写入具体引脚，引脚多了后，鬼知道这些引脚是干嘛用的
int potpin4=A3;
int val1,val2,val3,val4;   //存取电位器模拟引脚的数值
//使用电位器，setup中不需要写额外内容

/*
重复判定是否操纵了电位器
*/
void loop(){

  //电位器底座控制与信息记录
  val1=analogRead(potpin1);  //读取模拟引脚值
  val1=map(val1,0,1023,0,120);//模拟值映射为舵机角度值，[0,1023]到[0,180]
  myservo1.write(val1);      //根据自己实际情况设映射范围

  
  //电位器大臂控制与信息记录
  val2=analogRead(potpin2);
  val2=map(val2,0,1023,0,70);
  myservo2.write(val2);


  //电位器小臂控制与信息记录
  val3=analogRead(potpin3);
  val3=map(val3,0,1023,30,130);
  myservo3.write(val3);

  //电位器爪子控制与信息记录
  val4=analogRead(potpin4);
  val4=map(val4,0,1023,0,80);
  myservo4.write(val4);


  //测试每个舵机实时角度，可观察电位器操作引起的舵机角度变化
  // delay(15);   
  // Serial.print("val1:\t");
  // Serial.print(val1);
  // Serial.print("\tval2:\t");
  // Serial.print(val2);
  // Serial.print("\tval3\t");
  // Serial.print(val3);
  // Serial.print("\tval4:\t");
  // Serial.println(val4);
  
}

3 记录路径法

（1）算法设计思路：

问题	解决方案
实现功能	记录一次完整操作中，每一个舵机运动的初始角度，和结束角度。按时间顺序存到结构体数组中，遍历输出一遍即可
记录方式	结构体数组，每个结构体元素含：舵机编号、初始角度、结束角度3个变量
方法限制	同一刻，只有一个舵机被操控
优势	一个舵机一次运动只记录2个数值，大大节省空间，对于复杂的运动操作，记录路径大大简化
缺点	同一舵机，如果初始角度与结束角度相同，会被算法优化掉这次运动。（并非100%还原，有待改进）

（2）实现原理

设计思路很直白，但问题在于：
怎么精准的记录每个舵机运动的开始和结束的角度？通过什么判断一次运动的开始与结束？
通过两种标记来实现，实现如下：

• 1 设一个变量con实时记录这一刻哪一个舵机正在被操控，con赋值为舵机的编号。
• 2 设X个变量fctX实时记录：X舵机是否处于连续运动的状态,只要真假2种状态
• 3 当a舵机第一次被操纵时，记录下a舵机开始角度；
• 4 当b舵机舵机被操纵时，记录a舵机的结束角度；
• 5 数组编号递增，记录b舵机的开始角度，进入下一次记录过程
• 不断通过con与fct的值来重复3 - 5这一过程

开始一次记录：以1号舵机为例：

步骤
舵机开始运动角度	当1号舵机角度发生变化时，如果fct1=0时，表示现在1号舵机被操纵，并且1号舵机之前还未被操纵，说明这是1号舵机的初始角度，此时把1号舵机现在的角度计入结构体数组中，做初始角度即可
持续操纵	当1号舵机角度发生变化，但是fct1为真时，表明1号舵机在被持续操纵，不需要记录，及时更新舵机角度就行
舵机运动结束	当检测1号舵机的舵机角度值没有变化时，此时有两中可能，一是我们没有进行任何电位器操作，不做处理
	二是我们控制了1号舵机以外的舵机。这就表示1号舵机这一次运动已经结束了，需要将结束角度记录道结构体中去。故此方法是依据其它舵机被控来记录上一个舵机运动的结束角度，这会带来一个问题：最后一次操作的结束角度怎么办？
	于是就需要再添加一个判断，来确定本次是不是最后一次操作，如果是也需要记录一下结束角度

本方法为该项目最有趣的地方，也是最不好理解的地方，（嗯，因为我手搓的方法），简称该方法为路径穿梭。
【存储】需要定义一个结构体，包含舵机编号，记录一次舵机运动中初始角度st与结束角度ed，然后用结构体数组记录全部舵机运动信息。
【原理】简单讲就是：学习模式打开，然后结构体存入舵机运动信息，最后学习结束后，结构体里有舵机编号，有开始角度和结束角度，并且还是按运动顺序记录的，直接for循环遍历一遍，就能重复动作。

（3）代码实现

代码部分如下：

//3 记录路径信息结构体
struct memory{   //记录操纵电位器的过程中，每个舵机运动的初始与结束角度
  int num;       //舵机编号
  int st;        //初始变化角度；
  int ed;        //最终变化角度
}a[200];
int k=1;  //记录结构体储存记录数量，后面复位时要使用

int con=0;                          //control：表示现在是哪个舵机在被控制,赋值为01234，0代表初始值
int fct1=-1,fct2=-1,fct3=-1,fct4=-1;//flag continue：表示舵机持续运动状态
                                    //-1:关闭，学习模式不可用；0:待机；非0：运动
/*
简称穿梭法：记录每个舵机每一次运动开始与运动结束时的角度位置，复位时起点直达重点。
优点：复位速度流畅，支持记录复杂操作，支持同时控制多个电位（容易撑爆数组），可输出舵机运动角度表
缺点：因为本方法的性质，会自动过滤一些无效操作，无法完全复制机械臂行动，比如同一个舵机运动起点与终点一致的运动
*/
void loop(){

  //电位器底座控制
  val1=analogRead(potpin1);
  val1=map(val1,0,1023,0,120);

  //if中表示当检测到1号舵机有操作时的情况：
  if(abs(myservo1.read()-val1)>=5){  //收舵机精度与map映射误差影响，防止连续多次记录1°记录,abs是求绝对值。
       con=1;                   //现在控制的是1号舵机；
       if(!fct1){              //记录1号舵机初始状态，fct1==0时才开始。
         fct1=k;               //在本次状态结束时，k可能已发生变化，不能a[k].ed记录，这里顺手废物利用一下fct1充当下标
         a[k].num=1;
         a[k].st=myservo1.read();//记录最早的一方
         k++;                  //k自在记录到一次开始后就增加。
       }
       myservo1.write(val1);  //先记录，再写入；否则a[k].st记录不到read()的角度了
  }else{                       
    //else中表示当1号舵机无操作时：
    //(1)如果234舵机被控制了,并且1号舵机操纵态为打开，应结束1号机的运动并记录：
    //   if条件:   con！=0,1说明现在其他舵机被控，fct1>0则说明1号舵机与运动态为打开；
    //(2)如果1号机仍然被控，但是触控板按下结束，也需结束1号机的运动并记录：
    //   因为穿梭法记录原理：当下一个舵机被控时，才会结束上次的记录，所以最后一次是记不上的，
    //   需要特判学习模式结束时记录最后一次的结束操作；
    //   if条件:   con=1和触控板结束按下，可以确定本次为最后一次操作
    if((con!=1&&con!=0&&fct1>0)||((con==1)&&(ed_touchval==HIGH))){
      a[fct1].ed=val1;                             
      cout_road(fct1);    //打印本次路径的方法，函数在下方
      fct1=0;             //1号机运动态，归0。待机等待下一次记录；
    }
  }
  
  //电位器大臂控制
  val2=analogRead(potpin2);
  val2=map(val2,0,1023,0,70);
  //注意，因为穿梭法依据下一个舵机被控来结束上一次记录，并且fct1234都设为全局变量，
  //所以234号舵机不能写一个通用的函数方法调用，必须一个个写

  if(abs(myservo2.read()-val2)>=5){
       con=2;
       if(!fct2){
         fct2=k;
         a[k].num=2;
         a[k].st=myservo2.read();
         k++;
       }
       myservo2.write(val2);
  }else{
    if((con!=2&&con!=0&&fct2>0)||((con==2)&&(ed_touchval==HIGH))){
         a[fct2].ed=val2;
         cout_road(fct2);//测试！
         fct2=0;
    }
  }

  //电位器小臂控制
  val3=analogRead(potpin3);
  val3=map(val3,0,1023,30,130);

  if(abs(myservo3.read()-val3)>=10){
       con=3;
       if(!fct3){
         fct3=k;
         a[k].num=3;
         a[k].st=myservo3.read();
         k++;
       }
       myservo3.write(val3);
  }else{
    if((con!=3&&con!=0&&fct3>0)||((con==3)&&(ed_touchval==HIGH))){
         a[fct3].ed=val3;
         cout_road(fct3);//测试！ 
         fct3=0;
    }
  }

  //电位器爪子控制
  val4=analogRead(potpin4);
  val4=map(val4,0,1023,0,80);
  
  if(abs(myservo4.read()-val4)>=8){
       con=4;
       if(!fct4){
         fct4=k;
         a[k].num=4;
         a[k].st=myservo4.read();
         k++;
       }
       myservo4.write(val4);
  }else{
    if((con!=4&&con!=0&&fct4>0)||((con==4)&&(ed_touchval==HIGH))){
         a[fct4].ed=val4;
         cout_road(fct4);//测试！ 
         fct4=0;
    }
  }
  
}
/*
@@@
打印路径信息,可以将输出的路径记录保存下来，设置成不同的机械臂运动方案
*/
void cout_road(int i){  
    //查看：结构体记录的每一个舵机开始与结束角度信息
    // Serial.print(a[i].num);
    // Serial.print("\tstart:\t");
    // Serial.print(a[i].st);
    // Serial.print("°\tend:\t");
    // Serial.println(a[i].ed);

    //输出路径信息，电位器操作完，在串口处可以直接复制。粘贴到另一个文件目录exe里面直接上传执行
    Serial.print("go(myservo");
    Serial.print(a[i].num);
    Serial.print(",");
    Serial.print(a[i].st);
    Serial.print(",");
    Serial.print(a[i].ed);
    Serial.println(");");
}

本方法大概原理就这些，代码中很多地方用了一些小细节，比如fct废物利用赋值k啦，k自增的时机啦，改变舵机角度在代码中的位置啦，等等，有些是有背后原因的，就不在详细介绍。用条件，循环加结构体强行模拟捏了一个方法，没什么算法思想，就纯逻辑，开源欢迎大家学习改进。

---

4 触控板与三色灯板

该部分也比较简单，添加2个触控板表示学习模式的开始与结束；使用三色灯板，不同颜色可以提示现在正处于哪种模式，方便我们观察。

//4 触控板与三色灯板
//三色灯
int lightr=13;   //红灯：复位中，请勿操作
int lightg=12;   //绿灯，手操模式
int lightb=11;  //蓝灯，学习模式
//触控板
int st_touchpin=3;  //开始学习pin
int ed_touchpin=2;  //结束学习pin
int st_touchval;  //读触控板1输入值：HIGH，LOW
int ed_touchval;  //读触控板2输入值：HIGH，LOW

//初始引脚，输入输出模式，与初始灯光
void setup() {
  Serial.begin(9600);
  myservo1.attach(10);   //舵机引脚设定
  myservo2.attach(9);
  myservo3.attach(7);
  myservo4.attach(5);
  
  myservo1.write(pos1);   //舵机角度设定
  myservo2.write(pos2);
  myservo3.write(pos3);
  myservo4.write(pos4);  

  pinMode(st_touchpin,INPUT);//触控板引脚设置
  pinMode(ed_touchpin,INPUT);

  pinMode(lightr,OUTPUT);//rgb灯引脚设置
  pinMode(lightg,OUTPUT);
  pinMode(lightb,OUTPUT);

  digitalWrite(lightr,LOW); //初始绿灯，手操模式
  digitalWrite(lightg,HIGH);
  digitalWrite(lightb,LOW);

  catchs(60);  //机械臂开机热身动画:60度转身操~

}
void loop(){
   //注意了，触控板状态判断这两个if结构，需要放到loop中的最下面
   //因为在前一个穿梭法中，记录好最后一次操作后，才可以进行复位操作

  //控制灯光变化与学习模式开和关
  if(st_touchval==HIGH){  //按下开始触摸板，开始记录路径
    digitalWrite(lightr,LOW); 
    digitalWrite(lightg,LOW);
    digitalWrite(lightb,HIGH);//蓝灯，学习模式

    fct1=0;   //舵机持续运动状态设为：待机
    fct2=0;
    fct3=0;
    fct4=0;
  }
  if(ed_touchval==HIGH){  //按下关闭触控板，且学习模式已打开，任意fct必然>=0
    digitalWrite(lightr,HIGH);
    digitalWrite(lightg,LOW); //复位模式：红灯
    digitalWrite(lightb,LOW);

    Serial.println("动作回放中.......");  
    back(); //该方法就是遍历一遍结构体数组，让对应编号的舵机遍历写入一遍角度，重复动作，函数在下面
    Serial.println("动作回放结束,学习模式关闭！");

    digitalWrite(lightr,LOW); 
    digitalWrite(lightg,HIGH);//回到操纵状态，绿灯
    digitalWrite(lightb,LOW);
  
    fct1=-1;   //-1表示所有舵机持续运动态：关闭
    fct2=-1;
    fct3=-1;
    fct4=-1;
    con=0;      //控制被控舵机初始化
    k=1;       //记录路径编号初始化
  }

}
/*
识别结构体中的舵机编号n，并写入角度m
*/
void idenservo(int n,int m){
  if(n==1) myservo1.write(m); 
  if(n==2) myservo2.write(m); 
  if(n==3) myservo3.write(m); 
  if(n==4) myservo4.write(m); 
}
/*
回退函数，把结构体中记录的角度值回退改变；
读取结构体中记录的k次次记录并写入
*/
void back(){
    //注意从打到小，还是从小到大
    for(int i=1;i<k;i++){
      int r=a[i].num;
      if(a[i].st<=a[i].ed){
        for(int j=a[i].st;j<=a[i].ed;j++){
          idenservo(r,j);//r号舵机写入角度j
          delay(speed);
        }
      }else{
        for(int j=a[i].st;j>=a[i].ed;j--){
          idenservo(r,j);
          delay(speed);
        }
      } 
    }
}

三 展示效果

源程序下载连接

网盘下载源程序

视频链接

开机执行了我设置的左转60度健身操，手动狗头~

Ardunio智能学习机械臂