Start line:  
End line:  

Snippet Preview

Snippet HTML Code

Stack Overflow Questions
  /*
   * Copyright 2012 Hanson Robokind LLC.
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   *      http://www.apache.org/licenses/LICENSE-2.0
   *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
 package org.robokind.impl.motion.dynamixel;
 

Author(s):
Matthew Stevenson <www.robokind.org>
 
 public class DynamixelCommandSender {
     public static boolean ping(
             DynamixelControlLoop controlLoop
             DynamixelServo.Id idlong timeout){
         DynamixelPacket[] packets = sendCommand(
                 controlLoop
                 id
                 .
                 new byte[0], 
                 1, (byte)0, timeout);
         if(packets == null || packets.length == 0 || packets[0] == null){
             return false;
         }
         return !packets[0].hasError();
     }
     
     public static int[] readRegisters(
             DynamixelControlLoop controlLoop
             DynamixelServo.Id id
             Register first,
             Register last,
             long timeout){
 		byte byteCount = (byte)(last.getByte() - first.getByte() + last.getLength());
         DynamixelPacket[] packets = 
                 sendCommand(
                 controlLoop
                 id
                 .
                 new byte[]{first.getByte(), byteCount}, 
                 1, byteCounttimeout);
         if(packets == null){
             return null;
         }
         int[][] data = 
                 DynamixelMultiReader.parsePackets(
                         packetsfirstlastbyteCount);
         if(data == null || data.length == 0){
             return null;
         }
         return data[0];
     }
     
     public static boolean writeRegister(
             DynamixelControlLoop controlLoop
             DynamixelServo.Id idRegister reg,
             Integer valuelong timeout){
         byte[] params = new byte[reg.getLength() + 1];
         params[0] = reg.getByte();
         int len = reg.getLength();
         if(len == 1){
             params[1] = (bytevalue.byteValue();
         }else{
             params[1] = (byte) (value & 0xff);
             params[2] = (byte) (value >> 8);
         }
         DynamixelPacket[] packets = sendCommand(
                 controlLoop
                 id
                 .
                 params
                 1, (byte)0, timeout);
         if(packets == null || packets.length == 0 || packets[0] == null){
             return false;
         }
         return !packets[0].hasError();
     }
     
     private static DynamixelPacket[] sendCommand(DynamixelControlLoop controlLoop
             DynamixelServo.Id idInstruction insbyte[] params
             int readCountbyte readByteslong timeout){
         return sendAndWait(controlLoop
                buildCommand(idinsparamsreadCountreadBytes), 
                timeout);
    }
    
    private static DynamixelPacket[] sendAndWait(DynamixelControlLoop controlLoopDynamixelCommand cmdlong timeout){
        if(controlLoop == null || cmd == null){
            return null;
        }
        controlLoop.queueCommand(cmd);
        if(cmd.getPacketReturnCount() > 0){
            return cmd.getCallback().waitForPackets(timeout);
        }else{
            return new DynamixelPacket[0];
        }
    }
    
    public static DynamixelCommand buildCommand(
            DynamixelServo.Id idInstruction insbyte[] params
            int readCountbyte readBytes){
            PacketCallback callback = new PacketCallback();
        byte[] cmd = buildInstruction(idinsparams);
        return new DynamixelCommand(cmdreadCountreadBytescallback);
    }
    
    private static byte[] buildInstruction(DynamixelServo.Id idInstruction insbyte... params){
        int paramLen = params == null ? 0 : params.length;
        byte[] data = new byte[6 + paramLen];
        data[0] = (byte)0xff;
        data[1] = (byte)0xff;
        data[2] = (byte)id.getIntValue();
        data[3] = (byte)(paramLen + 2);
        data[4] = ins.getByte();
        System.arraycopy(params, 0, data, 5, paramLen);
		data[data.length - 1] = Utils.checksum(data, 2, paramLen+3, true);
        return data;
	}
New to GrepCode? Check out our FAQ X