1、功能描述 

      本文提供的示例所实现的功能为：在3自由度串联机械臂样机上安装电磁铁，实现电磁铁搬运物品的功能。

2、电子硬件

/*------------------------------------------------------------------------------------版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.Distributed under MIT license.See file LICENSE for detail or copy athttps://opensource.org/licenses/MITby 机器谱 2023-01-31 https://www.robotway.com/------------------------------/** Bigfish扩展板舵机口; 4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19* 使用软件调节舵机时请拖拽对应序号的控制块*/#include <Servo.h>#define ANGLE_VALUE_MIN 0#define ANGLE_VALUE_MAX 180#define PWM_VALUE_MIN 500#define PWM_VALUE_MAX 2500#define SERVO_NUM 12Servo myServo[SERVO_NUM];int data_array[2] = {0,0};   //servo_pin: data_array[0], servo_value: data_array[1];int servo_port[SERVO_NUM] = {4, 7, 11, 3, 8, 12, 14, 15, 16, 17, 18, 19};int servo_value[SERVO_NUM] = {};String data = "";boolean dataComplete = false;void setup() {Serial.begin(9600);}void loop() {while(Serial.available()){int B_flag, P_flag, T_flag;data = Serial.readStringUntil('\n');data.trim();for(int i=0;i<data.length();i++){//Serial.println(data[i]);switch(data[i]){case '#':B_flag = i;  break;case 'P':{String pin = "";P_flag = i;for(int i=B_flag+1;i<P_flag;i++){pin+=data[i];}data_array[0] = pin.toInt();}break;case 'T':{String angle = "";T_flag = i;for(int i=P_flag+1;i<T_flag;i++){angle += data[i];}data_array[1] = angle.toInt();servo_value[pin2index(data_array[0])] = data_array[1];}break;default: break;}     }/*Serial.println(B_flag);Serial.println(P_flag);Serial.println(T_flag);for(int i=0;i<2;i++){Serial.println(data_array[i]);}*/dataComplete = true;}if(dataComplete){for(int i=0;i<SERVO_NUM;i++){ServoGo(i, servo_value[i]);/*********************************串口查看输出***********************************///      Serial.print(servo_value[i]);//      Serial.print(" ");}//    Serial.println();/*********************************++++++++++++***********************************/dataComplete = false;}}void ServoStart(int which){if(!myServo[which].attached() && (servo_value[which] != 0))myServo[which].attach(servo_port[which]);else return;pinMode(servo_port[which], OUTPUT);}void ServoStop(int which){myServo[which].detach();digitalWrite(servo_port[which],LOW);}void ServoGo(int which , int where){ServoStart(which);if(where >= ANGLE_VALUE_MIN && where <= ANGLE_VALUE_MAX){myServo[which].write(where);}else if(where >= PWM_VALUE_MIN && where <= PWM_VALUE_MAX){myServo[which].writeMicroseconds(where);}}int pin2index(int _pin){int index;switch(_pin){case 4: index = 0; break;case 7: index = 1; break;case 11: index = 2; break;case 3: index = 3; break;case 8: index = 4; break;case 12: index = 5; break;case 14: index = 6; break;case 15: index = 7; break;case 16: index = 8; break;case 17: index = 9; break;case 18: index = 10; break;case 19: index = 11; break;}return index;}

 ④ 拖动进度条，可以观察相应的舵机角度转动。写好对应的舵机调试角度，勾选左下角添加-转化，获得舵机调试的数组：

 ⑤ 将该数组直接复制到相应的Arduino程序中的get_coordinate（）部分进行使用。

3.2调试好角度后将电磁铁搬运例程（calculate_angle_test.ino）下载到主控板：

/*------------------------------------------------------------------------------------版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.Distributed under MIT license.See file LICENSE for detail or copy athttps://opensource.org/licenses/MITby 机器谱 2023-01-31 https://www.robotway.com/------------------------------*/#include <math.h>#include <Servo.h>#define SERVO_SPEED 3460                                        //定义舵机转动快慢的时间#define ACTION_DELAY 200                                        //定义所有舵机每个状态时间间隔#define L1 172#define L2 160#define L3 135Servo myServo[6];int f = 200;                                                    //定义舵机每个状态间转动的次数，以此来确定每个舵机每次转动的角度int servo_port[6] = {4,7,11,3,8,12};                            //定义舵机引脚int servo_num = sizeof(servo_port) / sizeof(servo_port[0]);     //定义舵机数量float value_init[6] = {1500, 1500, 1500, 0, 0, 0};              //定义舵机初始角度double theta[3] = {};float value_pwm[6] = {};float coordinate[3] = {};int data_num;boolean dataComplete = false;void setup() {Serial.begin(9600);pinMode(9, OUTPUT);pinMode(10, OUTPUT);for(int i=0;i<servo_num;i++){ServoGo(i,value_init[i]);}get_coordinate(160, 0, 45);delay(1000);}void loop() {get_coordinate(125, 100, 45);get_coordinate(125, 100, 20);delay(1000);put();get_coordinate(125, 100, 45);get_coordinate(125, 100, 20);delay(500);get();get_coordinate(125, 100, 45);get_coordinate(158, 5, 80);get_coordinate(158, 5, 51);delay(1000);put();get_coordinate(158, 5, 80);get_coordinate(158, 5, 51);delay(500);get();get_coordinate(110, 0, 158);get_coordinate(100, -80, 116);delay(1000);put();get_coordinate(100, -80, 140);get_coordinate(100, -80, 116);delay(500);get();get_coordinate(100, -80, 140);get_coordinate(110, 0, 160);get_coordinate(158, 5, 51);delay(1000);put();get_coordinate(158, 5, 80);get_coordinate(158, 5, 51);delay(500);get();get_coordinate(160, 5, 80);delay(200);}void get_coordinate(float x, float y, float z){coordinate[0] = x;coordinate[1] = y;coordinate[2] = z;angle();}void angle(){calculate_position(coordinate);for(byte i=0;i<3;i++){  value_pwm[i] = map(theta[i], 0, 180, 500, 2500);}servo_move(value_pwm[0], value_pwm[1], value_pwm[2], 0, 0, 0);dataComplete = false;}void calculate_position(float coordinate[3]){float a, b, c, posX, posY, posZ;double theta0, theta1, theta2;a = L2;b = L3;posX = coordinate[0] == 0 ? 1 : coordinate[0];posY = coordinate[1];posZ = coordinate[2];theta0 = atan(posX / posY);c = sqrt(posX * posX / sq(sin(theta0)) + sq(posZ - L1));theta2 = acos((a * a + b * b - c * c) / (2 * a * b));theta1 = asin((posZ - L1) / c) + acos((a * a + c * c - b * b) / (2 * a * c));if(theta0 >= 0){theta[0] = theta0 * 180 / PI;}else{theta[0] = 180 + theta0 * 180 / PI;}theta[1] = 90 - theta1 * 180 / PI;theta[2] = theta2 * 180 / PI;//   Serial.print("theta0 = ");//   Serial.println(theta[0]);//   Serial.print("theta1 = ");//   Serial.println(theta[1]);//   Serial.print("theta2 = ");//   Serial.println(theta[2]);//   Serial.println("-------------------------------------");}void ServoStart(int which){if(!myServo[which].attached())myServo[which].attach(servo_port[which]);pinMode(servo_port[which], OUTPUT);}void ServoStop(int which){myServo[which].detach();digitalWrite(servo_port[which],LOW);}void ServoGo(int which , int where){if(where!=200){if(where==201) ServoStop(which);else{ServoStart(which);myServo[which].writeMicroseconds(where);}}}void servo_move(float value0, float value1, float value2, float value3, float value4, float value5){float value_arguments[] = {value0, value1, value2, value3, value4, value5};float value_delta[servo_num];for(int i=0;i<servo_num;i++){value_delta[i] = (value_arguments[i] - value_init[i]) / f;}for(int i=0;i<f;i++){for(int k=0;k<servo_num;k++){value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];}for(int j=0;j<servo_num;j++){ServoGo(j,value_init[j]);}delayMicroseconds(SERVO_SPEED);}}void get(){digitalWrite(9, HIGH);digitalWrite(10, LOW);delay(1000);}void put(){digitalWrite(9, LOW);digitalWrite(10, LOW);delay(1000);}

程序源代码​​​​​​​、样机3D文件​​​​​​​​资料内容详见 3自由度串联机械臂