Code cleanup.

- firmware now supports SM buffering, so SMQB command is no longer needed
- commmented code and repeated fragments removed

EggDuino.ino

@@ -45,7 +45,6 @@ Thanks to my wife and my daughter for their patience. :-)
 // #define penToggleButton 12 // pen up/down button
 // #define motorsButton 4 // motors enable button
 
-// #define CommandSMQB
 //-----------------------------------------------------------------------------------------------------------
 
   #define penUpPosEEAddress ((uint16_t *)0)
@@ -92,15 +91,12 @@ void setup() {
 }
 
 void loop() {
-	if ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
-		penMotor.runSpeedToPosition(); // Moving.... moving... moving....
-		rotMotor.runSpeedToPosition();
-	}
+	moveOneStep();
 	
 	SCmd.readSerial();
 	
 	#ifdef penToggleButton
-	   penToggle.check();
+		penToggle.check();
 	#endif
 	
 	#ifdef motorsButton

Functions.ino

@@ -5,9 +5,6 @@ void makeComInterface(){
   SCmd.addCommand("SC",stepperModeConfigure);
   SCmd.addCommand("SP",setPen);
   SCmd.addCommand("SM",stepperMove);
-  #ifdef CommandSMQB
-	  SCmd.addCommand("SMQB",stepperMoveQueryButton); // composite function enabling smooth movement
-  #endif
   SCmd.addCommand("SE",ignore);
   SCmd.addCommand("TP",togglePen);
   SCmd.addCommand("PO",ignore);    //Engraver command, not implemented, gives fake answer
@@ -20,7 +17,7 @@ void makeComInterface(){
   SCmd.addCommand("QP",queryPen);
   SCmd.addCommand("QB",queryButton);  //"PRG" Button, 
   SCmd.setDefaultHandler(unrecognized); // Handler for command that isn't matched (says "What?") 
-  }
+}
 
 void queryPen() {
 	char state;
@@ -89,10 +86,8 @@ void stepperMove() {
   uint16_t duration=0; //in ms
   int penStepsEBB=0; //Pen
   int rotStepsEBB=0; //Rot
-  while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
-	  		penMotor.runSpeedToPosition(); // Moving.... moving... moving....
-	  		rotMotor.runSpeedToPosition();
-  }
+
+  moveToDestination();
 
   if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB)) {
     sendError();
@@ -103,46 +98,19 @@ void stepperMove() {
   
   if ( (penStepsEBB==0) && (rotStepsEBB==0) ) {
     delay(duration);
-    //sendAck();
     return;
   }
 
-  doMove(duration, penStepsEBB, rotStepsEBB);
-  //sendAck();
+  prepareMove(duration, penStepsEBB, rotStepsEBB);
 }
 
-#ifdef CommandSMQB
-	void stepperMoveQueryButton() {
-	  uint16_t duration=0; //in ms
-	  int penStepsEBB=0; //Pen
-	  int rotStepsEBB=0; //Rot
-
-	  if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB)) {
-		sendError();
-		return;
-	  }
-
-	  if ( (penStepsEBB==0) && (rotStepsEBB==0) ) {
-		delay(duration);
-		queryButton();
-		return;
-	  }
-
-	  // sending ACK before actual move to allow buffering 
-	  queryButton();
-	  doMove(duration, penStepsEBB, rotStepsEBB);
-	}
-#endif
-
-
 void setPen(){
   int cmd;
   int value;
   char *arg;
-    while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
-	    penMotor.runSpeedToPosition(); // Moving.... moving... moving....
-	    rotMotor.runSpeedToPosition();
-    }
+
+  moveToDestination();
+
   arg = SCmd.next(); 
   if (arg != NULL) {
       cmd = atoi(arg);
@@ -180,10 +148,9 @@ void setPen(){
 void togglePen(){
   int value;
   char *arg;
-    while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
-				penMotor.runSpeedToPosition(); // Moving.... moving... moving....
-				rotMotor.runSpeedToPosition();
-    }
+
+  moveToDestination();
+
   arg = SCmd.next(); 
   if (arg != NULL) 
       value = atoi(arg);
@@ -286,12 +253,12 @@ void stepperModeConfigure(){
 void sendVersion(){
   Serial.print(initSting);
   Serial.print("\r\n");
-  }
+}
   
-  void unrecognized(const char *command){
+void unrecognized(const char *command){
   sendError();
-  }
+}
   
-  void ignore(){
+void ignore(){
   sendAck();
-  }
+}

Helper_Functions.ino

@@ -7,18 +7,6 @@ void initHardware(){
   pinMode(enableRotMotor, OUTPUT); 
   pinMode(enablePenMotor, OUTPUT);  
 
-  	#ifdef prgButton
-		pinMode(prgButton, INPUT_PULLUP);
-  	#endif
-	  
-	#ifdef penToggleButton
-		pinMode(penToggleButton, INPUT_PULLUP);
-	#endif
-	
-	#ifdef motorsButton
-		pinMode(motorsButton, INPUT_PULLUP);
-	#endif
-
   rotMotor.setMaxSpeed(2000.0);
   rotMotor.setAcceleration(10000.0);
   penMotor.setMaxSpeed(2000.0);
@@ -26,7 +14,7 @@ void initHardware(){
   motorsOff();
   penServo.attach(servoPin);
   penServo.write(penState);
-  }
+}
 
 void inline loadPenPosFromEE() {
   penUpPos = eeprom_read_word(penUpPosEEAddress);
@@ -92,7 +80,7 @@ bool parseSMArgs(uint16_t *duration, int *penStepsEBB, int *rotStepsEBB) {
 	return false;
 }
 
-void doMove(uint16_t duration, int penStepsEBB, int rotStepsEBB) {
+void prepareMove(uint16_t duration, int penStepsEBB, int rotStepsEBB) {
 	if (!motorsEnabled) {
 		motorsOn();
 	}
@@ -126,12 +114,20 @@ void doMove(uint16_t duration, int penStepsEBB, int rotStepsEBB) {
 		penMotor.move(penStepsToGo);
 		penMotor.setSpeed( penSpeed );
 	}
-/*
+}
+
+void moveOneStep() {
+	if ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
+		penMotor.runSpeedToPosition(); // Moving.... moving... moving....
+		rotMotor.runSpeedToPosition();
+	}
+}
+
+void moveToDestination() {
 	while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
 		penMotor.runSpeedToPosition(); // Moving.... moving... moving....
 		rotMotor.runSpeedToPosition();
 	}
-	*/
 }
 
 void setprgButtonState(){

button.h

@@ -29,7 +29,10 @@ private:
 	
 public:
 	
-	Button(byte p, ActionCb a): debounce(0), state(1), lastState(1), action(a), pin(p) {} ;
+	Button(byte p, ActionCb a): debounce(0), state(1), lastState(1), action(a), pin(p) {
+		pinMode(pin, INPUT_PULLUP);
+	}
+
 	void check() {
 		byte b = digitalRead(pin);
 		long t = millis();
@@ -49,10 +52,7 @@ public:
 		}
 
 		lastState = b;
-	};
-
-
-
+	}
 }; //button
 
 #endif //__BUTTON_H__