Hi,
I modified a sketch I found in internet to:
1) Get commands to move the DXL motors sent from my PC over the USB serial connection (handled by a USB interruption)
2) Read the status of the DXL motors and send it to my PC over the USB serial connection (handled periodically on the loop() function

The problem is that both the reading and the command writing only work fine the first time I download the sketch and it begins to execute. Once I disconnect the USB cable and connect it again, the read values are always 255 (byte) or 65555 (word) and the commands are ignored. The commands are correctly received: I can see the reply on the USB serial monitor and it is the same the first time (when it works) and after unplugging-plugging (when it does not work).

I am also checking the result of calling getTxRxStatus after sending a command to the motors. In both scenarios, when it works fine and when the commands are ignored, the result of this function is 65. Does this mean 1+64? Why does it listen to the commands and move the first time the sketch runs, if it returns 65?

I tried adding some delay before starting to read the servos (in the loop() function) because I thought the problem could be that I tried to read before some initialization of the bus was completed. However, the behavior with the delay is the same.

The code:

Code:
/*
 
 Sketch to read commands from the USB and send them to the Dynamixels XL 320
 and to read the status of the Dynamixels and send it also over the USB connection
 
 The commands from the USB connection are handled with an interruption
 
 The reading of the motor status is handled in the periodic loop() function
 
 Note that all values in <> are ASCII decimal integers. 
 <pos> is a value from 0 to 1023. 
 <vel> is a velocity, 0 to 1023 is forward, 1024-2047 is negative. 
 
 Possible commands: 
 
 "Wj<ID>" to make motor <ID> enter joint mode.
 "WJ" to make all the motors (IDs from 1 to 16) enter joint mode. 
 "Wp<ID>,<pos>,<vel>" to make motor <ID> move to position <pos> with velocity <vel>. 
 
 */


#define NUM_MOTORS 16


#define DXL_BUS_SERIAL1 1  //Dynamixel on Serial1(USART1)  <-OpenCM9.04
#define DXL_BAUDRATE_NUMBER 3  // number = 1 is 1000000 bps, 34 is 57600 bps (dxl 1.0)
// Dynamixel 2.0 Baudrate -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps 


#define XL_MOVING 49


Dynamixel Dxl(DXL_BUS_SERIAL1); 


void setup() 
{
  Dxl.begin(DXL_BAUDRATE_NUMBER);  // initialize again as changed baud rate 1Mbps  
  SerialUSB.attachInterrupt(usbInterrupt);
}


//USB max packet data is maximum 64byte, so nCount can not exceeds 64 bytes
void usbInterrupt(byte* buffer, byte nCount){
  SerialUSB.print("nCount =");
  SerialUSB.println(nCount);
  for(unsigned int i=0; i < nCount;i++)  //printf_SerialUSB_Buffer[N]_receive_Data
    SerialUSB.print((char)buffer[i]);
  SerialUSB.println("");
  byte cmd = buffer[0];
  int id = 0;
  unsigned int buf_idx =1;
  for(; buf_idx < nCount;buf_idx++)  //printf_SerialUSB_Buffer[N]_receive_Data
  {
    if(buffer[buf_idx] != ',')
    {
      id *= 10;
      id += (buffer[buf_idx] - '0');
    }
    else
    {
      buf_idx++;
      break;
    }
  }
  int pos = 0;
  for(; buf_idx < nCount;buf_idx++)  //printf_SerialUSB_Buffer[N]_receive_Data
  {
    if(buffer[buf_idx] != ',')
    {
      pos *= 10;
      pos += (buffer[buf_idx] - '0');
    }
    else
    {
      buf_idx++;
      break;
    }
  }
  int vel = 0;
  for(; buf_idx < nCount;buf_idx++)  //printf_SerialUSB_Buffer[N]_receive_Data
  {
    if(buffer[buf_idx]!=',')
    {
      vel *= 10;
      vel += (buffer[buf_idx] - '0');
    }
  }        
  processWrite(cmd, id, pos, vel); 
}


void processWrite(byte cmd, int id, int pos, int vel)
{
  switch (cmd)
  {
  case 'j':
    Dxl.jointMode(id); 
    SerialUSB.print("Setting into joint mode Dxl ");
    SerialUSB.println(id);
    Dxl.ledOn(id);
    break;
  case 'J': //All joints in position mode
    for(unsigned int i=1; i<= NUM_MOTORS ; i++)
    {
      Dxl.jointMode(i); 
      SerialUSB.print("Setting into joint mode Dxl ");
      SerialUSB.println(i);
      Dxl.ledOn(i);
    }
    break;
  case 'p':
    {
      //int pos, vel; 
      //if (!(readInt(&pos) && readInt(&vel))) break; 
      Dxl.setPosition(id, pos, vel); 
      Dxl.ledOn(id,2);
      SerialUSB.print("Commanding Dxl ");
      SerialUSB.print(id);
      SerialUSB.print(" to position ");
      SerialUSB.print(pos);
      SerialUSB.print(" with velocity ");
      SerialUSB.println(vel);
    }
    break; 
  }
  byte error_ss = Dxl.getTxRxStatus();
  if(error_ss != 0)
  {
    if(Dxl.getError(ERRBIT_RESULT_FAIL))
      SerialUSB.println("Error ERRBIT_RESULT_FAIL");
    else if(Dxl.getError(ERRBIT_INST_ERROR))
      SerialUSB.println("Error ERRBIT_INST_ERROR");
    else if(Dxl.getError(ERRBIT_CRC))
      SerialUSB.println("Error ERRBIT_CRC");
    else if(Dxl.getError(ERRBIT_DATA_RANGE))
      SerialUSB.println("Error ERRBIT_DATA_RANGE");
    else if(Dxl.getError(ERRBIT_DATA_LENGTH))
      SerialUSB.println("Error ERRBIT_DATA_LENGTH");
    else if(Dxl.getError(ERRBIT_DATA_LIMIT))
      SerialUSB.println("Error ERRBIT_DATA_LIMIT");
    else if(Dxl.getError(ERRBIT_ACCESS))
      SerialUSB.println("Error ERRBIT_ACCESS");
    else
    {
      SerialUSB.print("Error code (from getTxRxStatus): ");
      SerialUSB.println(error_ss);
    }
  }
}


void readMotors()
{
  for(unsigned int i=1; i<= NUM_MOTORS ; i++)
  {
    SerialUSB.print("Joint ");
    SerialUSB.println(i);


    //SerialUSB.print("Model Number(L): "); 
    //SerialUSB.println(Dxl.readByte(i,0));


    //SerialUSB.print("Version Firmware: "); 
    //SerialUSB.println(Dxl.readByte(i,2));
    //SerialUSB.print("ID: "); 
    //SerialUSB.println(Dxl.readByte(i,3));
    //SerialUSB.print("Baud Rate: "); 
    //SerialUSB.println(Dxl.readByte(i,4)); 
    //SerialUSB.print("CW Angle Limit(L): "); 
    //SerialUSB.println(Dxl.readByte(i,6));


    //SerialUSB.print("CCW Angle Limit(L): "); 
    //SerialUSB.println(Dxl.readByte(i,8));


    //SerialUSB.print("Highest Limit Temperature: "); 
    //SerialUSB.println(Dxl.readByte(i,11));
    //SerialUSB.print("Lowest Limit Voltage: "); 
    //SerialUSB.println(Dxl.readByte(i,12));
    //SerialUSB.print("Highest Limit Voltage: "); 
    //SerialUSB.println(Dxl.readByte(i,13)); 
    //SerialUSB.print("Max Torque(L): "); 
    //SerialUSB.println(Dxl.readByte(i,14));
    SerialUSB.print("Position ");
    SerialUSB.println(Dxl.getPosition(i));
    SerialUSB.print("Velocity ");
    SerialUSB.println(Dxl.getSpeed(i));
    SerialUSB.print("Load ");
    SerialUSB.println(Dxl.getLoad(i));
    //SerialUSB.print("Volt ");
    //SerialUSB.println(Dxl.getVolt(i));
    //SerialUSB.print("Temperature ");
    //SerialUSB.println(Dxl.getTemperature(i));
    SerialUSB.print("Is moving? ");
    byte isMoving = Dxl.readByte(i, XL_MOVING);
    if( isMoving == 0 ){ //if ID i dynamixel is stopped
      SerialUSB.println("Not moving");
    }
    else if(isMoving == 1)
    {
      SerialUSB.println("Moving");
    } 
    else
    {
      SerialUSB.println("Shit"); 
    }
    SerialUSB.println("-----------");
  }
  SerialUSB.println("==============");
}


int delay_steps = 300;
void loop() 
{
  if(delay_steps == 0)
  {
    readMotors();
  }
  else{
    //toggleLED();
    delay(100);
    delay_steps--;
  }
}


boolean readInt(int * outVal)
{
  byte inBuffer[sizeof(short)]; 
  int i; 
  for (i = 0; i < sizeof(short); i ++)
  {
    inBuffer[i] = blockingRead();
  }
  *outVal = *((short*)inBuffer); 
  return true; 
}


byte blockingRead()
{
  while (!SerialUSB.available()) delay(10); 
  return SerialUSB.read(); 
}