Exemplo n.º 1
0
void Servotor32::process(Stream *serial){
  if(serial->available()) { //process input from the USB
    char inChar = (char)serial->read();
    switch(inChar){
      case '#':
        servoCounting = true;
        numCount = 0;
        inServo = -1;
        inPos = -1;
        break;
      case 'D':
        printStatus(serial);
        break;
      case 'P':
        if(servoCounting){
          inServo = tallyCount();
          servoCounting = false;
        }
        posCounting =  true;
        numCount = 0;
        break;
      case '\r':
      case '\n':
        if(posCounting){
          inPos = tallyCount();
          posCounting = false;
        }
        if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
          changeServo(inServo,inPos);
          inServo = -1;
          inPos = -1;
        }
        numCount = 0;
        break;
      case 'V':
        serial->println("SERVOTOR32_v2.0_lams");
        break;
      case 'C':
        for(int i=0; i<32; i++){
          changeServo(i, 1500);
        }
        serial->println("All Centered");
        break;
      case 'K':
        for(int i=0; i<32; i++){
          changeServo(i,-1);
        }
        serial->println("All Turned Off");
        break;
      case 'L':
        if(servoCounting){
          inServo = tallyCount();
          servoCounting = false;
        }
        changeServo(inServo, -1);
        break;
      case 'S': // make an arcPing and send distances back over serial
        arcPing();
        for(int i = 0; i < ARCPING_STEPS; i++)
        {
          serial->println(arcPingDistances[i]);
        }
        break;
      default:
        if((inChar > 47)&&(inChar < 58)){
          if(numCount<4){
            numString[numCount] = inChar-48;
            numCount++;
          }
        }
        break;
    }
  }
}
Exemplo n.º 2
0
void Servotor32::processChar(char inChar){
	if (binary) {
	  switch(inChar){
		case '\r':
		case '\n':
			binary = false;
                        changeServo(5,inAllPos[0]);
                        changeServo(6,inAllPos[1]);
                        changeServo(7,inAllPos[2]);
                        changeServo(9,inAllPos[3]);
                        changeServo(10,inAllPos[4]);
                        changeServo(11,inAllPos[5]);
                        changeServo(13,inAllPos[6]);
                        changeServo(14,inAllPos[7]);
                        changeServo(15,inAllPos[8]);
                        changeServo(16,inAllPos[9]);
                        changeServo(17,inAllPos[10]);
                        changeServo(18,inAllPos[11]);
                        changeServo(20,inAllPos[12]);
                        changeServo(21,inAllPos[13]);
                        changeServo(22,inAllPos[14]);
                        changeServo(24,inAllPos[15]);
                        changeServo(25,inAllPos[16]);
                        changeServo(26,inAllPos[17]);
                        changeServo(31,inAllPos[18]);
		  break;
		default:
                  inAllPos[numCount] = byte(inChar)*10;
                  numCount++;
		  break;
	   }
        }
	else {
	  switch(inChar){
		case '$':
		  binary = true;
		  numCount = 0;
		  break;
		case '#':
		  servoCounting = true;
		  numCount = 0;
		  inServo = -1;
		  inPos = -1;
		  break;
		case 'D':
		  printStatus();
		  break; 
		case 'P':
		  if(servoCounting){
			inServo = tallyCount();
			servoCounting = false;
		  }
		  posCounting =  true;
		  numCount = 0;
		  break; 
		case '\r':
		case '\n':
		  if(posCounting){
			inPos = tallyCount();
			posCounting = false;
		  }
		  if((inServo >=0)&&(inServo <=31)&&(((inPos >= 500)&&(inPos <= 2500))||(inPos == -1))){
			changeServo(inServo,inPos);        
			inServo = -1;
			inPos = -1;
		  }
		  numCount = 0;
		  break;
		case 'V':
		  Serial.println("SERVOTOR32_v2.1a");
		  Serial1.println("SERVOTOR32_v2.1a");
		  break;
		case 'C':
		  for(int i=0; i<32; i++){
			changeServo(i, 1500);
		  }
		  Serial.println("All Centered");
		  Serial1.println("All Centered");
		  break;
		case 'K':
		  for(int i=0; i<32; i++){
			changeServo(i,-1);
		  }
		  Serial.println("All Turned Off");
		  Serial1.println("All Turned Off");
		  break;
		case 'L':
		  if(servoCounting){
			inServo = tallyCount();
			servoCounting = false;
		  }
		  changeServo(inServo, -1);
		  break;
		case 'S': // make an arcPing and send distances back over serial
		  arcPing();
		  for(int i = 0; i < ARCPING_STEPS; i++)
		  {
			Serial.println(arcPingDistances[i]);
			Serial1.println(arcPingDistances[i]);
		  }
		  delay_ms(200);
		  break;
		case 'M': // send multiple distance readings back over serial
		  Serial.println(multiPing(5));
		  delay_ms(200);
		  break;  
		default:
		  if((inChar > 47)&&(inChar < 58)){
			if(numCount<4){
			  numString[numCount] = inChar-48;
			  numCount++;
			}
		  }
		  break;
	  } 
	}
}