【meArm机械臂】第二篇·Arduino控制程序

系列文章目录

【meArm机械臂】第一篇·结构设计及搭建
【meArm机械臂】第二篇·Arduino控制程序

---

前言

基于Arduino的机械臂控制程序，可以实现机械臂各个关节的位置初始化、特定位置抓取、手柄方式控制、蓝牙远程控制等功能，该程序是看太极创客教学视频时学的，我对于程序的部分内容进行了优化及改进，视频教程在此。

---

一、测试程序

1.单个电机测试程序

该测试程序是对单个电机进行测试的程序，SG90舵机的转角范围为0°-180°，将舵机连接到Arduino的TestPin引脚上，将该程序上传到Arduino上后，电机应该可以进行从0到180的转动。

#include

Servo TestMotor;
#define TestPin 10

void setup() {
  // put your setup code here, to run once:
  TestMotor.attach(TestPin);
  delay(200);
}

void loop() {
  // put your main code here, to run repeatedly:
  for(int i=0;i++;i<180){
    TestMotor.write(i);
    delay(10);
  }
  for(int j=180;j--;j>0){
    TestMotor.write(j);
    delay(10);
  }
}

2.四舵机控制测试程序

将底盘舵机连接到11号引脚、大臂舵机连接到10号引脚、小臂舵机连接到9号引脚，钳子舵机连接到6号引脚（这些引脚都是PWM引脚），并将该程序上传给arduino，通过串口监视器输入控制指令，可以实现四个舵机的位置控制。

指令格式

b135:将底盘舵机旋转到135°的位置
r60：将大臂舵机旋转到60°的位置
f120:将小臂舵机旋转刀120°的位置
c60:将钳子舵机旋转到60°的位置
输入其他命令则会报错

#include

Servo base,fArm,rArm,claw;

int dataIndex=0;

void setup() {
  // put your setup code here, to run once:
  base.attach(11);
  rArm.attach(10);
  fArm.attach(9);
  claw.attach(6);
  Serial.begin(9600);
  Serial.println("Please input serial data.");
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()>0){
    char servoName=Serial.read();//获取电机指令中的电机编号信息,因为read函数按字节读取

    Serial.print("servoName = ");
    Serial.print(servoName);
    Serial.print(" , ");

    int data=Serial.parseInt();//获取角度信息

    switch(servoName){
      case 'b':
        base.write(data);
        Serial.print("Set base servo value: ");
        Serial.println(data);
        break;
      case 'r':
        rArm.write(data);
        Serial.print("Set rArm servo value: ");
        Serial.println(data);
        break;
      case 'f':
        fArm.write(data);
        Serial.print("Set fArm servo value: ");
        Serial.println(data);
        break;
      case 'c':
        claw.write(data);
        Serial.print("Set claw servo value: ");
        Serial.println(data);
        break;
      default:
        Serial.println("Input ERROR!Please Input repeatly!");
        break;
    }
  }
}

3.极限位置测量

由于机械结构和舵机自身的问题，每个舵机都有自己的最大极限角和最小极限角，需要通过该程序测量出每个舵机的极限位置并存放在相应的常量中，要注意需要留出一定的裕量。

参数说明：

const int baseMin：底盘舵机最小转角
const int baseMax：底盘舵机最大转角
const int rArmMin：大臂舵机最小转角
const int rArmMax：大臂舵机最大转角
const int fArmMin：小臂舵机最小转角
const int fArmMax：小臂舵机最大转角
const int clawMin：钳子舵机最小转角
const int clawMax：钳子舵机最大转角

/*该函数主要用来测试出各个电机的极限转角位置*/
#include
Servo base,fArm,rArm,claw;

//建立4个int型变量存储当前电机角度值，设定初始值
int basePos=90;
int rArmPos=90;
int fArmPos=90;
int clawPos=90;

//存储电机极限值
const int baseMin=0;
const int baseMax=180;
const int rArmMin=45;//留一定裕度空间
const int rArmMax=180;
const int fArmMin=50;
const int fArmMax=120;
const int clawMin=40;
const int clawMax=90;

void setup() {
  // put your setup code here, to run once:
  base.attach(11);
  delay(200);
  rArm.attach(10);
  delay(200);
  fArm.attach(9);
  delay(200);
  claw.attach(6);
  delay(200);
  Serial.begin(9600);
  Serial.println("Welcome to meArm from K.Fire");
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()>0){
    char serialCmd=Serial.read();
    armDataCmd(serialCmd);//实现电机按转角旋转的函数
  }

  base.write(basePos);
  delay(10);
  fArm.write(fArmPos);
  delay(10);
  rArm.write(rArmPos);
  delay(10);
  claw.write(clawPos);
  delay(10);
  //如果设置某个电机角度则产生旋转，否则将保持初值
}



void armDataCmd(char serialCmd){//实现电机按转角旋转的函数
  Serial.print("serialCmd = ");
  Serial.print(serialCmd);

  int servoData = Serial.parseInt();//读取指令中的电机转角
  switch(serialCmd){
    case 'b':
      basePos = servoData;
      Serial.print(" Set base servo value: ");
      Serial.println(servoData);
      break;
    case 'r':
      rArmPos = servoData;
      Serial.print(" Set rArm servo value: ");
      Serial.println(servoData);
      break;
    case 'f':
      fArmPos = servoData;
      Serial.print(" Set fArm servo value: ");
      Serial.println(servoData);
      break;
    case 'c':
      clawPos = servoData;
      Serial.print(" Set claw servo value: ");
      Serial.println(servoData);
      break;
    default:
      Serial.println("Input ERROR!Please Input repeatly!");
      break;
  }
}

二、基本控制程序

该程序可以通过输入舵机旋转指令，控制舵机的旋转，如果输入的转角超出舵机的转角限制范围则会报错，输入错误指令也会报错，也可以通过输入特定的字母控制机械臂完成相应的动作。

指令格式及说明

b135:将底盘舵机旋转到135°的位置
r60：将大臂舵机旋转到60°的位置
f120:将小臂舵机旋转刀120°的位置
c60:将钳子舵机旋转到60°的位置
i:输出各个电机的转角状态及参数信息
g:实现设定抓取动作
l:电机位置初始化
输入其他命令或超出电机限制范围则会报错

/*该函数主要用来控制机械臂工作*/
#include
Servo base,fArm,rArm,claw;

#define pi 3.1415926

void reportStatus();
void armDataCmd(char serialCmd);
void servoCmd(char serialCmd,int toPos);
void vel_segmentation(int fromPos,int toPos,Servo arm_servo);
void Arminit();

//建立4个int型变量存储当前电机角度值，设定初始值
int basePos=90;
int rArmPos=90;
int fArmPos=90;
int clawPos=90;

//存储电机极限值
const int baseMin=0;
const int baseMax=180;
const int rArmMin=40;//留一定裕度空间
const int rArmMax=150;//留一定裕度空间
const int fArmMin=20;
const int fArmMax=120;
const int clawMin=40;
const int clawMax=90;

int Dtime = 15;//机械臂运动延迟时间，通过改变该值来控制机械臂运动速度
               //机械臂运动角速度为：π*1000/(180*Dtime) rad/s

void setup() {
  // put your setup code here, to run once:
  base.attach(11);
  delay(200);
  rArm.attach(10);
  delay(200);
  fArm.attach(9);
  delay(200);
  claw.attach(6);
  delay(200);
  Serial.begin(9600);
  Serial.println("-----------------------------------------------");
  Serial.println("Welcome to meArm from K.Fire");
  Serial.println("Control any motor:Input a command like: b135");
  Serial.println("View motors status information:Input 'i'.");
  Serial.println("Fetching function:Input 'g'.");
  Serial.println("Initialize all motors:Input 'l'.");
  Serial.println("-----------------------------------------------");
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()>0){
    char serialCmd=Serial.read();//read函数为按字节读取，要注意
    armDataCmd(serialCmd);//实现电机按转角旋转
  }

  base.write(basePos);
  delay(10);
  fArm.write(fArmPos);
  delay(10);
  rArm.write(rArmPos);
  delay(10);
  claw.write(clawPos);
  delay(10);
  //如果设置某个电机角度则产生旋转，否则将保持初值
}



void armDataCmd(char serialCmd){//实现电机按转角旋转的函数
  if(serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){
    Serial.print("serialCmd = ");
    Serial.print(serialCmd);
    int servoData = Serial.parseInt();//读取指令中的电机转角
    servoCmd(serialCmd,servoData);
  }
  else{
    switch(serialCmd){
      case 'i': //输出电机状态信息
        reportStatus();
        break;
      case 'l'://电机位置初始化
        Arminit();
        break;
      case 'g'://抓取功能
        GrabSth();
        break;
      default:
        Serial.println();
        Serial.println("【Error】Please Input repeatly!");
        Serial.println();
        break;
    }
  }
}

void servoCmd(char serialCmd,int toPos){
  Serial.println("");
  Serial.print("Command:Servo ");
  Serial.print(serialCmd);
  Serial.print(" to ");
  Serial.print(toPos);
  Serial.print(" at servoVelcity value ");
  Serial.print(1000*pi/(180*Dtime));
  Serial.println(" rad/s.");
  
  int fromPos;//起始位置
  
  switch(serialCmd){
    case 'b':
      if(toPos >= baseMin && toPos <= baseMax){
        fromPos = base.read();
        vel_segmentation(fromPos,toPos,base);
        basePos = toPos;
        Serial.print("Set base servo position value: ");
        Serial.println(toPos);
        Serial.println();
        break;
      }
      else{
        Serial.println("【Warning】Base Servo Position Value Out Of Limit!");
        Serial.println();
        return;
      }
      break;
    case 'r':
      if(toPos >= rArmMin && toPos <= rArmMax){
        fromPos = rArm.read();
        vel_segmentation(fromPos,toPos,rArm);
        rArmPos = toPos;
        Serial.print("Set rArm servo position value: ");
        Serial.println(toPos);
        Serial.println();
        break;
      }
      else{
        Serial.println("【Warning】Base Servo Value Position Out Of Limit!");
        Serial.println();
        return;
      }
      break;
    case 'f':
      if(toPos >= fArmMin && toPos <= fArmMax){
        fromPos = fArm.read();
        vel_segmentation(fromPos,toPos,fArm);
        fArmPos = toPos;
        Serial.print("Set fArm servo position value: ");
        Serial.println(toPos);
        Serial.println();
        break;
      }
      else{
        Serial.println();
        Serial.println("【Warning】Base Servo Value Position Out Of Limit!");
        Serial.println();
        return;
      }
      break;
    case 'c':
      if(toPos >= clawMin && toPos <= clawMax){
        fromPos = claw.read();
        vel_segmentation(fromPos,toPos,claw);
        clawPos = toPos;
        Serial.print("Set claw servo position value: ");
        Serial.println(toPos);
        Serial.println();
        break;
      }
      else{
        Serial.println("【Warning】Base Servo Position Value Out Of Limit!");
        Serial.println();
        return;
      }
      break;
  }
}

void vel_segmentation(int fromPos,int toPos,Servo arm_servo){
  //该函数通过对位置的细分和延时实现电机速度控制
  //这样的控制平均角速度大约为：1°/15ms = 1.16 rad/s
  if(fromPos < toPos){
    for(int i=fromPos;i<=toPos;i++){
        arm_servo.write(i);
        delay(Dtime);
      }
  }
  else{
    for(int i=fromPos;i>=toPos;i--){
        arm_servo.write(i);
        delay(Dtime);
      }
  }
}

void reportStatus(){
  Serial.println();
  Serial.println("---Robot-Arm Status Report---");
  Serial.print("Base Position: ");
  Serial.println(basePos);
  Serial.print("Claw Position: ");
  Serial.println(clawPos);
  Serial.print("rArm Position: ");
  Serial.println(rArmPos);
  Serial.print("fArm Position: ");
  Serial.println(fArmPos);
  Serial.println("-----------------------------");
}

void Arminit(){
  //将电机恢复初始状态
  char ServoArr[4] = {'c','f','r','b'};
  for(int i=0;i<4;i++){
    servoCmd(ServoArr[i],90);
  }
  delay(200);
  Serial.println("Robot Arm has been initized!");
  Serial.println();
}

void GrabSth(){
  //抓取功能
  int GrabSt[4][2] = {
    {'b',135},
    {'r',150},
    {'f',30},
    {'c',40}
  };
  for(int i=0;i<4;i++){
    servoCmd(GrabSt[i][0],GrabSt[i][1]);
  }
}

---

三、最终控制程序

最终控制程序在基本控制程序的基础上进行了优化，包括实现了手柄模式控制机械臂动作、模式转换、蓝牙远程控制等功能，并对程序的内存进行了合理的分配和优化，流出更多的动态空间，提高了程序的稳定性。

指令格式及说明

b135:将底盘舵机旋转到135°的位置
r60：将大臂舵机旋转到60°的位置
f120:将小臂舵机旋转刀120°的位置
c60:将钳子舵机旋转到60°的位置
i:输出各个电机的转角状态及参数信息
g:实现设定抓取动作
l:电机位置初始化
m:控制模式切换
a:小臂向前旋转
s:底盘向左旋转
d:大臂向前旋转
w:钳子张开
4:小臂向后旋转
5:底盘向右旋转
6:大臂向后旋转
8:钳子闭合
输入其他命令或超出电机限制范围则会报错

/*该函数主要用来控制机械臂工作*/
#include
Servo base,fArm,rArm,claw;

#define pi 3.1415926

void Introduce();
void armDataCmd(char serialCmd);
void armJoyCmd(char serialCmd);
void servoCmd(char serialCmd,int toPos);
void vel_segmentation(int fromPos,int toPos,Servo arm_servo);
void reportStatus();
void Arminit();
void GrabSth();

//建立4个int型变量存储当前电机角度值，设定初始值
int basePos=90;
int rArmPos=90;
int fArmPos=90;
int clawPos=90;

//存储电机极限值
const int PROGMEM baseMin=0;
const int PROGMEM baseMax=180;
const int PROGMEM rArmMin=40;//留一定裕度空间
const int PROGMEM rArmMax=150;//留一定裕度空间
const int PROGMEM fArmMin=20;
const int PROGMEM fArmMax=120;
const int PROGMEM clawMin=40;
const int PROGMEM clawMax=90;

const int PROGMEM Dtime = 15;//机械臂运动延迟时间，通过改变该值来控制机械臂运动速度
               //机械臂运动角速度为：π*1000/(180*Dtime) rad/s

bool mode = 1;//mode = 1:指令模式；mode = 0:手柄模式
const int PROGMEM moveStep = 3;//按下手柄按键，舵机的位移量

void setup() {
  // put your setup code here, to run once:
  base.attach(11);
  delay(200);
  rArm.attach(10);
  delay(200);
  fArm.attach(9);
  delay(200);
  claw.attach(6);
  delay(200);
  Serial.begin(9600);
  Introduce();

  base.write(90);
  delay(10);
  fArm.write(90);
  delay(10);
  rArm.write(90);
  delay(10);
  claw.write(90);
  delay(10);
}

void loop() {
  // put your main code here, to run repeatedly:
  if(Serial.available()>0){
    char serialCmd=Serial.read();//read函数为按字节读取，要注意!
    if(mode == 1){
      armDataCmd(serialCmd);//实现电机按转角指令旋转
    }
    else{
      armJoyCmd(serialCmd);//手柄控制机械臂
    }
  }
}

void Introduce(){//介绍该程序的操作方法及功能
  Serial.println(F("Welcome to meArm from K.Fire!"));
  Serial.println(F("The rated mode is Instruction Mode."));
  Serial.println(F("-----------------------------------------------"));
  Serial.println(F("Instruction mode!"));
  Serial.println(F("Control any motor:Input a command like: b135"));
  Serial.println(F("-----------------------------------------------"));
  Serial.println(F("Handle mode!"));
  Serial.println(F("fArm forward:  'a'--- fArm backward:  '4'."));
  Serial.println(F("base leftward: 's'--- base rightward: '5'."));
  Serial.println(F("rArm forward:  'd'--- rArm backward:  '6'."));
  Serial.println(F("claw open:     'w'--- claw close:     '8'."));
  Serial.println(F("-----------------------------------------------"));
  Serial.println(F("View motors status information:Input 'i'."));
  Serial.println(F("Fetching function:Input 'g'."));
  Serial.println(F("Initialize all motors:Input 'l'."));
  Serial.println(F("Change working Mode:Input 'm'."));
  Serial.println(F("-----------------------------------------------"));
}

void armDataCmd(char serialCmd){//实现机械臂在指令模式下工作
  if(serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){
    Serial.print("serialCmd = ");
    Serial.print(serialCmd);
    int servoData = Serial.parseInt();//读取指令中的电机转角
    servoCmd(serialCmd,servoData);
  }
  else{
    switch(serialCmd){
      case 'i': //输出电机状态信息
        reportStatus();
        break;
      case 'l'://电机位置初始化
        Arminit();
        break;
      case 'g'://抓取功能
        GrabSth();
        break;
      case 'm'://切换电机工作模式
        Serial.println("meArm has been changed into Handle Mode ");
        mode = 0;
        break;
      default:     
        Serial.println();
        Serial.println("【Error】Please Input repeatly!");
        Serial.println();
        break;
    }
  }
}

void armJoyCmd(char serialCmd){//实现机械臂在手柄模式下工作
  int baseJoyPos;
  int rArmJoyPos;
  int fArmJoyPos;
  int clawJoyPos;
  switch(serialCmd){
    case 'a'://小臂正转
      fArmJoyPos = fArm.read() - moveStep;
      servoCmd('f',fArmJoyPos);
      break;
    case 's'://底盘左转
      baseJoyPos = base.read() + moveStep;
      servoCmd('b',baseJoyPos);
      break;
    case 'd'://大臂正转
      rArmJoyPos = rArm.read() + moveStep;
      servoCmd('r',rArmJoyPos);
      break;
    case 'w'://钳子张开
      clawJoyPos = claw.read() - moveStep;
      servoCmd('c',clawJoyPos);
      break;
    case '4'://小臂反转
      fArmJoyPos = fArm.read() + moveStep;
      servoCmd('f',fArmJoyPos);
      break;
    case '5'://底盘右转
      baseJoyPos = base.read() - moveStep;
      servoCmd('b',baseJoyPos);
      break;
    case '6'://大臂反转
      rArmJoyPos = rArm.read() - moveStep;
      servoCmd('r',rArmJoyPos);
      break;
    case '8'://钳子闭合
      clawJoyPos = claw.read() + moveStep;
      servoCmd('c',clawJoyPos);
      break;
    case 'i': //输出电机状态信息
        reportStatus();
        break;
    case 'l'://电机位置初始化
      Arminit();
      break;
    case 'g'://抓取功能
      GrabSth();
      break;
    case 'm'://切换电机工作模式
      Serial.println("meArm has been changed into Instruction Mode");
      mode = 1;
      break;
    default:
      Serial.println();
      Serial.println("【Error】Invalid handle key!");
      Serial.println();
      break;
  }
}

void servoCmd(char serialCmd,int toPos){//电机旋转功能函数
  Serial.println("");
  Serial.print("Command:Servo ");
  Serial.print(serialCmd);
  Serial.print(" to ");
  Serial.print(toPos);
  Serial.print(" at servoVelcity value ");
  Serial.print(1000*pi/(180*Dtime));
  Serial.println(" rad/s.");
  
  int fromPos;//起始位置
  
  switch(serialCmd){
    case 'b':
      if(toPos >= baseMin && toPos <= baseMax){
        fromPos = base.read();
        vel_segmentation(fromPos,toPos,base);
        basePos = toPos;
        Serial.print("Set base servo position value: ");
        Serial.println(toPos);
        Serial.println();
        break;
      }
      else{
        Serial.println("【Warning】Base Servo Position Value Out Of Limit!");
        Serial.println();
        return;
      }
      break;
    case 'r':
      if(toPos >= rArmMin && toPos <= rArmMax){
        fromPos = rArm.read();
        vel_segmentation(fromPos,toPos,rArm);
        rArmPos = toPos;
        Serial.print("Set rArm servo position value: ");
        Serial.println(toPos);
        Serial.println();
        break;
      }
      else{
        Serial.println("【Warning】Base Servo Value Position Out Of Limit!");
        Serial.println();
        return;
      }
      break;
    case 'f':
      if(toPos >= fArmMin && toPos <= fArmMax){
        fromPos = fArm.read();
        vel_segmentation(fromPos,toPos,fArm);
        fArmPos = toPos;
        Serial.print("Set fArm servo position value: ");
        Serial.println(toPos);
        Serial.println();
        break;
      }
      else{
        Serial.println();
        Serial.println("【Warning】Base Servo Value Position Out Of Limit!");
        Serial.println();
        return;
      }
      break;
    case 'c':
      if(toPos >= clawMin && toPos <= clawMax){
        fromPos = claw.read();
        vel_segmentation(fromPos,toPos,claw);
        clawPos = toPos;
        Serial.print("Set claw servo position value: ");
        Serial.println(toPos);
        Serial.println();
        break;
      }
      else{
        Serial.println("【Warning】Base Servo Position Value Out Of Limit!");
        Serial.println();
        return;
      }
      break;
  }
}

void vel_segmentation(int fromPos,int toPos,Servo arm_servo){//速度控制函数
  //该函数通过对位置的细分和延时实现电机速度控制
  //这样的控制平均角速度大约为：1°/15ms = 1.16 rad/s
  if(fromPos < toPos){
    for(int i=fromPos;i<=toPos;i++){
        arm_servo.write(i);
        delay(Dtime);
      }
  }
  else{
    for(int i=fromPos;i>=toPos;i--){
        arm_servo.write(i);
        delay(Dtime);
      }
  }
}

void reportStatus(){//电机状态信息控制函数
  Serial.println();
  Serial.println("---Robot-Arm Status Report---");
  Serial.print("Base Position: ");
  Serial.println(basePos);
  Serial.print("Claw Position: ");
  Serial.println(clawPos);
  Serial.print("rArm Position: ");
  Serial.println(rArmPos);
  Serial.print("fArm Position: ");
  Serial.println(fArmPos);
  Serial.println("-----------------------------");
  Serial.println("Motor Model:Micro Servo 9g-SG90");
  Serial.println("Motor size: 23×12.2×29mm");
  Serial.println("Work temperature:0-60℃");
  Serial.println("Rated voltage: 5V");
  Serial.println("Rated torque: 0.176 N·m");
  Serial.println("-----------------------------");
}

void Arminit(){//电机初始化函数
  //将电机恢复初始状态
  char ServoArr[4] = {'c','f','r','b'};
  for(int i=0;i<4;i++){
    servoCmd(ServoArr[i],90);
  }
  delay(200);
  Serial.println("meArm has been initized!");
  Serial.println();
}

void GrabSth(){//抓取函数
  //抓取功能
  int GrabSt[4][2] = {
    {'b',135},
    {'r',150},
    {'f',30},
    {'c',40}
  };
  for(int i=0;i<4;i++){
    servoCmd(GrabSt[i][0],GrabSt[i][1]);
  }
}

蓝牙控制：使用HC-06蓝牙模块，将其RX、TX引脚分别与arduino的TX、RX引脚相连，使用相应的软件即（如：Arduino bluetooth controller.apk）可实现控制。

所有程序资源大家可以免费下载

---

总结

目前实现的功能都很简单，并且是基于Arduino开发的程序，况且对于Arduino我认为也没有特别深入的理解，有很多功能尚未开发使用，现在我正在开发另一个项目-差速小车，想完成激光雷达SLAM建图、路径规划及智能循迹，完成这之后应该会再捞起机械臂，进行机械臂的再升级改良，比如实现机械臂抓取的正解、逆解问题，更换舵机进行控制算法的优化、以及机械臂和移动小车底盘的协作等功能，继续加油。