项目代码
- 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);
- }
- }
复制代码
|