Fixing tiny bugs in grabber code

4DrobotServer/4DrobotServer_globalVars.h

@@ -37,8 +37,8 @@ unsigned long      temp_update_prev = 0;       //[ms]
 
 Servo grabber_servo;
 bool  grabber_go   = false;
+int   grabber_goto = 0;
 int   grabber_pos  = grabber_closed; //Assumed initial position
-int   grabber_goto = grabber_closed;
 
 // Stepper motors
 

4DrobotServer/ServoControl.ino

@@ -16,27 +16,34 @@ void setup_servo() {
 
 //Program for controlling the servomotor for the grabber
 void servo_control() {
-  if (grabber_go) {
-    do {
-      if (grabber_pos < grabber_goto) {
-        grabber_pos++;
-      }
-      else {
-        grabber_pos--;
-      }
-      
-      #ifndef DUMMY_SERVO
-      grabber_servo.write(grabber_pos);
-      #else
-      Serial.print("Grabber to: ");
-      Serial.print(grabber_pos);
-      Serial.print('\n');
-      #endif
-      delay(grabber_stepWait);
-    } while (grabber_pos != grabber_goto);
-    
-    grabber_go  = false;
+  if (not grabber_go) {
+    return;
+  }
 
-    bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GRABBER GO FINISHED\n"));
+  //Move!
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GRABBER MOVING...\n"));
+  output_buff_flush();
+  
+  while (grabber_pos != grabber_goto) {
+    if (grabber_pos < grabber_goto) {
+      grabber_pos++;
+    }
+    else {
+      grabber_pos--;
+    }
+
+    #ifndef DUMMY_SERVO
+    grabber_servo.write(grabber_pos);
+    #else
+    Serial.print("Grabber to: ");
+    Serial.print(grabber_pos);
+    Serial.print('\n');
+    #endif
+    delay(grabber_stepWait);
   }
+ 
+  grabber_go   = false;
+  stepper_goto = 0;
+
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GRABBER GO FINISHED\n"));
 }

4DrobotServer/StepperControl.ino

@@ -29,7 +29,7 @@ void stepper_control() {
   //Move!
   bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< STEPPER MOVING "));
   bufferWrite(output_buff, sizeof(output_buff), output_buffCount, stepper_axnames[stepper_go_axis]);
-  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, '\n');
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("...\n"));
   output_buff_flush();
 
   //Task: Zeroseek an axis