系列文章目录
【meArm机械臂】第一篇·结构设计及搭建
【meArm机械臂】第二篇·Arduino控制程序
文章目录
前言
基于Arduino的机械臂控制程序，可以实现机械臂各个关节的位置初始化、特定位置抓取、手柄方式控制、蓝牙远程控制等功能，该程序是看太极创客教学视频时学的，我对于程序的部分内容进行了优化及改进，视频教程在此。
一、测试程序
1.单个电机测试程序
该测试程序是对单个电机进行测试的程序，SG90舵机的转角范围为0°-180°，将舵机连接到Arduino的TestPin引脚上，将该程序上传到Arduino上后，电机应该可以进行从0到180的转动。
#include<Servo.h>
Servo TestMotor;#defineTestPin10voidsetup(){// put your setup code here, to run once:
TestMotor.attach(TestPin);delay(200);}voidloop(){// 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.h>
Servo base,fArm,rArm,claw;int dataIndex=0;voidsetup(){// 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.");}voidloop(){// 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.h>
Servo base,fArm,rArm,claw;//建立4个int型变量存储当前电机角度值，设定初始值int basePos=90;int rArmPos=90;int fArmPos=90;int clawPos=90;//存储电机极限值constint baseMin=0;constint baseMax=180;constint rArmMin=45;//留一定裕度空间constint rArmMax=180;constint fArmMin=50;constint fArmMax=120;constint clawMin=40;constint clawMax=90;voidsetup(){// 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");}voidloop(){// 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);//如果设置某个电机角度则产生旋转，否则将保持初值}voidarmDataCmd(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.h>
Servo base,fArm,rArm,claw;#definepi3.1415926voidreportStatus();voidarmDataCmd(char serialCmd);voidservoCmd(char serialCmd,int toPos);voidvel_segmentation(int fromPos,int toPos,Servo arm_servo);voidArminit();//建立4个int型变量存储当前电机角度值，设定初始值int basePos=90;int rArmPos=90;int fArmPos=90;int clawPos=90;//存储电机极限值constint baseMin=0;constint baseMax=180;constint rArmMin=40;//留一定裕度空间constint rArmMax=150;//留一定裕度空间constint fArmMin=20;constint fArmMax=120;constint clawMin=40;constint clawMax=90;int Dtime =15;//机械臂运动延迟时间，通过改变该值来控制机械臂运动速度//机械臂运动角速度为：π*1000/(180*Dtime) rad/svoidsetup(){// 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("-----------------------------------------------");}voidloop(){// 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);//如果设置某个电机角度则产生旋转，否则将保持初值}voidarmDataCmd(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;}}}voidservoCmd(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;}}voidvel_segmentation(int fromPos,int toPos,Servo arm_servo){//该函数通过对位置的细分和延时实现电机速度控制//这样的控制平均角速度大约为：1°/15ms = 1.16 rad/sif(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);}}}voidreportStatus(){
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("-----------------------------");}voidArminit(){//将电机恢复初始状态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();}voidGrabSth(){//抓取功能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.h>
Servo base,fArm,rArm,claw;#definepi3.1415926voidIntroduce();voidarmDataCmd(char serialCmd);voidarmJoyCmd(char serialCmd);voidservoCmd(char serialCmd,int toPos);voidvel_segmentation(int fromPos,int toPos,Servo arm_servo);voidreportStatus();voidArminit();voidGrabSth();//建立4个int型变量存储当前电机角度值，设定初始值int basePos=90;int rArmPos=90;int fArmPos=90;int clawPos=90;//存储电机极限值constint PROGMEM baseMin=0;constint PROGMEM baseMax=180;constint PROGMEM rArmMin=40;//留一定裕度空间constint PROGMEM rArmMax=150;//留一定裕度空间constint PROGMEM fArmMin=20;constint PROGMEM fArmMax=120;constint PROGMEM clawMin=40;constint PROGMEM clawMax=90;constint PROGMEM Dtime =15;//机械臂运动延迟时间，通过改变该值来控制机械臂运动速度//机械臂运动角速度为：π*1000/(180*Dtime) rad/s
bool mode =1;//mode = 1:指令模式；mode = 0:手柄模式constint PROGMEM moveStep =3;//按下手柄按键，舵机的位移量voidsetup(){// 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);}voidloop(){// 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);//手柄控制机械臂}}}voidIntroduce(){//介绍该程序的操作方法及功能
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("-----------------------------------------------"));}voidarmDataCmd(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;}}}voidarmJoyCmd(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;}}voidservoCmd(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;}}voidvel_segmentation(int fromPos,int toPos,Servo arm_servo){//速度控制函数//该函数通过对位置的细分和延时实现电机速度控制//这样的控制平均角速度大约为：1°/15ms = 1.16 rad/sif(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);}}}voidreportStatus(){//电机状态信息控制函数
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("-----------------------------");}voidArminit(){//电机初始化函数//将电机恢复初始状态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();}voidGrabSth(){//抓取函数//抓取功能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建图、路径规划及智能循迹，完成这之后应该会再捞起机械臂，进行机械臂的再升级改良，比如实现机械臂抓取的正解、逆解问题，更换舵机进行控制算法的优化、以及机械臂和移动小车底盘的协作等功能，继续加油。
本文转载自: https://blog.csdn.net/qq_52785580/article/details/122509669
版权归原作者 K.Fire浑身是肝 所有， 如有侵权，请联系我们删除。
版权归原作者 K.Fire浑身是肝 所有， 如有侵权，请联系我们删除。