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.openservo.feedback;
 

Author(s):
Matthew Stevenson <www.robokind.org>
 
 public class OpenServoReader {
     private final static Logger theLogger = Logger.getLogger(OpenServoReader.class.getName());
 
     public synchronized static OpenServoFeedbackValues getFeedback(
             RXTXSerialPort portOpenServo.Id id){
         if(port == null || . != port.getConnectionStatus()){
             return null;
         }
         int[] vals = 
                 readServo(portid
                         ..);
         if(vals == null){
             return null;
         }
         long now = TimeUtils.now();
         if(validateValues(vals)){
             return new OpenServoFeedbackValues(idvalsnow);
         }
         return null;
     }
     
     private static boolean validateValues(int[] vals){
 //        if(vals == null){
 //            return false;
 //        }
 //        for(int i=0; i<vals.length; i++){
 //            if(vals[i] != 0){
 //                return true;
 //            }
 //        }
 //        return false;
         return true;
     }
     
     private static int[] readServo(RXTXSerialPort port,
             OpenServo.Id idRegister regFirstRegister regLast) {
         byte byteCount = (byte) (regLast.getRegister() - regFirst.getRegister() + 1);
         byte[] cmd = buildReadCommand(idregFirstbyteCount);
         if (!port.write(cmd
                 || !port.flushWriter()) {
             return null;
         }
         OpenServoPacket packet = readPacket(portbyteCount);
         if(packet == null){
             return null;
         }
         return parsePacket(packetbyteCount);
     }
 
     private static byte[] buildReadCommand(OpenServo.Id idRegister startRegisterint regCount) {
         return OpenServoCommandSet.readRegisters(
                 id.getRS485Addr(), id.getI2CAddr(), startRegisterregCount);
     }
 
     public static OpenServoPacket readPacket(
             RXTXSerialPort portbyte byteCount) {
         int packetLen = byteCount + 5;
         byte[] data = port.read(packetLen);
         return OpenServoPacket.parsePacket(data, 0);
     }
 
     public static int[] parsePacket(OpenServoPacket packetbyte byteCount) {
         if (packet == null || packet.getData().length < byteCount) {
                 return null;
         }
         int[] vals = new int[byteCount/2];
         byte[] data = packet.getData();
         int j = 0;
         for(int i=0; j<vals.length && i<byteCount-1; i+=2, j++){
            int hi = (data[i] & 0xFF) << 8;
            int lo = data[i+1] & 0xFF;
            vals[j] = hi + lo;
        }
        return vals;
    }
New to GrepCode? Check out our FAQ X