Arduino 太极创客 机械臂 在拼多多上买到62元,
但背后蕴含的物联网应用知识确实无价的宝藏,
以下代码是自己修改的,可以直接烧录:
/*MeArm机械臂控制程序-6by 太极创客 (2017-06-02)www.taichi-maker.com 本程序用于太极创客《零基础入门学用Arduino教程 - MeArm篇》。1 使用串口获得电机数据,设置机械臂动作 将串口指令解读改写为函数,加入状态汇报函数reportStatus2 加入servoCmd函数控制电机速度3 加入判断人类指令是否超出舵机限值4 MeArm执行一系列动作(执行程序) armIniPos - 1 (一维数组)5 MeArm执行一系列动作(执行程序) armIniPos - 2 (二维数组)6 加入手柄控制模式 加入机器判断:人类输入信息错误检查 如需要获得具体电路连接细节,请查阅太极创客制作的《零基础入门学用Arduino教程 - MeArm篇》页面。*/ #include <Servo.h> //使用servo库 Servo myservo; // 创建一个Servo对象 Servo base, fArm, rArm, claw ; //创建4个servo对象 /*Servo baseServo,rArmServo,fArmServo,clawServo; // 在全局作用域中声明变量 */ // 创建四个Servo对象 Servo baseServo; Servo shoulderServo; Servo elbowServo; Servo wristServo; char command; // 声明变量command //Servo myservo; // 创建一个Servo对象 const int minAngle = 0; // 舵机的最小角度限制 const int maxAngle = 180; // 舵机的最大角度限制 // 创建两个Servo对象 /*Servo myservo1; Servo myservo2;*/ // 定义连接到Arduino的LED引脚 int ledPin = 3; // 定义摇杆的模拟输入引脚 /*const int potX = A0; // 左摇杆X轴连接到A0 const int potY = A1; // 左摇杆Y轴连接到A1 const int potRightX = A2; // 右摇杆X轴连接到A2 const int potRightY = A3; // 右摇杆Y轴连接到A3*/ // 定义摇杆的模拟输入引脚 const int potBase = A0; const int potShoulder = A1; const int potElbow = A3; const int potWrist = A2; // 定义舵机连接的数字引脚 const int basePin = 11; const int shoulderPin = 10; const int elbowPin = 9; const int wristPin = 8; //存储电机极限值(const指定该数值为常量,常量数值在程序运行中不能改变) const int baseMin = 0; const int baseMax = 180; const int rArmMin = 45; const int rArmMax = 180; const int fArmMin = 35; const int fArmMax = 120; const int clawMin = 25; const int clawMax = 100; int DSD = 15; //Default Servo Delay (默认电机运动延迟时间) //此变量用于控制电机运行速度.增大此变量数值将 //降低电机运行速度从而控制机械臂动作速度。 bool mode; //mode = 1: 指令模式, mode = 0: 手柄模式 int moveStep = 3; // 每一次按下手柄按键,舵机移动量(仅适用于手柄模式) void servoCmd(char servoName, int toPos, int servoDelay){ Servo servo2go; //创建servo对象 //串口监视器输出接收指令信息 Serial.println(""); Serial.print("+Command: Servo "); Serial.print(servoName); Serial.print(" to "); Serial.print(toPos); Serial.print(" at servoDelay value "); Serial.print(servoDelay); Serial.println("."); Serial.println(""); int fromPos; //建立变量,存储电机起始运动角度值 switch(servoName){ case 'b': if(toPos >= baseMin && toPos <= baseMax){ servo2go = base; fromPos = base.read(); // 获取当前电机角度值用于“电机运动起始角度值” break; } else { Serial.println("+Warning: Base Servo Value Out Of Limit!"); return; } case 'c': if(toPos >= clawMin && toPos <= clawMax){ servo2go = claw; fromPos = claw.read(); // 获取当前电机角度值用于“电机运动起始角度值” break; } else { Serial.println("+Warning: Claw Servo Value Out Of Limit!"); return; } case 'f': if(toPos >= fArmMin && toPos <= fArmMax){ servo2go = fArm; fromPos = fArm.read(); // 获取当前电机角度值用于“电机运动起始角度值” break; } else { Serial.println("+Warning: fArm Servo Value Out Of Limit!"); return; } case 'r': if(toPos >= rArmMin && toPos <= rArmMax){ servo2go = rArm; fromPos = rArm.read(); // 获取当前电机角度值用于“电机运动起始角度值” break; } else { Serial.println("+Warning: rArm Servo Value Out Of Limit!"); return; } } //指挥电机运行 if (fromPos <= toPos){ //如果“起始角度值”小于“目标角度值” for (int i=fromPos; i<=toPos; i++){ servo2go.write(i); delay (servoDelay); } } else { //否则“起始角度值”大于“目标角度值” for (int i=fromPos; i>=toPos; i--){ servo2go.write(i); delay (servoDelay); } } } void reportStatus(){ //舵机状态信息 Serial.println(""); Serial.println(""); Serial.println("+ Robot-Arm Status Report +"); Serial.print("Claw Position: "); Serial.println(claw.read()); Serial.print("Base Position: "); Serial.println(base.read()); Serial.print("Rear Arm Position:"); Serial.println(rArm.read()); Serial.print("Front Arm Position:"); Serial.println(fArm.read()); Serial.println("++++++++++++++++++++++++++"); Serial.println(""); } void armIniPos(){ Serial.println("+Command: Restore Initial Position."); int robotIniPosArray[4][3] = { {'b', 90, DSD}, {'r', 90, DSD}, {'f', 90, DSD}, {'c', 90, DSD} }; for (int i = 0; i < 4; i++){ servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]); } } void armDataCmd(char command); void setup(){ // 初始化串行通信 Serial.begin(9600); // 将舵机连接到数字引脚 baseServo.attach(basePin); shoulderServo.attach(shoulderPin); elbowServo.attach(elbowPin); wristServo.attach(wristPin); /*myservo.attach(7); // 将舵机连接到数字引7 base.attach(11); // base 伺服舵机连接引脚targetAngle11 舵机代号'b' delay(20); // 稳定性等待 rArm.attach(10); // rArm 伺服舵机连接引脚10 舵机代号'r' delay(20); // 稳定性等待 fArm.attach(9); // fArm 伺服舵机连接引脚9 舵机代号'f' delay(20); // 稳定性等待 claw.attach(8); // claw 伺服舵机连接引脚6 舵机代号'c' delay(20); // 稳定性等待 */ /* base.write(90); delay(10); fArm.write(90); delay(10); rArm.write(90); delay(10); claw.write(90); delay(10); */ Serial.begin(9600); Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial"); Serial.println("Input mode: 1 (armDataCmd) & 0 (armJoyCmd)"); delay(1000); // 等待3秒 // 检查串行缓冲区是否有数据可读 if (Serial.available() > 0){ // 读取一个字符 char serialCmd = Serial.read(); // 打印接收到的命令到串行监视器,用于调试 Serial.println("Received command: " + String(serialCmd)); // 如果模式为1,调用armDataCmd函数处理命令 if( mode == 1 ){ armDataCmd(serialCmd); //指令模式 } digitalWrite(LED_BUILTIN, HIGH); // 打开内置LED灯 delay(1000); // 等待1秒 digitalWrite(LED_BUILTIN, LOW); // 关闭内置LED灯 } else { // armJoyCmd(serialCmd); //手柄模式 } pinMode(ledPin, OUTPUT); // 设置LED引脚为输出模式 } void armDataCmd(char serialCmd){ //Arduino根据串行指令执行相应操作 //指令示例:b45 底盘转到45度角度位置 // o 输出机械臂舵机状态信息 //判断人类是否因搞错模式而输入错误的指令信息(指令模式下输入手柄按键信息) if ( serialCmd == 'w' || serialCmd == 's' || serialCmd == 'a' || serialCmd == 'd' || serialCmd == '5' || serialCmd == '4' || serialCmd == '6' || serialCmd == '8' ){ Serial.println("+Warning: Robot in Instruction Mode..."); delay(10); while(Serial.available()>0) char wrongCommand = Serial.read(); //清除串口缓存的错误指令 return; } if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){ int servoData = Serial.parseInt(); servoCmd(serialCmd, servoData, DSD); // 机械臂舵机运行函数(参数:舵机名,目标角度,延迟/速度) } else { switch(serialCmd){ // 使用switch语句根据接收到的命令执行不同的操作 //switch (command) { case '1': // 如果命令是'1',打印信息并打开LED灯 Serial.println("Command 'A' received: LED ON"); digitalWrite(ledPin, HIGH); // 设置LED引脚为高电平,打开LED灯 break; // 退出switch语句 case 'B': // 如果命令是'c',打印信息并关闭LED灯 Serial.println("Command 'c' received: LED OFF"); digitalWrite(ledPin, LOW); // 设置LED引脚为低电平,关闭LED灯 break; // 退出switch语句 default: // 如果命令不是'A'或'B',打印未知命令信息 Serial.println("Unknown command received"); break; // 退出switch语句 case 'm' : //切换至手柄模式 mode = 0; Serial.println("Command: Switch to Joy-Stick Mode."); break; case 'o': // 输出舵机状态信息 reportStatus(); break; case 'i': armIniPos(); break; } } } void armJoyCmd(char serialCmd){ //Arduino根据手柄按键执行相应操作 // 读取左摇杆的X轴和Y轴位置 int leftX = analogRead(A0); // X轴位置 int leftY = 1023 - analogRead(A1); // Y轴位置(取反) // 读取右摇杆的X轴和Y轴位置 int rightX = analogRead(A3); // X轴位置 int rightY = 1023 - analogRead(A2); // Y轴位置(取反) Serial.println("Command received: " + String(command)); // 打印摇杆位置到串行监视器 Serial.print("Left X: "); Serial.print(leftX); Serial.print(", Left Y: "); Serial.print(leftY); Serial.print(", Right X: "); Serial.print(rightX); Serial.print(", Right Y: "); Serial.println(rightY); delay(100); // 稍作延迟 // 读取左摇杆的X轴和Y轴位置 /* leftX = analogRead(potX); leftY = analogRead(potY);*/ // 读取右摇杆的X轴和Y轴位置 /* rightX = analogRead(potRightX); rightY = analogRead(potRightY);*/ // 将模拟值映射到舵机角度(0-180度) /* int angleLeftX = map(leftX, 0, 1023, 0, 180); int angleLeftY = map(leftY, 0, 1023, 0, 180); int angleRightX = map(rightX, 0, 1023, 0, 180); int angleRightY = map(rightY, 0, 1023, 0, 180);*/ // 打印摇杆位置到串行监视器 /* Serial.print("Left X: "); Serial.print(leftX); Serial.print(", Left Y: "); Serial.print(leftY); Serial.print(", Right X: "); Serial.print(rightX); Serial.print(", Right Y: "); Serial.println(rightY);*/ // 根据摇杆位置设置舵机角度 /*myservo1.write(angleLeftX); // 控制左摇杆X轴对应的舵机 myservo2.write(angleLeftY); // 控制左摇杆Y轴对应的舵机*/ // 如果需要,也可以根据右摇杆的位置来控制舵机 //myservo1.write(angleRightX); //myservo2.write(angleRightY); delay(15); // 等待舵机到达指定位置 //判断人类是否因搞错模式而输入错误的指令信息(手柄模式下输入舵机指令) if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){ Serial.println("+Warning: Robot in Joy-Stick Mode..."); delay(50); while(Serial.available()>0) char wrongCommand = Serial.read(); //清除串口缓存的错误指令 return; } int baseJoyPos; int rArmJoyPos; int fArmJoyPos; int clawJoyPos; switch(serialCmd){ case '1' : //切换至命令模式 mode = 1; Serial.println("Command: Switch to armDataCmd Mode."); break; case 'a': // Base向左 Serial.println("Received Command: Base Turn Left"); baseJoyPos = base.read() - moveStep; servoCmd('b', baseJoyPos, DSD); break; case 'd': // Base向右 Serial.println("Received Command: Base Turn Right"); baseJoyPos = base.read() + moveStep; servoCmd('b', baseJoyPos, DSD); break; case 's': // rArm向下 Serial.println("Received Command: Rear Arm Down"); rArmJoyPos = rArm.read() + moveStep; servoCmd('r', rArmJoyPos, DSD); break; case 'w': // rArm向上 Serial.println("Received Command: Rear Arm Up"); rArmJoyPos = rArm.read() - moveStep; servoCmd('r', rArmJoyPos, DSD); break; case '8': // fArm向上 Serial.println("Received Command: Front Arm Up"); fArmJoyPos = fArm.read() + moveStep; servoCmd('f', fArmJoyPos, DSD); break; case '5': // fArm向下 Serial.println("Received Command: Front Arm Down"); fArmJoyPos = fArm.read() - moveStep; servoCmd('f', fArmJoyPos, DSD); break; case '4': // Claw关闭 Serial.println("Received Command: Claw Close Down"); clawJoyPos = claw.read() + moveStep; servoCmd('c', clawJoyPos, DSD); break; case '6': // Claw打开 Serial.println("Received Command: Claw Open Up"); clawJoyPos = claw.read() - moveStep; servoCmd('c', clawJoyPos, DSD); break; case 'm' : //切换至指令模式 mode = 1; Serial.println("Command: Switch to Instruction Mode."); break; case 'o': reportStatus(); break; case 'i': armIniPos(); break; default: Serial.println("Unknown Command."); return; } } void loop(){ // 读取摇杆的位置 int basePos = analogRead(potBase); int shoulderPos = analogRead(potShoulder); int elbowPos = analogRead(potElbow); int wristPos = analogRead(potWrist); /*int targetAngle = analogRead(A2); // 从模拟脚A0读取值,范围是0-1023 // 将读取的模拟值映射到舵机的角度范围,并添加角度限制 targetAngle = map(targetAngle, 0, 1023, minAngle, maxAngle); // 限制角度在0到180度之间 targetAngle = constrain(targetAngle, 0, 180); myservo.write(targetAngle); // 设置舵机角度 delay(3); // 等待15毫秒观察效果*/ // 将摇杆的位置映射到舵机的角度范围 basePos = map(basePos, 0, 1023, 0, 180); shoulderPos = map(shoulderPos, 0, 1023, 0, 180); elbowPos = map(elbowPos, 0, 1023, 0, 180); wristPos = map(wristPos, 0, 1023, 0, 180); /* int basePosition = map(analogRead(potX), 0, 1023, baseMin, baseMax); int rArmPosition = map(analogRead(potY), 0, 1023, rArmMin, rArmMax); int fArmPosition = map(analogRead(potRightX), 0, 1023, fArmMin, fArmMax); int clawPosition = map(analogRead(potRightY), 0, 1023, clawMin, clawMax); */ // 设置舵机的角度 baseServo.write(basePos); shoulderServo.write(shoulderPos); elbowServo.write(elbowPos); wristServo.write(wristPos); /* baseServo.write(basePosition); rArmServo.write(rArmPosition); fArmServo.write(fArmPosition); clawServo.write(clawPosition);*/ // 打印摇杆的位置到串行监视器,用于调试 Serial.print("Base: "); Serial.print(basePos); Serial.print(", Shoulder: "); Serial.print(shoulderPos); Serial.print(", Elbow: "); Serial.print(elbowPos); Serial.print(", Wrist: "); Serial.println(wristPos); if (Serial.available() > 0) { command = Serial.read(); Serial.println("Received command: " + String(command)); } // 呼吸灯效果:逐渐增加亮度,然后逐渐减少亮度 for (int brightness = 0; brightness <= 200; brightness++) { // 设置LED的亮度 analogWrite(ledPin, brightness); // 等待一段时间,以便人眼可以感知亮度变化 delay(10); } for (int brightness = 200; brightness >= 0; brightness--) { // 设置LED的亮度 analogWrite(ledPin, brightness); // 等待一段时间,以便人眼可以感知亮度变化 delay(10); } } 里面有重复的部分,有些是不执行的,比如机械臂的这个部分: //判断人类是否因搞错模式而输入错误的指令信息(手柄模式下输入舵机指令) if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){ Serial.println("+Warning: Robot in Joy-Stick Mode..."); delay(50); while(Serial.available()>0) char wrongCommand = Serial.read(); //清除串口缓存的错误指令 return; } int baseJoyPos; int rArmJoyPos; int fArmJoyPos; int clawJoyPos; switch(serialCmd){ case '1' : //切换至命令模式 mode = 1; Serial.println("Command: Switch to armDataCmd Mode."); break; case 'a': // Base向左 Serial.println("Received Command: Base Turn Left"); baseJoyPos = base.read() - moveStep; servoCmd('b', baseJoyPos, DSD); break; case 'd': // Base向右 Serial.println("Received Command: Base Turn Right"); baseJoyPos = base.read() + moveStep; servoCmd('b', baseJoyPos, DSD); break; case 's': // rArm向下 Serial.println("Received Command: Rear Arm Down"); rArmJoyPos = rArm.read() + moveStep; servoCmd('r', rArmJoyPos, DSD); break; case 'w': // rArm向上 Serial.println("Received Command: Rear Arm Up"); rArmJoyPos = rArm.read() - moveStep; servoCmd('r', rArmJoyPos, DSD); break; case '8': // fArm向上 Serial.println("Received Command: Front Arm Up"); fArmJoyPos = fArm.read() + moveStep; servoCmd('f', fArmJoyPos, DSD); break; case '5': // fArm向下 Serial.println("Received Command: Front Arm Down"); fArmJoyPos = fArm.read() - moveStep; servoCmd('f', fArmJoyPos, DSD); break; case '4': // Claw关闭 Serial.println("Received Command: Claw Close Down"); clawJoyPos = claw.read() + moveStep; servoCmd('c', clawJoyPos, DSD); break; case '6': // Claw打开 Serial.println("Received Command: Claw Open Up"); clawJoyPos = claw.read() - moveStep; servoCmd('c', clawJoyPos, DSD); break; case 'm' : //切换至指令模式 mode = 1; Serial.println("Command: Switch to Instruction Mode."); break; case 'o': reportStatus(); break; case 'i': armIniPos(); break; default: Serial.println("Unknown Command."); return; } 这部分是串口命令控制舵机,未理解,所以没有改动,下面是原来的程序,希望大佬对应修改,发出来: /* MeArm机械臂控制程序-6 by 太极创客 (2017-06-02) www.taichi-maker.com 本程序用于太极创客《零基础入门学用Arduino教程 - MeArm篇》。 1 使用串口获得电机数据,设置机械臂动作 将串口指令解读改写为函数,加入状态汇报函数reportStatus 2 加入servoCmd函数控制电机速度 3 加入判断人类指令是否超出舵机限值 4 MeArm执行一系列动作(执行程序) armIniPos - 1 (一维数组) 5 MeArm执行一系列动作(执行程序) armIniPos - 2 (二维数组) 6 加入手柄控制模式 加入机器判断:人类输入信息错误检查 如需要获得具体电路连接细节,请查阅太极创客制作的 《零基础入门学用Arduino教程 - MeArm篇》页面。 */ #include <Servo.h> //使用servo库 Servo base, fArm, rArm, claw ; //创建4个servo对象 //存储电机极限值(const指定该数值为常量,常量数值在程序运行中不能改变) const int baseMin = 0; const int baseMax = 180; const int rArmMin = 45; const int rArmMax = 180; const int fArmMin = 35; const int fArmMax = 120; const int clawMin = 25; const int clawMax = 100; int DSD = 15; //Default Servo Delay (默认电机运动延迟时间) //此变量用于控制电机运行速度.增大此变量数值将 //降低电机运行速度从而控制机械臂动作速度。 bool mode; //mode = 1: 指令模式, mode = 0: 手柄模式 int moveStep = 3; // 每一次按下手柄按键,舵机移动量(仅适用于手柄模式) void setup(){ base.attach(11); // base 伺服舵机连接引脚11 舵机代号'b' delay(200); // 稳定性等待 rArm.attach(10); // rArm 伺服舵机连接引脚10 舵机代号'r' delay(200); // 稳定性等待 fArm.attach(9); // fArm 伺服舵机连接引脚9 舵机代号'f' delay(200); // 稳定性等待 claw.attach(6); // claw 伺服舵机连接引脚6 舵机代号'c' delay(200); // 稳定性等待 base.write(90); delay(10); fArm.write(90); delay(10); rArm.write(90); delay(10); claw.write(90); delay(10); Serial.begin(9600); Serial.println("Welcome to Taichi-Maker Robot Arm Tutorial"); } void loop(){ if (Serial.available()>0) { char serialCmd = Serial.read(); if( mode == 1 ){ armDataCmd(serialCmd); //指令模式 } else { armJoyCmd(serialCmd); //手柄模式 } } } void armDataCmd(char serialCmd){ //Arduino根据串行指令执行相应操作 //指令示例:b45 底盘转到45度角度位置 // o 输出机械臂舵机状态信息 //判断人类是否因搞错模式而输入错误的指令信息(指令模式下输入手柄按键信息) if ( serialCmd == 'w' || serialCmd == 's' || serialCmd == 'a' || serialCmd == 'd' || serialCmd == '5' || serialCmd == '4' || serialCmd == '6' || serialCmd == '8' ){ Serial.println("+Warning: Robot in Instruction Mode..."); delay(100); while(Serial.available()>0) char wrongCommand = Serial.read(); //清除串口缓存的错误指令 return; } if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){ int servoData = Serial.parseInt(); servoCmd(serialCmd, servoData, DSD); // 机械臂舵机运行函数(参数:舵机名,目标角度,延迟/速度) } else { switch(serialCmd){ case 'm' : //切换至手柄模式 mode = 0; Serial.println("Command: Switch to Joy-Stick Mode."); break; case 'o': // 输出舵机状态信息 reportStatus(); break; case 'i': armIniPos(); break; default: //未知指令反馈 Serial.println("Unknown Command."); } } } void armJoyCmd(char serialCmd){ //Arduino根据手柄按键执行相应操作 //判断人类是否因搞错模式而输入错误的指令信息(手柄模式下输入舵机指令) if (serialCmd == 'b' || serialCmd == 'c' || serialCmd == 'f' || serialCmd == 'r'){ Serial.println("+Warning: Robot in Joy-Stick Mode..."); delay(100); while(Serial.available()>0) char wrongCommand = Serial.read(); //清除串口缓存的错误指令 return; } int baseJoyPos; int rArmJoyPos; int fArmJoyPos; int clawJoyPos; switch(serialCmd){ case 'a': // Base向左 Serial.println("Received Command: Base Turn Left"); baseJoyPos = base.read() - moveStep; servoCmd('b', baseJoyPos, DSD); break; case 'd': // Base向右 Serial.println("Received Command: Base Turn Right"); baseJoyPos = base.read() + moveStep; servoCmd('b', baseJoyPos, DSD); break; case 's': // rArm向下 Serial.println("Received Command: Rear Arm Down"); rArmJoyPos = rArm.read() + moveStep; servoCmd('r', rArmJoyPos, DSD); break; case 'w': // rArm向上 Serial.println("Received Command: Rear Arm Up"); rArmJoyPos = rArm.read() - moveStep; servoCmd('r', rArmJoyPos, DSD); break; case '8': // fArm向上 Serial.println("Received Command: Front Arm Up"); fArmJoyPos = fArm.read() + moveStep; servoCmd('f', fArmJoyPos, DSD); break; case '5': // fArm向下 Serial.println("Received Command: Front Arm Down"); fArmJoyPos = fArm.read() - moveStep; servoCmd('f', fArmJoyPos, DSD); break; case '4': // Claw关闭 Serial.println("Received Command: Claw Close Down"); clawJoyPos = claw.read() + moveStep; servoCmd('c', clawJoyPos, DSD); break; case '6': // Claw打开 Serial.println("Received Command: Claw Open Up"); clawJoyPos = claw.read() - moveStep; servoCmd('c', clawJoyPos, DSD); break; case 'm' : //切换至指令模式 mode = 1; Serial.println("Command: Switch to Instruction Mode."); break; case 'o': reportStatus(); break; case 'i': armIniPos(); break; default: Serial.println("Unknown Command."); return; } } void servoCmd(char servoName, int toPos, int servoDelay){ Servo servo2go; //创建servo对象 //串口监视器输出接收指令信息 Serial.println(""); Serial.print("+Command: Servo "); Serial.print(servoName); Serial.print(" to "); Serial.print(toPos); Serial.print(" at servoDelay value "); Serial.print(servoDelay); Serial.println("."); Serial.println(""); int fromPos; //建立变量,存储电机起始运动角度值 switch(servoName){ case 'b': if(toPos >= baseMin && toPos <= baseMax){ servo2go = base; fromPos = base.read(); // 获取当前电机角度值用于“电机运动起始角度值” break; } else { Serial.println("+Warning: Base Servo Value Out Of Limit!"); return; } case 'c': if(toPos >= clawMin && toPos <= clawMax){ servo2go = claw; fromPos = claw.read(); // 获取当前电机角度值用于“电机运动起始角度值” break; } else { Serial.println("+Warning: Claw Servo Value Out Of Limit!"); return; } case 'f': if(toPos >= fArmMin && toPos <= fArmMax){ servo2go = fArm; fromPos = fArm.read(); // 获取当前电机角度值用于“电机运动起始角度值” break; } else { Serial.println("+Warning: fArm Servo Value Out Of Limit!"); return; } case 'r': if(toPos >= rArmMin && toPos <= rArmMax){ servo2go = rArm; fromPos = rArm.read(); // 获取当前电机角度值用于“电机运动起始角度值” break; } else { Serial.println("+Warning: rArm Servo Value Out Of Limit!"); return; } } //指挥电机运行 if (fromPos <= toPos){ //如果“起始角度值”小于“目标角度值” for (int i=fromPos; i<=toPos; i++){ servo2go.write(i); delay (servoDelay); } } else { //否则“起始角度值”大于“目标角度值” for (int i=fromPos; i>=toPos; i--){ servo2go.write(i); delay (servoDelay); } } } void reportStatus(){ //舵机状态信息 Serial.println(""); Serial.println(""); Serial.println("+ Robot-Arm Status Report +"); Serial.print("Claw Position: "); Serial.println(claw.read()); Serial.print("Base Position: "); Serial.println(base.read()); Serial.print("Rear Arm Position:"); Serial.println(rArm.read()); Serial.print("Front Arm Position:"); Serial.println(fArm.read()); Serial.println("++++++++++++++++++++++++++"); Serial.println(""); } void armIniPos(){ Serial.println("+Command: Restore Initial Position."); int robotIniPosArray[4][3] = { {'b', 90, DSD}, {'r', 90, DSD}, {'f', 90, DSD}, {'c', 90, DSD} }; for (int i = 0; i < 4; i++){ servoCmd(robotIniPosArray[i][0], robotIniPosArray[i][1], robotIniPosArray[i][2]); } }
原程序只有欢迎的串口打印,没有任何功能。