From be9db779422ccccbfbe5f9fb32e2a7499386be9a Mon Sep 17 00:00:00 2001
From: 18650180552 <a7458969@gmail.com>
Date: Fri, 17 Apr 2020 23:38:16 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B5=8B=E8=AF=95=E5=BE=97=E5=88=B0=E5=8F=AF?=
 =?UTF-8?q?=E7=94=A8=E6=AF=8F=E4=B8=AA=E8=BD=B4=E6=97=8B=E8=BD=AC=E4=B8=80?=
 =?UTF-8?q?=E5=91=A8=E7=9A=84=E6=AD=A5=E8=BF=9B=E6=95=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../MultiStepperTest/MultiStepperTest.ino     | 109 ++++++++++++++++++
 1 file changed, 109 insertions(+)
 create mode 100644 arduino/arduino/MultiStepperTest/MultiStepperTest.ino

diff --git a/arduino/arduino/MultiStepperTest/MultiStepperTest.ino b/arduino/arduino/MultiStepperTest/MultiStepperTest.ino
new file mode 100644
index 0000000..8c18200
--- /dev/null
+++ b/arduino/arduino/MultiStepperTest/MultiStepperTest.ino
@@ -0,0 +1,109 @@
+// MultiStepper.pde
+// -*- mode: C++ -*-
+// Use MultiStepper class to manage multiple steppers and make them all move to 
+// the same position at the same time for linear 2d (or 3d) motion.
+
+#include <AccelStepper.h>
+#include <MultiStepper.h>
+
+// Joint 1
+#define E1_STEP_PIN   53
+#define E1_DIR_PIN   51
+#define E1_ENABLE_PIN 31
+
+// Joint 2
+#define Z_STEP_PIN         32
+#define Z_DIR_PIN          30
+#define Z_ENABLE_PIN       62
+#define Z_MIN_PIN          18
+#define Z_MAX_PIN          19
+
+// Joint 3
+#define Y_STEP_PIN         48
+#define Y_DIR_PIN          46
+#define Y_ENABLE_PIN       56
+#define Y_MIN_PIN          14
+#define Y_MAX_PIN          15
+
+// Joint 4
+#define X_STEP_PIN 40
+#define X_DIR_PIN 42
+#define X_ENABLE_PIN 38
+
+// Joint 5 
+#define E0_STEP_PIN 43
+#define E0_DIR_PIN 41
+#define E0_ENABLE_PIN 24
+
+
+// EG X-Y position bed driven by 2 steppers
+// Alas its not possible to build an array of these with different pins for each :-(
+AccelStepper joint1(1,E1_STEP_PIN, E1_DIR_PIN);
+AccelStepper joint2(1,Z_STEP_PIN, Z_DIR_PIN);
+AccelStepper joint3(1,Y_STEP_PIN, Y_DIR_PIN);
+AccelStepper joint4(1,X_STEP_PIN, X_DIR_PIN);
+AccelStepper joint5(1, E0_STEP_PIN, E0_DIR_PIN);
+
+// Up to 10 steppers can be handled as a group by MultiStepper
+MultiStepper steppers;
+
+//test with uint8 converted to long
+unsigned int x = 1000;
+
+void setup() {
+  Serial.begin(115200);
+
+  // Configure each stepper
+  joint1.setMaxSpeed(100);
+  joint2.setMaxSpeed(200);
+  joint3.setMaxSpeed(500);
+  joint4.setMaxSpeed(500);
+  joint5.setMaxSpeed(200);
+
+  // Then give them to MultiStepper to manage
+  steppers.addStepper(joint1);
+  steppers.addStepper(joint2);
+  steppers.addStepper(joint3);
+  steppers.addStepper(joint4);
+  steppers.addStepper(joint5);
+
+  pinMode(22, OUTPUT);
+  pinMode(26, OUTPUT);
+  pinMode(24, INPUT);
+  digitalWrite(22,0);
+  digitalWrite(26,0);
+
+}
+
+/// link1 -960*4
+/// link2 -2250*4
+/// link3 -2780*4
+/// link4 790*4
+/// link5 220*4
+
+void loop() {
+  if(digitalRead(24) == 1){
+    Serial.print("enter test mode");
+    long positions[5]; // Array of desired stepper positions
+  
+    // Back of the envelope calculation for microsteps/revolution, where positions[i] is the number of steps (or microsteps).
+    positions[0] = 0; //4100 microsteps is 1/8 revolutions ----> 32800 microsteps/rev
+    positions[1] = 0; //2000 is 40/360 revolutions ---> 18000 microsteps/rev
+    positions[2] = 0; //4000 is 20/360 revolutions ---> 72000 microsteps/rev
+    positions[3] = 0; //820 is 1/4 revolution (200steps/revolution * 16microsteps/step (since microstepping) ~= 32800 microsteps/rev)
+    positions[4] = 0; //2000 is 50/360 revolution ---> 14400
+    
+    steppers.moveTo(positions);
+    steppers.runSpeedToPosition(); // Blocks until all are in position
+    delay(3000);
+    // Move to a different coordinate
+    positions[0] = 0;
+    positions[1] = 0;
+    positions[2] = 0;
+    positions[3] = 0;
+    positions[4] = 220;
+    steppers.moveTo(positions);
+    steppers.runSpeedToPosition(); // Blocks until all are in position
+    delay(5000);
+  }
+}