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;
 
 import java.util.List;

Author(s):
Matthew Stevenson <www.robokind.org>
 
 public class DynamixelMultiReader {
     private final static Logger theLogger = Logger.getLogger(DynamixelMultiReader.class.getName());
     private final static int MULTIREAD_BYTE_COUNT = 8;
 
     public synchronized static List<FeedbackUpdateValuesgetFeedback(
             DynamixelController controllerList<DynamixelServo.Idids){
         int[][] vals = 
                 readServos(controllerids
                         ..);
         if(vals == null){
             return null;
         }
         long now = TimeUtils.now();
         List<FeedbackUpdateValuesfeedback = new ArrayList(vals.length);
         boolean error = false;
         for(int i=0; i<vals.lengthi++){
             boolean valid = validateValues(vals[i]);
             error = error || !valid;
             if(valid){
                 feedback.add(new FeedbackUpdateValues(ids.get(i), vals[i], now));
             }
         }
         return feedback;
     }
     
     private static boolean validateValues(int[] vals){
         if(vals == null){
             return false;
         }
         for(int i=0; i<vals.lengthi++){
             if(vals[i] != 0){
                 return true;
             }
         }
         return false;
     }
     
     private static int[][] readServos(
             DynamixelController controllerList<DynamixelServo.Idids
             Register regFirstRegister regLast) {
         byte byteCount = (byte) (regLast.getByte() - regFirst.getByte() + regLast.getLength());
         byte[] cmd = buildMultiReadCommand(idsregFirst.getByte(), byteCount);
         if (!controller.getPort().write(cmd
                 || !controller.getPort().flushWriter()) {
             return null;
         }
         DynamixelPacket[] packets = 
                 readPackets(controllerids.size(), byteCount);
         if(packets == null){
             return null;
         }
         return parsePackets(packetsregFirstregLastbyteCount);
     }
 
     private static byte[] buildMultiReadCommand(
             List<DynamixelServo.Ididsbyte startRegisterbyte byteCount) {
         byte[] data = new byte[ids.size() * ];
         for (int j = 0; j < ids.size(); j++) {
             int i = j * ;
             data[i + 0] = (byte) 0xff;
             data[i + 1] = (byte) 0xff;
             data[i + 2] = (byteids.get(j).getIntValue();
             data[i + 3] = (byte) 4;
             data[i + 4] = ..getByte();
             data[i + 5] = startRegister;
             data[i + 6] = byteCount;
             data[i + 7] = Utils.checksum(datai + 2, 5, true);
         }
         return data;
    }
    public static DynamixelPacket[] readPackets(
            DynamixelController controllerint countbyte byteCount) {
        int packetLen = byteCount + 6;//+2(header)+1(id)+1(packet size)+1(error byte)+1(checksum)
        byte[] data = controller.getPort().read(count * packetLen);
        DynamixelPacket[] packets = new DynamixelPacket[count];
        int offset = nextPacket(data, 0);
        int i = 0;
        while(i < count && offset != -1) {
            DynamixelPacket p = DynamixelPacket.parsePacket(dataoffset);
            packets[i] = p;
            i++;
            if(p != null && !p.hasError()){
                offset += p.getData().length + 6;
                if(offset >= data.length){
                    return packets;
                }
            }else{
                offset = nextPacket(dataoffset+5);
            }
        }
        return packets;
    }
    
    private static int nextPacket(byte[] dataint offset){
        if(offset >= data.length-5){
            return -1;
        }
        do{
            if(Utils.unsign(data[offset]) == 0xff 
                    && Utils.unsign(data[offset+1]) == 0xff){
                return offset;
            }
        }while(++offset < data.length-5);
        return -1;
    }
    public static int[][] parsePackets(DynamixelPacket[] packetsRegister regFirstRegister regLastbyte byteCount) {
        if(packets == null){
            return null;
        }
        int first = regFirst.ordinal();
        int last = regLast.ordinal();
        int rows = packets.length;
        int cols = last - first + 1;
        int[][] vals = new int[rows][cols];
        for (int i = 0; i < packets.lengthi++) {
            DynamixelPacket p = packets[i];
            if (p == null || p.getData().length < byteCount) {
                continue;
            }
            parsePacketData(firstlastp.getData(), vals[i]);
        }
        return vals;
    }
    private static void parsePacketData(int firstRegint lastRegbyte[] dataint[] dest) {
        Register[] regs = Register.values();
        for (int i = firstRegi <= lastRegi++) {
            Register reg = regs[i];
            int offset = reg.getByte() - regs[firstReg].getByte();
            dest[i - firstReg] = Utils.unsign(data[offset]);
            if (reg.getLength() > 1) {
                dest[i - firstReg] += (Utils.unsign(data[offset + 1]) << 8);
            }
        }
    }
New to GrepCode? Check out our FAQ X