finish multiaxis plan

Author: archibate

stepPlan/stepPlan.ino

@@ -1,22 +1,22 @@
-const int XSTP = 2;
-const int YSTP = 3;
-const int ZSTP = 4;
-const int XDIR = 5;
-const int YDIR = 6;
-const int ZDIR = 7;
-const int EN = 8;
-const int XLIM = 9;
-const int YLIM = 10;
-const int ZLIM = 11;
-const int ASTP = 12;
-const int ADIR = 13;
-const int ABORT = A0;
-const int HOLD = A1;
-const int RESUME = A2;
-const int COOLANT = A3;
-
-const int N_AXIS = 2;
-const int N_SEGMENTS = 1;
+const uint8_t XSTP = 2;
+const uint8_t YSTP = 3;
+const uint8_t ZSTP = 4;
+const uint8_t XDIR = 5;
+const uint8_t YDIR = 6;
+const uint8_t ZDIR = 7;
+const uint8_t EN = 8;
+const uint8_t XLIM = 9;
+const uint8_t YLIM = 10;
+const uint8_t ZLIM = 11;
+const uint8_t ASTP = 12;
+const uint8_t ADIR = 13;
+const uint8_t ABORT = A0;
+const uint8_t HOLD = A1;
+const uint8_t RESUME = A2;
+const uint8_t COOLANT = A3;
+
+const uint8_t N_AXIS = 2;
+const uint32_t N_SEGMENTS = 1;
 const float STEP_SIZE = 0.05f; // mm
 
 struct Segment {
@@ -39,12 +39,15 @@ void initMotors() {
   }
 }
 
+bool stepNow[N_AXIS];
+
 void enableMotors() {
   for (uint8_t a = 0; a < N_AXIS; ++a) {
     digitalWrite(XDIR + a, HIGH);
   }
   for (uint8_t a = 0; a < N_AXIS; ++a) {
     digitalWrite(XSTP + a, LOW);
+    stepNow[a] = 0;
   }
   digitalWrite(EN, LOW);
 }
@@ -58,7 +61,7 @@ void setDirection(uint8_t axis, bool direction) {
 }
 
 void sendStep(uint8_t axis) {
-  digitalWrite(XSTP + axis, !digitalRead(XSTP));
+  digitalWrite(XSTP + axis, (stepNow[axis] = !stepNow[axis]));
 }
 
 void delayFor(float stepTime) {
@@ -67,7 +70,7 @@ void delayFor(float stepTime) {
 
 void sendSegment(Segment const &segment) {
   uint32_t maxAxisSteps = 0;
-  uint32_t maxAxis = N_AXIS;
+  uint8_t maxAxis = N_AXIS;
   for (uint8_t a = 0; a < N_AXIS; ++a) {
     setDirection(a, segment.directions[a]);
     if (segment.steps[a] > maxAxisSteps) {
@@ -82,24 +85,28 @@ void sendSegment(Segment const &segment) {
   float topStepTime = 1.0f / sqrtf(segment.topSpeedSqr);
   float invertAcceleration =  1.0f / segment.acceleration;
   float twiceAcceleration = 2.0f * segment.acceleration;
-  int slaveAxis = 1 - maxAxis;
-  int slaveDelta = 2 * segment.steps[slaveAxis] - maxAxisSteps;
+  uint8_t slaveAxis[N_AXIS - 1];
+  int32_t slaveDelta[N_AXIS - 1];
+  for (uint8_t a = 0; a < N_AXIS - 1; ++a) {
+    slaveAxis[a] = (maxAxis + 1 + a) % N_AXIS;
+    slaveDelta[a] = 2 * segment.steps[slaveAxis[a]] - maxAxisSteps;
+  }
   for (uint32_t i = 0; i < maxAxisSteps; ++i) {
     float speed1Sqr = segment.initSpeedSqr + twiceAcceleration * i;
     float speed2Sqr = segment.exitSpeedSqr + twiceAcceleration * (maxAxisSteps - 1 - i);
     float speedMinSqr = fminf(speed1Sqr, speed2Sqr);
-    float stepTime;
-    if (speedMinSqr >= segment.topSpeedSqr) {
-      stepTime = topStepTime;
-    } else {
+    float stepTime = topStepTime;
+    if (speedMinSqr < segment.topSpeedSqr) {
       stepTime = (sqrtf(speedMinSqr + twiceAcceleration) - sqrtf(speedMinSqr)) * invertAcceleration; 
     }
     sendStep(maxAxis);
-    if (slaveDelta > 0) {
-      sendStep(slaveAxis);
-      slaveDelta -= 2 * maxAxisSteps;
+    for (uint8_t a = 0; a < N_AXIS - 1; ++a) {
+      if (slaveDelta[a] > 0) {
+        sendStep(slaveAxis[a]);
+        slaveDelta[a] -= 2 * maxAxisSteps;
+      }
+      slaveDelta[a] += 2 * segment.steps[slaveAxis[a]];
     }
-    slaveDelta += 2 * segment.steps[slaveAxis];
     delayFor(stepTime);
   }
 }
@@ -108,13 +115,13 @@ void setup() {
   Serial.begin(115200);
 
   segments[0].directions[0] = 1;
-  segments[0].steps[0] = 1000;
+  segments[0].steps[0] = 400;
   segments[0].directions[1] = 0;
-  segments[0].steps[1] = 300;
-  segments[0].acceleration = 4000;
-  segments[0].initSpeedSqr = squaref(10);
-  segments[0].topSpeedSqr = squaref(1000);
-  segments[0].exitSpeedSqr = squaref(10);
+  segments[0].steps[1] = 4000;
+  segments[0].acceleration = 100000;
+  segments[0].initSpeedSqr = squaref(100);
+  segments[0].topSpeedSqr = squaref(5000);
+  segments[0].exitSpeedSqr = squaref(100);
 
   initMotors();
   enableMotors();