byte opcode,len1,nodeID,subindex,ack,failack;
word crc,warray[256],index;


void setup(){

 Serial.begin(9600);

 
  opcode=0x11;
  len1=0x03;
  nodeID=0x1;
  index=0x206B;
  subindex=0x00;
  ack=0x4F;
  failack=0x46;
  warray[0]=opcode<<8|len1;
  warray[1]=index;
  warray[2]=nodeID<<8|subindex;
  warray[3]=500;  / rounds per min
  warray[4]=0;
  warray[5]=0x0;
 
}  
  
void loop(){


        crc=CalcFieldCRC(warray,6);
        warray[5]=crc;


        if(sendOpCode(opcode)){
          
         sendStructure((char*)&len1,1);
         sendStructure((char*)&warray[1],(len1+1)*2);
         sendStructure((char*)&warray[5],2);
         }
}












void sendStructure( char *structurePointer, int structureLength)
{
  int i;

  for (i = 0 ; i < structureLength ; i++)
    Serial.write(structurePointer[i]);
  }









word CalcFieldCRC(word *pDataArray, word numberOfWords)
{
 word shifter,c;
 word carry;
 word CRC=0;

  while(numberOfWords--){
   shifter=0x8000;
   c=*pDataArray++;
  do
 {
  carry=CRC & 0x8000;
  CRC <<=1;
  if(c&shifter) CRC++;
  if(carry)CRC^=0x1021;
  shifter>>=1;
 } while(shifter);
  } 
  return CRC;
}


boolean sendOpCode(byte opcode){
  int incomingbyte = 0;
  int b=0;
  while(1){
  if(b==0){
  sendStructure((char*)&opcode,1); 
  b=1;
  }
  if(b==1){
    if(Serial.available()>0){
      incomingbyte=Serial.read();
      if(incomingbyte==ack){
      return true;
      }
      else{
      b=0;
      }
}}}}