KobukiPacketReader.java

Go to the documentation of this file.

/*
        Copyright (c) 2013 - York College of Pennsylvania, Patrick J. Martin
        The MIT License
        See license.txt for details.
*/

package com.github.c77.base_driver.kobuki;
import org.apache.commons.codec.binary.Hex;
import org.apache.commons.logging.Log;
import org.apache.commons.logging.LogFactory;

import java.nio.ByteBuffer;
import java.util.Arrays;

public class KobukiPacketReader {

    Log log = LogFactory.getLog(KobukiPacketReader.class);

    //kobuki sensors read from here to update. Accessible fields
        private byte[] sensorPacket = new byte[15];
        private byte[] inertialSensorPacket = new byte[4];
        private byte[] dockingIRPacket = new byte[3];
        private byte[] wheelCurrentPacket = new byte[4];
        private enum parserState {
                INIT, PARTIAL, COMPLETE;
        }
        //Fields for class
        private parserState curState;
        private volatile byte[] stored;
        private int bytesStillNeeded;
        private int totalSize;
        private int bytesStoredIndex;

        public KobukiPacketReader() {
                curState = parserState.INIT;
                stored = new byte[500]; // Should be big enough for the biggest packets
                bytesStillNeeded = 0;
                totalSize = 0;
                bytesStoredIndex = 0;
        }

        public final void newPacket(ByteBuffer buff) { // look for main headers in buffer
                if(curState == parserState.INIT || curState == parserState.COMPLETE) {
                        for(int i=0; i < buff.array().length-2; i++){ // currently ignoring -86|85 split case... should be rare
                                if(buff.array()[i] == -86 && buff.array()[i+1] == 85) { // Header 0 and Header 1 means packet
                                        curState = parserState.PARTIAL;
                                        if((buff.array().length-i) >= (buff.array()[i+2]+4)) { // Whole packet is in the buffer. +3 accounts for headers and size. Don't think this is right. Change(+4 for checksum)
                                                byte[] packet = new byte[byteToInt(buff.array()[i+2])+2]; // make byte array of proper size. +1 just in case!!!! Needs to be + 2 to account for checksum and length
                                                //If x + 2 is the length of the payload, the length of payload + length + check sum = length of payload + 2
                                                System.arraycopy(buff.array(), i + 2, packet, 0, byteToInt(buff.array()[i+2]) + 2); //copy in data needed
                                                if(verifyChecksum(byteToInt(buff.array()[i+2]) + 2, packet)) {
                                                        System.arraycopy(packet, 1, packet, 0, packet.length - 1);
                                                        goodPacket(byteToInt(buff.array()[i+2]) ,packet); // send goodPacket the array and length of array.  At this point, the packet contains ONLY the payload...
                                                } else {
                                                        //packet not valid
                                                }
                                                curState = parserState.COMPLETE;
                                        } else {
                                                System.arraycopy(buff.array(), i+2, stored, 0, (buff.array().length-(i+2))); //Copy the partial packet into the stored array (only for partial packets)
                                                bytesStillNeeded = (buff.array()[i+2] + 2) - (buff.array().length-(i+2));
                                                totalSize = buff.array()[i+2] + 2;  //This is the payload length + 2.  This accounts for both the length of the payload byte as well as the checksum byte
                                                bytesStoredIndex = buff.array().length - (i+2); //Basically, in the case of a partial packet, this variable stores the length of the partial packet
                                        }
                                }
                        }
                } else {
                        System.arraycopy(buff.array(), 0, stored, bytesStoredIndex, bytesStillNeeded);  //Copy in the "rest" of the packet
                        if(verifyChecksum(totalSize, stored)) {
                                System.arraycopy(stored, 1, stored, 0, stored.length - 1);  //Get the array without the checksum or length
                                goodPacket(totalSize - 2, stored);
                        } else {
                                //packet not valid
                        }
                        curState = parserState.COMPLETE;
                }
        }

        private final boolean verifyChecksum(int length, byte[] packet) { //Method for verifying checksum

                byte checksum = 0;

                for(int i = 0; i < length - 1; i++) {  //length - 1 so that checksum is not included in total!

                        checksum ^= packet[i];   //XOR's all values in packet EXCEPT: HEADERS AND CHECKSUM
                }

                if(packet[length - 1] == checksum) { //Ensures that calculated checksum is equal to the checksum found in the packet
                        return true;
                } else {
                        return false;
                }
        }

        private final void goodPacket(int length ,byte[] packet) { // Without first 2 headers, length, or checksum!
                //header
                //size
                //payload

                int curPlace = 0;
                byte[] toSend = new byte[100];
                System.arraycopy(packet, 0, toSend, 0, (packet[1]+2));
                curPlace = packet[1] + 2;
                sortParts(toSend);

                for(int i = 0; i < 6; i++) { // seven total packets six left
                         Arrays.fill(toSend, (byte)0);
                         //Changed from curPlace + 1 to curPlace. Changed last field from curPlace + 2 to packet[curPlace + 1]
                         System.arraycopy(packet, curPlace, toSend, 0, packet[curPlace+1] + 2); //Curplace is current index that needs to be accessed. packet[curPlace + 1] + 2 accounts for the size of the payload AND the index + payload size bytes
                         curPlace = curPlace + packet[curPlace+1] + 2; //Essentially, the index of the element we just accessed. + the length of the payload + 2 for the index and payload size
                         sortParts(toSend);  //Sorts packet array into individual packets (see method)
                }
        }

        private final void sortParts(byte[] packet){
                switch(packet[0]){ // Sort by packet[0] (The index of the packet. See kobuki driver page for more info).
                        case 1:
                                sensorPacket(packet); // Sorts sensor packet
                                break;
                        case 3:
                                dockingIRPacket(packet); //sorts dockingIR packet
                                break;
                        case 4:
                                inertialSensorPacket(packet); //Sorts inertial sensor packet
                                break;
                        case 6:
                                wheelCurrentPacket(packet); //Sorts wheel current packet
                                break;
                        default:
                                break;
                }
        }

        private final int byteToInt(byte b) {
                return b & 0xFF;
        }

        public final void sensorPacket(byte[] packet) { // Store sensor values from packet
                System.arraycopy(packet, 2, sensorPacket, 0, 15); //Copy packet from incoming byte array
        }

        private final void inertialSensorPacket(byte[] packet) {
                System.arraycopy(packet, 2, inertialSensorPacket, 0, 4); //Copy packet from incoming byte array
        }

        private final void dockingIRPacket(byte[] packet) {  //Sorts the IR docking packet
                System.arraycopy(packet, 2, dockingIRPacket, 0, 3); //Copy packet from incoming byte array
        }

        private final void wheelCurrentPacket(byte[] packet) {  //Sorts wheel current data packet
                System.arraycopy(packet, 2, wheelCurrentPacket, 0, 2); //Copy packet from incoming byte array
        }

        public final byte[] getSensorPacket() {
                return sensorPacket;
        }

        public final byte[] getInertialSensorPacket() {
                return inertialSensorPacket;
        }

        public final byte[] getDockingIRPacket() {
                return dockingIRPacket;
        }

        public final byte[] getWheelCurrentPacket() {
                return wheelCurrentPacket;
        }
}