 void sendBytes(uint8_t* bytes,long len) 
 {
   fillChecksum(bytes,len);
-
+  int i = 0;
+  /*Serial.print("< ");
+  for (i = 0; i < len; i++) {
+    Serial.print(bytes[i],HEX);
+    Serial.print(" ");
+  }
+  Serial.println();*/
   if (Serial.write(bytes,len) != len)
     error(10);
 }

/**
 * Send a command packet recieves responce
 * packet - command packet to send
 */
CommandPacket* sendCommand(CommandPacket* packet)
{
  sendBytes((uint8_t*)packet,packet->len);
  return readCommand();
}
/**
 * Sends bytes.  All is little endian so no conversion necessary.

{
  //Get empty command so memory is zeroed, and len is set
  CommandPacket* packet = getCommand(NIL);
  recvBytes((uint8_t*)packet,packet->len);
  return packet;
}
/**
 * Reads bytes.  All is little endian so no conversion necessary.
 * bytes - pointer to bytes to read into
 * len - length of bytes
 */
void recvBytes(uint8_t* bytes,long len)
{
  //Read bytes waiting if data is not found
  long i = 0;
  long j = 0;
  for (i = 0; i < len;)
  {
    int b = Serial.read();
    if (b == -1)
    {
      /*for (j = 0; j < i+1; j++) {
        digitalWrite(ERROR_PIN, HIGH);
        delay(100);
        digitalWrite(ERROR_PIN, LOW);
        delay(100);  
      }
      delay(1000);*/
      continue;
    }
    bytes[i++] = (uint8_t) b;
  }
  /*Serial.print("< ");
  for (i = 0; i < len; i++) {
    Serial.print(bytes[i],HEX);
    Serial.print(" ");
  }
  Serial.println();*/
}
