arduino写一个控制5轴机械臂到指定角度和速度可以来回运动的代码

这是一个比较复杂的项目，需要考虑到机械臂的结构、角度控制方式、运动轨迹规划等问题。以下是一个简单的示例代码，仅供参考：

#include <Servo.h>

// 机械臂关节数量
const int JOINT_NUM = 5;

// 机械臂关节对应的舵机引脚
const int JOINT_PINS[JOINT_NUM] = {2, 3, 4, 5, 6};

// 机械臂关节的初始角度
const int JOINT_ANGLES[JOINT_NUM] = {90, 90, 90, 90, 90};

// 机械臂关节的最小角度
const int JOINT_MIN_ANGLES[JOINT_NUM] = {0, 0, 0, 0, 0};

// 机械臂关节的最大角度
const int JOINT_MAX_ANGLES[JOINT_NUM] = {180, 180, 180, 180, 180};

// 机械臂运动速度
const int SPEED = 50;

// 机械臂运动方向
const int DIRECTION[2] = {1, -1};

// Servo 对象
Servo joints[JOINT_NUM];

// 机械臂当前角度
int joint_angles[JOINT_NUM];

// 初始化机械臂
void init_arm() {
  for (int i = 0; i < JOINT_NUM; i++) {
    joints[i].attach(JOINT_PINS[i]);
    joint_angles[i] = JOINT_ANGLES[i];
    joints[i].write(joint_angles[i]);
  }
}

// 将角度限制在最小值和最大值之间
int limit_angle(int angle, int min_angle, int max_angle) {
  if (angle < min_angle) {
    angle = min_angle;
  } else if (angle > max_angle) {
    angle = max_angle;
  }
  return angle;
}

// 控制机械臂到指定角度
void set_joint_angles(int angles[JOINT_NUM]) {
  for (int i = 0; i < JOINT_NUM; i++) {
    joint_angles[i] = limit_angle(angles[i], JOINT_MIN_ANGLES[i], JOINT_MAX_ANGLES[i]);
    joints[i].write(joint_angles[i]);
  }
}

// 计算机械臂在两个角度之间的运动轨迹
void calculate_trajectory(int start_angles[JOINT_NUM], int end_angles[JOINT_NUM], int trajectory[JOINT_NUM][SPEED]) {
  for (int i = 0; i < JOINT_NUM; i++) {
    int diff = end_angles[i] - start_angles[i];
    int step = diff / SPEED;
    for (int j = 0; j < SPEED; j++) {
      trajectory[i][j] = start_angles[i] + step * j;
    }
  }
}

// 控制机械臂运动
void run_arm() {
  int start_angles[JOINT_NUM];
  int end_angles[JOINT_NUM];
  int trajectory[JOINT_NUM][SPEED];

  // 从初始角度运动到最大角度
  for (int i = 0; i < JOINT_NUM; i++) {
    start_angles[i] = joint_angles[i];
    end_angles[i] = JOINT_MAX_ANGLES[i];
  }
  for (int j = 0; j < 2; j++) {
    calculate_trajectory(start_angles, end_angles, trajectory);
    for (int i = 0; i < SPEED; i++) {
      int angles[JOINT_NUM];
      for (int k = 0; k < JOINT_NUM; k++) {
        angles[k] = trajectory[k][i];
      }
      set_joint_angles(angles);
      delay(20);
    }
  }

  // 从最大角度运动到最小角度
  for (int i = 0; i < JOINT_NUM; i++) {
    start_angles[i] = joint_angles[i];
    end_angles[i] = JOINT_MIN_ANGLES[i];
  }
  for (int j = 0; j < 2; j++) {
    calculate_trajectory(start_angles, end_angles, trajectory);
    for (int i = 0; i < SPEED; i++) {
      int angles[JOINT_NUM];
      for (int k = 0; k < JOINT_NUM; k++) {
        angles[k] = trajectory[k][i];
      }
      set_joint_angles(angles);
      delay(20);
    }
  }
}

void setup() {
  init_arm();
}

void loop() {
  run_arm();
}

这个代码通过 Servo 库控制舵机实现机械臂的运动。首先需要定义机械臂的结构、舵机引脚、初始角度、最小角度、最大角度等参数。然后在 `init_arm()` 函数中初始化舵机，将机械臂关节的角度设置为初始角度。在 `set_joint_angles()` 函数中，将机械臂关节的角度限制在最小值和最大值之间，并通过 `joints[i].write(joint_angles[i])` 控制舵机转动到对应的角度。

在 `calculate_trajectory()` 函数中，根据起始角度和目标角度计算出机械臂在两个角度之间的运动轨迹。这个函数将机械臂的运动分为若干步，每一步角度变化相等。在 `run_arm()` 函数中，通过多次调用 `calculate_trajectory()` 和 `set_joint_angles()` 函数控制机械臂运动到指定角度，并通过 `delay()` 函数控制运动速度。

注意，这个代码只是一个示例，需要根据实际情况进行修改和优化。