Test

byte response[16];


int checkSensor(byte whichSensor, byte howManyBytes) {
   
   int timeOut = 40;
   int bufferPos = 0;
   
   roombaSerial.write(142);
   roombaSerial.write((byte)whichSensor);
   
   while(howManyBytes and timeOut) {
      
      while (roombaSerial.available() == 0 and timeOut) {  
         timeOut -= 1;   
         delayMicroseconds(1);    
      } 
      
      response[bufferPos++] = roombaSerial.read();
      
      howManyBytes -= 1;
      
   }
   
   if (timeOut) {
      return 1;
   }
   
}

void brushMotorCurrentTest_00(){
   
//    int current = word(byte[0], byte[1])
//    short int16 = (short)(((bytes[0] & 0xFF) << 8) | (bytes[1] & 0xFF)); 
   
   cleaningMotors(primary, 30);
   checkSensors(56, 16);
   for (i=0; i=16; i++){
      Serial.println(response[i]);
   }      
   
}