/*
 *
 * $Id: syncwritepacket.cpp 2 2008-07-30 09:28:03Z mschneider $
 * $URL: svn://localhost/Bioloid/trunk/syncwritepacket.cpp $
 
 * \version   $Rev: 2 $
 * \author    Markus Schneider
 * \date      17.07.08
 *
 */

#include "syncwritepacket.h"

SyncWritePacket::SyncWritePacket(byte _address, byte _nbytes)
  : address(_address), nbytes(_nbytes)
{ 
  id = BROADCAST_ID;
  
  //Length (L+1) * N+4 (L: Data length for each Dynamixel actuator, N: The number of Dynamixel)
  len = 4;
  cmd = INST_SYNC_WRITE;
  paramPtr = 0;
  param[paramPtr++] = address;
  param[paramPtr++] = nbytes;
  crc = calcChecksum();
}


int SyncWritePacket::add(const byte id, const word wparam[])
{
  byte bparam[nbytes];
  byte bptr = 0;
  byte wptr = 0;
  while(bptr < nbytes){
    bparam[bptr++] = lowbyte(wparam[wptr]);
    bparam[bptr++] = highbyte(wparam[wptr]);
    wptr = wptr+1;
  }
  return add(id, bparam);
}


int SyncWritePacket::add(const byte id, const byte _param[])
{
  len = len + (nbytes+1);
  
  param[paramPtr++] = id;
  for(int i=0; i<nbytes; i++)
    param[paramPtr++] = _param[i];
    
  crc = calcChecksum();
  return 0;
}
