[WIP] notice: take care when homing

firmware/firmware.ino

@@ -21,9 +21,12 @@ int HOME = 11;
 int FAULT = 12;
 int motors[6] = {2, 3, 4, 5, 6, 7};
 
+
 // variables
 int i = 0;  // for looping
+int j = 0;  // second loop variable
 int u = 1;  // microstepping amount
+int selected = 0; // selected motor
 #define INPUT_SIZE 100  // TODO: make this a reasonable value
 #define sep " "
 char input[INPUT_SIZE + 1];
@@ -35,6 +38,10 @@ int number = 0;
 int remaining[6];
 bool home_after[6] = {false, false, false, false, false, false};
 unsigned long prev = 0;
+int interval = 5;  // wait time for main loop; can be extended if needed
+int default_interval = 5;  
+int homed = false;
+int ms_step = 2;  // delay between up and down steps
 
 void setup() {
   // initialize pins
@@ -67,34 +74,41 @@ void setup() {
 }
 
 void loop() {
+  // each loop we check all motors and perform a step on each line if needed
+
   // interval comparison avoids overflow issues
   unsigned long waited = millis() - prev;
 
-  if (waited >= 5){
+  if (waited >= interval){
     prev += waited;
+    interval = default_interval;  // reset interval
     for (i = 0; i <= 5; i++){
       if (remaining[i] > 0) {
         stepMotor(i);
         --remaining[i];
-      }  
+        // Serial.println(String(remaining[i]));
+      }
+
       else if (home_after[i]) { // ready to home after moving off interrupt
         setSelect(i);
         setDirection(HIGH);
         home_after[i] = false;
         remaining[i] = -1;
       }
-      if (remaining[i] == -1) {  // motor currently homing
-        setSelect(i);
+      else if (remaining[i] == -1) {  // motor currently homing
         stepMotor(i);
         // interrupt is low when blocked
-        if (digitalRead(HOME) == 0) remaining[i] = 0;
+        setSelect(i);
+        // read homed twice to make sure
+        if (carefulReadHome()) remaining[i] = 1; // go one past the limit to avoid unstable readings
       }
     }
   }
 }
 
 void serialEvent() {  // occurs whenever new data comes in the hardware serial RX
   // read serial into input char array
+  // Serial.println("serial event");
   byte size = Serial.readBytesUntil('\n', input, INPUT_SIZE);
   input[size] = 0;
   // parse input
@@ -108,6 +122,7 @@ void serialEvent() {  // occurs whenever new data comes in the hardware serial R
     Serial.println("INVALID");
     return;
   }
+  // Serial.println(String(index));
   // call appropriate function
   if (*code == 'M') {  // move relative
     setSelect(index);
@@ -121,7 +136,7 @@ void serialEvent() {  // occurs whenever new data comes in the hardware serial R
     // first, we must handle the special case where the motor
     //   is already at the interrupt
     setSelect(index);
-    if (digitalRead(HOME) == 0) {  // interrupt is low when blocked
+    if (carefulReadHome()) {  // interrupt is low when blocked
       // issue instruction to move 1/4 turn counter-clockwise
       setDirection(LOW);
       remaining[index] = 100 * u;
@@ -135,24 +150,31 @@ void serialEvent() {  // occurs whenever new data comes in the hardware serial R
     }
   }
   else if (*code == 'Q') {  // query motor status
-    setSelect(index);
-    if (digitalRead(FAULT) == 0) Serial.println('F');
-    else if (remaining[index] == 0) Serial.println('R');
+    // // ignore faults for now
+    // setSelect(index);
+    // if (digitalRead(FAULT) == 0) Serial.println('F');
+    if (remaining[index] == 0 and not home_after[index]) Serial.println('R');
     else Serial.println('B');
   }
+  else if (*code == '?') { // special code for debug
+    setSelect(index);
+    if (digitalRead(HOME)==0) Serial.println('HOME=LOW');
+    else Serial.println('HOME=HIGH');
+    Serial.println(String(remaining[index]));
+  }
   else if (*code == 'U') {  // set microstep integer
     u = index;
     setM();
   }
 }
 
+
 void serialEventRun(void) {
   // this runs inside of the main loop, essentially
   if (Serial.available()) serialEvent();
 }
 
 void setDirection(int dir) {
-  // direction is latched in using TI SN74HC259
   // facing the motor...
   //   LOW = counter clockwise
   //   HIGH = clockwise
@@ -163,9 +185,30 @@ void setDirection(int dir) {
 
 void setSelect(int index) {
   // selection reveals HOME and DIR pins for a specific motor
-  digitalWrite(S0, bitRead(index, 0));
-  digitalWrite(S1, bitRead(index, 1));
-  digitalWrite(S2, bitRead(index, 2));
+  if (index != selected) {
+    digitalWrite(S0, bitRead(index, 0));
+    digitalWrite(S1, bitRead(index, 1));
+    digitalWrite(S2, bitRead(index, 2));
+    selected = index;
+  }
+}
+
+bool carefulReadHome() {
+  // blocking, careful measurement of HOME status
+  // wait for 5 consistent measurements (with delay)
+  while (true) {
+    homed = 0;
+    for (j = 0; j < 5; j++){
+      delay(1);
+      homed += digitalRead(HOME);
+      // Serial.println(String(homed));
+    }
+    if (homed % 5 == 0) break;
+    // else Serial.println("retry");  // how often does this happen?
+  }
+  // if (homed == 5) Serial.println("HOME=HIGH");
+  // else Serial.println("HOME=LOW");
+  return (homed == 0);
 }
 
 void setM() {
@@ -208,7 +251,10 @@ void setM() {
 
 void stepMotor(int index) {
   digitalWrite(motors[index], HIGH);
-  delay(1);
+  delay(ms_step);
   digitalWrite(motors[index], LOW);
+  interval += ms_step;  // ensure time between the low-to-high step
+  // ensure we have a minimum delay equal to the delay between HIGH and LOW writing
+  // BUG: if we are already late, we may not actually add enough to interval
 }