EggDuino/Functions.ino

void makeComInterface(){
  SCmd.addCommand("v",sendVersion); 
  SCmd.addCommand("EM",enableMotors);
  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
  SCmd.addCommand("NI",nodeCountIncrement);
  SCmd.addCommand("ND",nodeCountDecrement);
  SCmd.addCommand("SN",setNodeCount);
  SCmd.addCommand("QN",queryNodeCount);
  SCmd.addCommand("SL",setLayer);
  SCmd.addCommand("QL",queryLayer);
  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;
	if (penState=penUpPos)
		state='1';
	else
		state='0';
	Serial.print(String(state)+"\r\n");
	sendAck();
}

void queryButton() {
	Serial.print(String(prgButtonState) +"\r\n");
	sendAck();
	prgButtonState = 0;
}
  
void queryLayer() {
  Serial.print(String(layer) +"\r\n");
  sendAck();
} 
  
void setLayer() {
	  uint32_t value=0;
	  char *arg1;
	  arg1 = SCmd.next();
	  if (arg1 != NULL) {
		  value = atoi(arg1);
		  layer=value;
		  sendAck();
	  }
	  else
	  sendError();
  }
  
void queryNodeCount() {
    Serial.print(String(nodeCount) +"\r\n");
	sendAck();
	
}

void setNodeCount() {
	uint32_t value=0;
	char *arg1;
	arg1 = SCmd.next();
	if (arg1 != NULL) {
		value = atoi(arg1);
		nodeCount=value;
		sendAck();
	}
	else
		sendError();
}

void nodeCountIncrement() {
	nodeCount=nodeCount++;
	sendAck();	
}

void nodeCountDecrement() {
	nodeCount=nodeCount--;
	sendAck();
}

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();
  }

  if (!parseSMArgs(&duration, &penStepsEBB, &rotStepsEBB)) {
    sendError();
    return;
  }

  sendAck();
  
  if ( (penStepsEBB==0) && (rotStepsEBB==0) ) {
    delay(duration);
    //sendAck();
    return;
  }

  doMove(duration, penStepsEBB, rotStepsEBB);
  //sendAck();
}

#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();
    }
  arg = SCmd.next(); 
  if (arg != NULL) {
      cmd = atoi(arg);
	  switch (cmd) {
		  case 0:
		  penServo.write(penUpPos);
		  penState=penUpPos;
		  break;
		  
		  case 1:
		  penServo.write(penDownPos);
		  penState=penDownPos;
		  break;
		  
		  default:
		  sendError();
	  }
  }
  char *val; 
  val = SCmd.next(); 
  if (val != NULL) {
		value = atoi(val);
		sendAck();
	    delay(value);
  }
  if (val==NULL && arg !=NULL)  {
		sendAck();
		delay(500);
  }
		//	Serial.println("delay");
  if (val==NULL && arg ==NULL)
			sendError();
}  

void togglePen(){
  int value;
  char *arg;
    while ( penMotor.distanceToGo() || rotMotor.distanceToGo() ) {
				penMotor.runSpeedToPosition(); // Moving.... moving... moving....
				rotMotor.runSpeedToPosition();
    }
  arg = SCmd.next(); 
  if (arg != NULL) 
      value = atoi(arg);
  else
      value = 500;

	doTogglePen();
	sendAck();
	delay(value); 
}

void doTogglePen() {
  if (penState==penUpPos) {
            penServo.write(penDownPos);
            penState=penDownPos;
  } else   {
            penServo.write(penUpPos);
            penState=penUpPos;
        }    
}

void enableMotors(){
  int cmd;
  int value;
  char *arg;
  char *val; 
  arg = SCmd.next(); 
  if (arg != NULL) 
      cmd = atoi(arg);
  val = SCmd.next(); 
  if (val != NULL) 
      value = atoi(val);
      //values parsed
  if ((arg != NULL) && (val == NULL)){
     switch (cmd) { 
       case 0: motorsOff();
               sendAck();
               break;       
       case 1: motorsOn();
               sendAck();
               break;
       default:
               sendError();
      }
}
//the following implementaion is a little bit cheated, because i did not know, how to implement different values for first and second argument.
  if ((arg != NULL) && (val != NULL)){
     switch (value) {    
       case 0: motorsOff();
               sendAck();
               break;  
       case 1: motorsOn();
               sendAck();
               break;
       default:
               sendError();
      }
  }
}

void stepperModeConfigure(){
  int cmd;
  int value;
  char *arg;
  arg = SCmd.next(); 
  if (arg != NULL) 
      cmd = atoi(arg);
  char *val; 
  val = SCmd.next(); 
  if (val != NULL) 
      value = atoi(val);
  if ((arg != NULL) && (val != NULL)){
     switch (cmd) {      
       case 4: penDownPos= (int) ((float) (value-6000)/(float) 133.3); // transformation from EBB to PWM-Servo
               storePenDownPosInEE();
               sendAck();
               break;
       case 5: penUpPos= (int)((float) (value-6000)/(float) 133.3); // transformation from EBB to PWM-Servo
               storePenUpPosInEE();
               sendAck();
               break;
       case 6: //rotMin=value;    ignored
               sendAck();
               break;
       case 7: //rotMax=value;    ignored
               sendAck();
               break;
       case 11: servoRateUp=value;
                sendAck();
                break;
       case 12: servoRateDown=value;
                sendAck();
                break;
       default:
               sendError();
      }
  }
}

void sendVersion(){
  Serial.print(initSting);
  Serial.print("\r\n");
  }
  
  void unrecognized(const char *command){
  sendError();
  }
  
  void ignore(){
  sendAck();
  }