/*
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 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[0], robotIniPosArray[1], robotIniPosArray[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);
}
}
|
楼主
标题置顶
标题高亮
