| 项目代码 
 
 复制代码int speedSetting;
int stepCounter;
int calibrationSpeed = 5;
int bottomArmPosition, middleArmPosition, topArmPosition;
byte incommingByte;
void setup() {
  Serial.begin(57600);
  //Magnet
  pinMode (A0, OUTPUT); //Magnet coil
  //Bottom arm
  pinMode(2, OUTPUT); //IN1
  pinMode(3, OUTPUT); //IN2
  pinMode(4, OUTPUT); //IN3
  pinMode(5, OUTPUT); //IN4
  //Middle arm
  pinMode(6, OUTPUT); //IN1
  pinMode(7, OUTPUT); //IN2
  pinMode(8, OUTPUT); //IN3
  pinMode(9, OUTPUT); //IN4
  //Top arm
  pinMode(10, OUTPUT); //IN4
  pinMode(11, OUTPUT); //IN3
  pinMode(12, OUTPUT); //IN2
  pinMode(13, OUTPUT); //IN1
  digitalWrite(2, HIGH);
  digitalWrite(6, HIGH);
  digitalWrite(10, HIGH);
  digitalWrite(A0, LOW);
  delay(5000);
  calibrateArms();
  bottomArmMove(120);
  middleArmMove(-290);
  topArmMove(170);
  bottomArmMove(-46);
}
void loop() {
  Serial.print("B=");
  Serial.print(bottomArmPosition);
  Serial.print(" M=");
  Serial.print(middleArmPosition);
  Serial.print(" T=");
  Serial.print(topArmPosition);
  Serial.print(" C=");
  Serial.println(digitalRead(A0));
  digitalWrite(A0, HIGH);
  delay(500);
  bottomArmMove(79);
  middleArmMove(70);
  bottomArmMove(-45);
  digitalWrite(A0, LOW);
  topArmMove(120);
  middleArmMove(-70);
  topArmMove(-120);
  bottomArmMove(-34);
  if (Serial.available()) {
    incommingByte = Serial.read();
    if (incommingByte == '7')bottomArmMove(10);
    if (incommingByte == '1')bottomArmMove(-10);
    if (incommingByte == '8')middleArmMove(10);
    if (incommingByte == '2')middleArmMove(-10);
    if (incommingByte == '9')topArmMove(10);
    if (incommingByte == '3')topArmMove(-10);
    if (incommingByte == '4')digitalWrite(A0, HIGH);
    if (incommingByte == '6')digitalWrite(A0, LOW);
    Serial.print("B=");
    Serial.print(bottomArmPosition);
    Serial.print(" M=");
    Serial.print(middleArmPosition);
    Serial.print(" T=");
    Serial.print(topArmPosition);
    Serial.print(" C=");
    Serial.println(digitalRead(A0));
    delay(100);
    while (Serial.available())incommingByte = Serial.read();
  }
}
void topArmMove(int steps) {
  topArmPosition += steps;
  speedSetting = 20;
  if (steps == 0)return;
  digitalWrite(10, HIGH);
  if (steps > 0) {
    for (stepCounter = 0 ; stepCounter < steps; stepCounter ++) {
      digitalWrite(10, LOW);
      digitalWrite(11, HIGH);
      delay(speedSetting);
      digitalWrite(11, LOW);
      digitalWrite(12, HIGH);
      delay(speedSetting);
      digitalWrite(12, LOW);
      digitalWrite(13, HIGH);
      delay(speedSetting);
      digitalWrite(13, LOW);
      digitalWrite(10, HIGH);
      delay(speedSetting);
      if (stepCounter > steps - 20 && speedSetting < 20)speedSetting += 2;
      if (speedSetting > 4)speedSetting --;
    }
  }
  else {
    steps *= -1;
    for (stepCounter = 0 ; stepCounter < steps; stepCounter ++) {
      digitalWrite(10, LOW);
      digitalWrite(13, HIGH);
      delay(speedSetting);
      digitalWrite(13, LOW);
      digitalWrite(12, HIGH);
      delay(speedSetting);
      digitalWrite(12, LOW);
      digitalWrite(11, HIGH);
      delay(speedSetting);
      digitalWrite(11, LOW);
      digitalWrite(10, HIGH);
      delay(speedSetting);
      if (stepCounter > steps - 20 && speedSetting < 20)speedSetting += 2;
      if (speedSetting > 4)speedSetting --;
    }
  }
  digitalWrite(11, LOW);
  digitalWrite(12, LOW);
  digitalWrite(13, LOW);
}
void middleArmMove(int steps) {
  middleArmPosition += steps;
  speedSetting = 20;
  if (steps == 0)return;
  digitalWrite(3, HIGH);
  if (steps > 0) {
    for (stepCounter = 0 ; stepCounter < steps; stepCounter ++) {
      digitalWrite(3, LOW);
      digitalWrite(5, HIGH);
      delay(speedSetting);
      digitalWrite(2, LOW);
      digitalWrite(4, HIGH);
      delay(speedSetting);
      digitalWrite(5, LOW);
      digitalWrite(3, HIGH);
      delay(speedSetting);
      digitalWrite(4, LOW);
      digitalWrite(2, HIGH);
      delay(speedSetting);
      if (stepCounter > steps - 20 && speedSetting < 20)speedSetting += 2;
      if (speedSetting > 4)speedSetting --;
    }
  }
  else {
    steps *= -1;
    for (stepCounter = 0 ; stepCounter < steps; stepCounter ++) {
      digitalWrite(2, LOW);
      digitalWrite(4, HIGH);
      delay(speedSetting);
      digitalWrite(3, LOW);
      digitalWrite(5, HIGH);
      delay(speedSetting);
      digitalWrite(4, LOW);
      digitalWrite(2, HIGH);
      delay(speedSetting);
      digitalWrite(5, LOW);
      digitalWrite(3, HIGH);
      delay(speedSetting);
      if (stepCounter > steps - 20 && speedSetting < 20)speedSetting += 2;
      if (speedSetting > 4)speedSetting --;
    }
  }
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
}
void bottomArmMove(int steps) {
  bottomArmPosition += steps;
  speedSetting = 20;
  if (steps == 0)return;
  digitalWrite(7, HIGH);
  if (steps > 0) {
    for (stepCounter = 0 ; stepCounter < steps; stepCounter ++) {
      digitalWrite(7, LOW);
      digitalWrite(9, HIGH);
      delay(speedSetting);
      digitalWrite(6, LOW);
      digitalWrite(8, HIGH);
      delay(speedSetting);
      digitalWrite(9, LOW);
      digitalWrite(7, HIGH);
      delay(speedSetting);
      digitalWrite(8, LOW);
      digitalWrite(6, HIGH);
      delay(speedSetting);
      if (stepCounter > steps - 20 && speedSetting < 20)speedSetting += 2;
      if (speedSetting > 4)speedSetting --;
    }
  }
  else {
    steps *= -1;
    for (stepCounter = 0 ; stepCounter < steps; stepCounter ++) {
      digitalWrite(6, LOW);
      digitalWrite(8, HIGH);
      delay(speedSetting);
      digitalWrite(7, LOW);
      digitalWrite(9, HIGH);
      delay(speedSetting);
      digitalWrite(8, LOW);
      digitalWrite(6, HIGH);
      delay(speedSetting);
      digitalWrite(9, LOW);
      digitalWrite(7, HIGH);
      delay(speedSetting);
      if (stepCounter > steps - 20 && speedSetting < 20)speedSetting += 2;
      if (speedSetting > 4)speedSetting --;
    }
  }
  digitalWrite(7, LOW);
  digitalWrite(8, LOW);
  digitalWrite(9, LOW);
}
void calibrateArms(void) {
  //Set top arm position
  for (stepCounter = 0 ; stepCounter < 300; stepCounter ++) {
    digitalWrite(10, LOW);
    digitalWrite(13, HIGH);
    delay(calibrationSpeed);
    digitalWrite(13, LOW);
    digitalWrite(12, HIGH);
    delay(calibrationSpeed);
    digitalWrite(12, LOW);
    digitalWrite(11, HIGH);
    delay(calibrationSpeed);
    digitalWrite(11, LOW);
    digitalWrite(10, HIGH);
    delay(calibrationSpeed);
  }
  //Set middle arm position
  for (stepCounter = 0 ; stepCounter < 300; stepCounter ++) {
    digitalWrite(2, LOW);
    digitalWrite(5, HIGH);
    delay(calibrationSpeed);
    digitalWrite(5, LOW);
    digitalWrite(4, HIGH);
    delay(calibrationSpeed);
    digitalWrite(4, LOW);
    digitalWrite(3, HIGH);
    delay(calibrationSpeed);
    digitalWrite(3, LOW);
    digitalWrite(2, HIGH);
    delay(calibrationSpeed);
  }
  //Set bottom arm position
  for (stepCounter = 0 ; stepCounter < 300; stepCounter ++) {
    digitalWrite(6, LOW);
    digitalWrite(7, HIGH);
    delay(calibrationSpeed);
    digitalWrite(7, LOW);
    digitalWrite(8, HIGH);
    delay(calibrationSpeed);
    digitalWrite(8, LOW);
    digitalWrite(9, HIGH);
    delay(calibrationSpeed);
    digitalWrite(9, LOW);
    digitalWrite(6, HIGH);
    delay(calibrationSpeed);
  }
}
 
 
 |