Motor Current Test

int checkSensor(byte whichSensor, byte* response, byte howManyBytes) {
   // This function will return the value of a sensor
   // The first perameter inidcates the sensor code to querry
   // The function takes a pointer as a parameter so it can copy the data into the buffer pointed to by that pointer
   // The third paremeter is for keeping track of the number of bytes that the querry will return
   
   int timeOut = 40;
   int bufferPos = 0;
   
   if(!response) return 0;
   
   roombaSerial.write(142); // instruction to prompt roomba for sensor data
   roombaSerial.write((byte)whichSensor); // send sensor code to roomba for data querry
   
   for(unsigned bufferPos=0; bufferPos < howManyBytes && timeOut; bufferPos++) {  // get the data or timeout
      while (roombaSerial.available() == 0 and timeOut) {  
         timeOut -= 1;   
         delayMicroseconds(1);    
      } 
      response[bufferPos] = roombaSerial.read();
      bufferPos += 1;
      howManyBytes -= 1;
   }
   
   if (timeOut) { // timeout if no response
      return 1;
   }
   
}

void brushMotorCurrentTest_00(){
   byte response[16] = {};
   int i = 0;
   cleaningMotors(primary, 30);
   checkSensor(56, response, 16);
   //int16_t current = (((uint16_t)response[0]) << 8) | response[1];
   for (i=0; i=16; i++){
      Serial.println(response[i]);
   }      
}