TwoWire.java

 
package robotCore;
 
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.List;
import java.util.concurrent.ArrayBlockingQueue;
import java.util.concurrent.TimeUnit;
import java.util.function.Consumer;
 
import org.wpilib.hardware.hal.HALUtil;
 
// import com.pi4j.io.i2c.I2CBus;
// import com.pi4j.io.i2c.I2CDevice;
// import com.pi4j.io.i2c.I2CFactory;
// import com.pi4j.io.i2c.I2CFactory.UnsupportedBusNumberException;
 
// import frc.robot.Robot;
import robotCore.Device.Callback;
 
public class TwoWire {
    public static final int k_maxCommandLength = 44; // Must match the value in the microcontroller
    /*
     * No response commands should be in the range 1-127
     */
    public static final int k_cmdKeepAlive = 1;
    public static final int k_configureMotor = 2;
    public static final int k_setMotor = 3;
    public static final int k_configureEncoder = 4;
    public static final int k_setMotorMode = 5;
    public static final int k_setFeedbackDevice = 6;
    public static final int k_setPIDFTerm = 7;
    public static final int k_setPIDPTerm = 8;
    public static final int k_setPIDITerm = 9;
    public static final int k_setPIDDTerm = 10;
    public static final int k_setPIDIZone = 11;
    public static final int k_enable = 12;
    public static final int k_disable = 13;
    public static final int k_setMotorInverted = 14;
    public static final int k_setEncoderInverted = 15;
    // public static final int k_initNavigator = 16;
    public static final int k_initGyro = 16;
    public static final int k_resetNavigator = 17;
    public static final int k_invertNavigator = 18;
    public static final int k_digitalWrite = 19;
    public static final int k_createCounter = 20;
    public static final int k_setZeroMotorPower = 21;
    public static final int k_setPIDMinPower = 22;
    public static final int k_setPIDWrap = 23;
    public static final int k_setPIDDeadZone = 24;
    public static final int k_multipleCommands = 25;
    public static final int k_setMultipleMotors = 26;
    public static final int k_setMotor32 = 27;
 
    /*
     * Response commands should be in the range 129-255
     */
    public static final int k_getBoardVersion = 129;
    public static final int k_getValidOutputPins = 130;
    public static final int k_getValidPWMPins = 131;
    public static final int k_getMaxMotors = 132;
    public static final int k_getValidAnalogPins = 133;
    public static final int k_getEncoderPos = 134;
    public static final int k_getEncoderSpeed = 135;
    public static final int k_digitalRead = 136;
    public static final int k_getEncoderRange = 137;
    // public static final int k_getNavigatorState = 138;
    public static final int k_getGyroState = 138;
    // public static final int k_getNavigatorYaw = 139;
    public static final int k_getGyroYaw = 139;
    public static final int k_getDigitalCount = 140;
    public static final int k_getMultipleEncoderPos = 141;
    public static final int k_getValidInterruptPins = 142;
    public static final int k_getValidInputPins = 143;
 
    class Tracer {
        class Item {
            String m_name;
            long m_time;
 
            Item(String name, long time) {
                m_name = name;
                m_time = time;
            }
        }
 
        long m_startTime = 0;
        ArrayList<Item> m_list = new ArrayList<Item>();
 
        void clearEpochs() {
            m_list.clear();
            m_startTime = System.nanoTime();
        }
 
        void addEpoch(String name) {
            m_list.add(new Item(name, System.nanoTime()));
        }
 
        void printEpochs(Consumer<String> output) {
            StringBuilder sb = new StringBuilder();
            long prevTime = m_startTime;
 
            sb.append("\n");
            for (Item item : m_list) {
                sb.append(String.format("%s,%f ms,%f ms\n", item.m_name, (item.m_time - m_startTime) / 1000000.0, (item.m_time - prevTime) / 1000000.0));
                prevTime = item.m_time;
            }
            output.accept(sb.toString());
        }
 
 
    }
 
    // Tracer m_tracer = new Tracer();
 
    /*
     * This exception is thrown when the max retries is exceeded when
     * sending commands to the Arduino. This probably means that
     * communication with the Arduino is permanently impaired.
     */
    public static class HardwareException extends RuntimeException {
        HardwareException(String msg) {
            super(msg);
        }
    }
 
    private class Command {
        private int m_addr;
        private int m_responseSize;
        private byte[] m_command;
        private byte[] m_response;
        private Callback m_callback = null;
        private Object m_object = null; // Optional object
 
        /*
         * Simple command, no data
         * Format: [size=4] [packetNo] [command] [cksum]
         */
        Command(int addr, int command, int responseSize) {
            m_addr = addr;
            m_responseSize = responseSize;
            m_command = new byte[4];
            m_command[0] = 4;
            m_command[2] = (byte) command;
        }
 
        /*
         * command with N one byte arguments
         * Format: [size=4+N] [packetNo] [command] [byte1] ... [byteN-1] [cksum]
         */
        Command(int addr, int command, int responseSize, byte[] args, Callback callback, Object object) {
            m_addr = addr;
            m_responseSize = responseSize;
            m_callback = callback;
            m_object = object;
            m_command = new byte[4 + args.length];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            for (int i = 0; i < args.length; i++) {
                m_command[3 + i] = args[i];
            }
        }
 
        /*
         * command with N one byte arguments (with specified length)
         * Format: [size=4+N] [packetNo] [command] [byte1] ... [byteN-1] [cksum]
         */
        Command(int addr, int command, int responseSize, byte[] args, int length, Callback callback, Object object) {
            // Logger.log("TwoWire", 1, String.format("length=%d, args.length=%d", length,
            // args.length));
            m_addr = addr;
            m_responseSize = responseSize;
            m_callback = callback;
            m_object = object;
            m_command = new byte[4 + length];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            for (int i = 0; i < length; i++) {
                m_command[3 + i] = args[i];
            }
        }
 
        /*
         * command with byte argument
         * Format: [size=5] [packetNo] [command] [byte] [cksum]
         */
        Command(int addr, int command, int responseSize, byte byte1, Callback callback, Object object) {
            m_addr = addr;
            m_responseSize = responseSize;
            m_callback = callback;
            m_object = object;
            m_command = new byte[4 + 1];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            m_command[3] = byte1;
        }
 
        /*
         * Command with a byte and 16 bit argument
         * Format [size=7] [packetNo] [command] [byte] [16bit data] [cksum]
         */
        Command(int addr, int command, int responseSize, byte byte1, short value) {
            m_addr = addr;
            m_responseSize = responseSize;
            m_command = new byte[3 + 1 + 2 + 1];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            m_command[3] = byte1;
 
            ByteBuffer buffer = ByteBuffer.wrap(m_command, 4, m_command.length - 4);
            buffer.order(ByteOrder.LITTLE_ENDIAN);
            buffer.putShort(value);
        }
 
        /*
         * Command with a byte and 32 bit argument
         * Format [size=9] [packetNo] [command] [byte] [32bit data] [cksum]
         */
        Command(int addr, int command, int responseSize, byte byte1, int value) {
            m_addr = addr;
            m_responseSize = responseSize;
            m_command = new byte[3 + 1 + 4 + 1];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            m_command[3] = byte1;
 
            ByteBuffer buffer = ByteBuffer.wrap(m_command, 4, m_command.length - 4);
            buffer.order(ByteOrder.LITTLE_ENDIAN);
            buffer.putInt(value);
        }
 
        /*
         * Command with N bytes of data
         * Format [size=3+N+1] [packetNo] [command] [byte1] ... [byteN-1] [cksum];
         * 
         */
        Command(int addr, int command, byte[] bytes) {
            m_addr = addr;
            m_responseSize = 0;
            m_command = new byte[3 + bytes.length + 1];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            for (int i = 0 ; i < bytes.length ; i++) {
                m_command[3+i] = bytes[i];
            }
        }
 
        /*
         * Command with 4 bytes of data
         * Format [size=8] [packetNo] [command] [byte1] [byte2] [byte3] [byte4] [cksum];
         */
        Command(int addr, int command, int responseSize, byte byte1, byte byte2, byte byte3, byte byte4) {
            // System.out.println(String.format("command3: byte2=%d", byte2));
            m_addr = addr;
            m_responseSize = responseSize;
            m_command = new byte[3 + 4 + 1];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            m_command[3] = byte1;
            m_command[4] = byte2;
            m_command[5] = byte3;
            m_command[6] = byte4;
        }
 
        /*
         * Command with a byte and float arguments
         * Format[size=9] [packetNo] [command] [byte] [float (32 bits)] [cksum]
         */
        Command(int addr, int command, int responseSize, byte byte1, float value) {
            m_addr = addr;
            m_responseSize = responseSize;
            m_command = new byte[3 + 1 + 4 + 1];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            m_command[3] = byte1;
 
            // Logger.log("TwoWire", 1, String.format("float=%f", value));
 
            ByteBuffer buffer = ByteBuffer.wrap(m_command, 4, m_command.length - 4);
            buffer.order(ByteOrder.LITTLE_ENDIAN);
            buffer.putFloat(value);
 
            // System.out.printf("v=%f\r\n", value);
            // for (int i = 0 ; i < m_command.length ; i++)
            // {
            //     System.out.printf("%02x ", m_command[i]);
            // }
            // System.out.println();
        }
 
        /*
         * Command with two bytes of arguments
         * Format [size=6] [packetNo] [command] [byte1] [byte2] [cksum]
         */
        Command(int addr, int command, int responseSize, byte byte1, byte byte2) {
            m_addr = addr;
            m_responseSize = responseSize;
            m_command = new byte[3 + 2 + 1];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
            m_command[3] = byte1;
            m_command[4] = byte2;
        }
 
        Command(int addr, int command, int responseSize, int yaw, int x, int y) {
            m_addr = addr;
            m_responseSize = responseSize;
            m_command = new byte[4 + 12];
            m_command[0] = (byte) m_command.length;
            m_command[2] = (byte) command;
 
            ByteBuffer buffer = ByteBuffer.wrap(m_command, 3, m_command.length - 3);
            buffer.order(ByteOrder.LITTLE_ENDIAN);
            buffer.putInt(yaw);
            buffer.putInt(x);
            buffer.putInt(y);
        }
    }
 
 
    private class Device {
        private int m_addr;
        // private I2CDevice m_device;
        private int m_packetNo = 0;
        private long m_usedPins = 0;
 
        private static final int k_writeDelay = 5;
        // private static final int k_readDelay = 1;
        private static final int k_errDelay = 10;
        private static final int k_maxReadRetry = 20;
        private static final int k_maxTries = 10;
        // private static final int k_maxCommandLength = 30;
        private static final int k_maxResponseLength = 36;
        private static final int k_ack = 0x55;
        private static final int k_ack2 = 0x56;
        // private static final int k_nak = 0xcc;
        private static final int k_nak2 = 0xcd;
        I2CInterface m_i2c;
 
        public Device(int addr) {
            m_addr = addr;
            m_i2c = new I2CInterface(addr);
 
            // m_addr = addr;
 
            // try {
            //     // Logger.log("TwoWire.Device", 1, String.format("getDevice(%d)", addr));
            //     m_device = m_i2cBus.getDevice(addr);
            // } catch (IOException e) {
            //     e.printStackTrace();
            //     m_device = null;
            // }
        }
 
        private byte computeCksum(byte[] data, int length) {
            byte cksum = 0;
            for (int i = 0; i < length; i++) {
                cksum += data[i];
            }
 
            // if (data[2] == 25)
            // {
            //     Logger.log("TwoWire", 1, String.format("cksum=%x", cksum & 0xff));
            // }
 
            return (cksum);
        }
 
        private void addCksum(byte[] data) {
 
            data[data.length - 1] = computeCksum(data, data.length - 1);
        }
 
        private void sleep(long ms) {
            try {
                Thread.sleep(ms);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }
 
        private byte[] readResponse(int responseSize) throws IOException {
            byte[] response = new byte[k_maxResponseLength];
 
            for (int retry = 0; retry < k_maxReadRetry; retry++) {
                response[0] = (byte) 0xff; // in case the read fails
 
                int ret = m_i2c.read(response, 0, responseSize);
 
                // The Raspberry Pi doesn't actually handle clock streching properly.
                // The CH32V003 is slow enough that it sometimes needs to stretch the clock between
                // the address field of a read and the first data byte. As a result
                // the Pi incorrectly reads the first byte with the high bit set.
                // This byte is the Ack/Nak field and since we don't use the high bit
                // we can mask it out here to get it to work for all cases.
                response[0] &= 0x7f;
 
                int ack = ((int) response[0]) & 0xff;
 
                // for (int i = 0 ; i < responseSize ; i++)
                // {
                //     System.out.printf("%02x ", response[i] & 0xff);
                // }
                // System.out.println("");
 
                if (ack == k_ack) {
                    int size = response[1];
 
                    if ((size >= 3) && (response[2] == (byte) m_packetNo)) {
                        // A cksum is only if there is data present
                        if ((size == 3) || computeCksum(response, size - 1) == response[size - 1]) {
                            m_packetNo++;
 
                            return (Arrays.copyOfRange(response, 0, size)); // Packet delivered, return response
                        } else {
                            System.out.println(String.format("%d: Invalid response cksum", m_addr));
                        }
                    } else {
                        System.out.println(String.format("Invalid packet: size=%d, packetNo: %d:%d", response[1],
                                (byte) m_packetNo, ack));
                    }
 
                    System.out.println(String.format("c=%d, e=%d", m_packetCount, m_responseErrors));
                } else if (ack == k_ack2) {
                    sleep(1);
                } else if (ack == k_nak2) {
                    System.out.println(String.format("%d: Queue Overflow", m_packetNo));
                    sleep(k_errDelay);
                    return (null);
                } else {
                    System.out.println(String.format("ack = %x", ack));
                    sleep(k_errDelay);
                    return (null);
                }
            }
 
            // sleep(1000);
 
            System.out.println(String.format("TwoWire: lastCommand=%d", m_lastCommand));
            throw new HardwareException(String.format("Max read retries exceeded @ %d", m_addr));
 
            // return(false);
        }
 
        int m_lastCommand = 0;
        int m_packetCount = 0;
        int m_packetErrors = 0;
        int m_responseErrors = 0;
 
        /*
         * Packet format [size] [packet #] [command] [data...] [cksum]
         */
        private byte[] sendPacket(byte[] data, int responseSize) {
            int count;
 
            m_packetCount++;
 
            m_lastCommand = ((int) data[2]) & 0xff;
 
            data[1] = (byte) m_packetNo;
 
            addCksum(data);
 
            // if (m_lastCommand == 25)
            // {
            //     Logger.log("TwoWire", 1, String.format("send: %d, len=%d, ck=%x", m_lastCommand,data.length, data[data.length-1] & 0xff));
            // }
 
            for (count = 0; count < k_maxTries; count++) {
                try {
                    byte[] response;
 
                    // Logger.log("TwoWire", 1, String.format("write cmd: %d", data[2]));
 
                    m_i2c.write(data);
 
                    // If the command needs a response, wait a short time before the read
                    // if (data[2] < 0) 
                    {
                        long waitTime = System.nanoTime() + ((data[2] < 2) ? 300000 : 50000);
                        while (System.nanoTime() < waitTime);
                    }
 
                    if ((response = readResponse(responseSize + 4)) != null) {
                        return (response);
                    }
                    Logger.log("TwoWire", 1, String.format("Response failed for %d @ %d", m_lastCommand, m_addr));
 
                } catch (IOException e) {
 
                    Logger.log("TwoWire", 1, String.format("write IOException for %d @ %d - ", m_lastCommand, m_addr) + e);
                }
 
                m_packetErrors++;
 
                if ((m_packetErrors % 10) == 0) {
                    Logger.log("TwoWire", 1, String.format("Write Retry: cmd=%d c=%d, e=%d", m_lastCommand,
                            m_packetCount, m_packetErrors));
                }
 
                sleep(k_writeDelay);
            }
 
            throw new HardwareException(String.format("Max write retries exceeded @ %d", m_addr)); // Something is seriously wrong
        }
 
        private long getUsedPins() {
            return (m_usedPins);
        }
 
        private void setUsedPin(int pin) {
//          Logger.log("TwoWire", 1, String.format("setUsedPin=%d", pin));
            m_usedPins |= (1L << pin);
        }
 
        long m_validPWMPins = 0;
        long m_validOutputPins = 0;
        long m_validInterruptPins = 0;
        long m_validInputPins = 0;
        long m_validAnalogPins = 0;
        int m_encoderRange = 0;
        
        private int getEncoderRange() {
            if (m_encoderRange == 0) {
                m_encoderRange = sendRequestInt(m_addr, TwoWire.k_getEncoderRange);
            }
            
            return m_encoderRange;
        }
 
        private boolean isPinValid(int pin, robotCore.Device.PinType type) {
            long valid = 0;
 
            switch (type) {
            case PWMPin:
                if (m_validPWMPins == 0) {
                    if (robotCore.Device.m_boardVersion != 3) {
                        m_validPWMPins = sendRequestInt(m_addr, TwoWire.k_getValidPWMPins);
                        // Logger.log("TwoWire", 1, String.format("validPWM=0x%x", (int) m_validPWMPins));
                        // while (true);
                    }
                    else {
                        m_validPWMPins = sendRequestLong(m_addr, TwoWire.k_getValidPWMPins);
                    }
                }
                valid = m_validPWMPins;
                break;
 
            case OutputPin:
                if (m_validOutputPins == 0) {
//                  Logger.log("TwoWire", 1, String.format("outputPin: ver=%d", robotCore.Device.m_boardVersion));
                    if (robotCore.Device.m_boardVersion == 4) {
//                      Logger.log("TwoWire", 1, "outputPin: sendRequestInt");
                        m_validOutputPins = sendRequestInt(m_addr, TwoWire.k_getValidOutputPins);
 
                    }
                    else {
//                      Logger.log("TwoWire", 1, "outputPin: sendRequestLong");
                        m_validOutputPins = sendRequestLong(m_addr, TwoWire.k_getValidOutputPins);
                    }
                }
                valid = m_validOutputPins;
                break;
 
            case AnalogPin:
                if (m_validAnalogPins == 0) {
                    if (robotCore.Device.m_boardVersion != 3) {
                        m_validAnalogPins = sendRequestInt(m_addr, TwoWire.k_getValidAnalogPins);
                    }
                    else {
                        m_validAnalogPins = sendRequestLong(m_addr, TwoWire.k_getValidAnalogPins);
                    }
                }
                valid = m_validAnalogPins;
                break;
 
            case InterruptPin:
                if (m_validInterruptPins == 0) {
                    if (robotCore.Device.m_boardVersion != 3) {
                        m_validInterruptPins = sendRequestInt(m_addr, TwoWire.k_getValidInterruptPins);
                    }
                    else {
                        m_validInterruptPins = sendRequestLong(m_addr, TwoWire.k_getValidInterruptPins);
                    }
                }
                valid = m_validInterruptPins;
                break;
 
            case InputPin:
                if (m_validInputPins == 0) {
                    if (robotCore.Device.m_boardVersion == 4) {
                        m_validInputPins = sendRequestInt(m_addr, TwoWire.k_getValidInputPins);
                    }
                    else {
                        m_validInputPins = sendRequestLong(m_addr, TwoWire.k_getValidInputPins);
                    }
                }
                valid = m_validInputPins;
                break;
            }
 
            return ((1L << pin) & valid) != 0;
        }
    }
 
    // private I2CBus m_i2cBus;
    private ArrayList<Device> m_devices = new ArrayList<Device>();
    private ArrayBlockingQueue<Command> m_commands = new ArrayBlockingQueue<Command>(30);
 
    interface RunCommand {
        void run();
    }
 
    private List<RunCommand> m_runCommands = Collections.synchronizedList(new ArrayList<RunCommand>());
    Object m_lock = new Object();
 
    public void addRunCommand(RunCommand command) {
//        Logger.log("TwoWire", 1, "addRunCommand");
        synchronized (m_lock) {
            m_runCommands.add(command);
        }
    }
 
    private int addCommand(Command command, byte[] data, int index) {
        // Logger.log("TwoWire", 1, String.format("addCommand: %d, len=%d", (int)
        // command.m_command[2], command.m_command[0]-4));
 
        data[index++] = command.m_command[2];
        for (int i = 0; i < command.m_command[0] - 4; i++) {
            data[index++] = command.m_command[i + 3];
        }
 
        return (index);
    }
 
    private Command combineCommands(Command command) {
        if (command.m_command[2] > 0) {
            // Can only combine commands that do not return a value
            Command nextCommand;
            byte[] data = new byte[k_maxCommandLength - 4];
            int index = 1;
            int nCommands = 0;
            int addr = command.m_addr;
 
            while ((nextCommand = m_commands.peek()) != null) {
                int commandNo = nextCommand.m_command[2];
 
                if ((commandNo < 0) || (nextCommand.m_addr != addr)) {
                    // Cannot combine commands which return a value or have different addresses
                    break;
                }
 
                int length = nextCommand.m_command[0] - 3; // Length of the command not including size, packetNo and
                                                           // chsum
 
                if (nCommands == 0) {
                    length += command.m_command[0] - 3; // Must add the first command
                }                                                           
 
                if ((index + length) > data.length) {
                    // The combined command would exceed the max command length
                    break;
                }
                // There is room for the is new command so copy the command number and all data
 
                m_commands.poll(); // pop the command
 
                if (nCommands == 0) {
                    // Add the first command
                    // if (command.m_command[0] >= 30) {
                    //     Logger.log("TwoWire", 1, String.format("len=%d,cmd=%d", command.m_command[0], command.m_command[2]));
                    // }
                    // System.out.println(String.format("C0:%d", command.m_command[0]));
                    index = addCommand(command, data, index);
                    nCommands++;
                }
                // System.out.println(String.format("c+:%d", nextCommand.m_command[0]));
                index = addCommand(nextCommand, data, index);
                nCommands++;
            }
 
            if (nCommands > 1) {
                // Logger.log("TwoWire", 1, String.format("%d(%d): %d combined, len=%d", addr, m_commands.remainingCapacity(), nCommands, index));
                // We have combined commands, so create a new command using this data
                data[0] = (byte) nCommands;
 
                // Logger.log("TwoWire", 1, String.format("CombineCommands: addr=%d,length=%d", addr, index));
 
                command = new Command(addr, k_multipleCommands, 0, data, index, null, null);
            }
        }
 
        return command;
    }
 
    private long m_threadId = 0;
 
    void processCommand(Command command) {
        // Logger.log("TwoWire", 1, String.format("processCommand: %d", command.m_command[2]));
        Device device = getDevice(command.m_addr);
        command.m_response = device.sendPacket(command.m_command, command.m_responseSize);
 
        if (command.m_callback != null) {
            ByteBuffer buffer = ByteBuffer.wrap(command.m_response, 3, command.m_response.length - 3);
            buffer.order(ByteOrder.LITTLE_ENDIAN);
            command.m_callback.callback(buffer, command.m_object);
        } else {
            synchronized (command) {        // TODO - is this needed?
                command.notify();
            }
        } 
        
        synchronized (m_keepAliveLock) {
            m_keepAliveTime = HALUtil.getMonotonicMilliseconds();
        }
    }
 
    static Object m_keepAliveLock = new Object();
    static long m_keepAliveTime = 0;
 
    static public long getKeepAliveTime() {
        synchronized (m_keepAliveLock) {
            return m_keepAliveTime;
        }
    }
 
    public TwoWire() {
        // Logger.setLogFile("TwoWireLog", "TwoWire", true);
        Logger.setLogLevel("TwoWireLog", 5);    // Turn off this logging
        // try {
        //     m_i2cBus = I2CFactory.getInstance(I2CBus.BUS_1);
        // } catch (UnsupportedBusNumberException | IOException e) {
        //     e.printStackTrace();
        // }
 
        /*
         * Start the thread that will actually send packets
         * This is the ONLY thread that will access the I2C bus
         */
        int k_runPeriod = 19; // Run commands every 20ms
 
        new Thread(
                new Runnable() {
                    public void run() {
                        m_threadId = Thread.currentThread().getId();
 
                        long nextRun = HALUtil.getMonotonicMilliseconds();
 
                        while (true) {
                            try {
                                Command command;
                                long dt = nextRun - HALUtil.getMonotonicMilliseconds();
 
                                if (dt <= 0) {
                                    command = m_commands.poll();
                                } else {
                                    command = m_commands.poll(dt, TimeUnit.MILLISECONDS);
                                }
 
                                if (command != null) {
                                    if (robotCore.Device.m_boardVersion != 4) {
                                        command = combineCommands(command);
                                    }
                                                        
                                    processCommand(command);
                                }
 
                                if ((nextRun - HALUtil.getMonotonicMilliseconds()) <= 0) {
                                      synchronized (m_lock) {
                                        for (RunCommand runCommand : m_runCommands) {
                                            runCommand.run();
                                        }
                                    }
 
                                    nextRun += k_runPeriod;
                                }
                            } catch (InterruptedException e) {
                                e.printStackTrace();
                            }
                        }
                    }
                }).start();
    }
 
    private Device getDevice(int addr) {
        Device device;
 
        for (int i = 0; i < m_devices.size(); i++) {
            device = m_devices.get(i);
 
            if (addr == device.m_addr) {
                return (device);
            }
        }
 
        // System.out.println(String.format("TwoWire: Adding device: %d", addr));
        if (addr == 0) {
            throw new HardwareException("Addr is zero");
        }
 
        device = new Device(addr);
 
        m_devices.add(device);
 
        return (device);
    }
 
    /*
     * Send command, no response expected
     */
    private void sendCommand(Command command) {
//        Logger.log("TwoWireLog", 1, String.format("sendCommand: t=%d", Thread.currentThread().getId()));
        try {
            if (Thread.currentThread().getId() == m_threadId) {
                // if (m_commands.remainingCapacity() == 0) {
                //     throw new HardwareException("Queue is full");
                // }
                // This is the command processing thread to process the command now
                processCommand(command);
            }
            else {
                 // If this is not the thread that reads from the queue, do not fill up the queue
                // while (m_commands.remainingCapacity() < 8) {
                //     Logger.log("TwoWire", 1,
                //             String.format("send: t=%d, s=%d", Thread.currentThread().getId(), m_commands.size()));
                //     RobotCoreBase.sleep(1);
                // }
                m_commands.put(command);
            }
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }
 
    /*
     * Command request, wait for response
     */
    private ByteBuffer sendRequest(Command command) {
        try {
            sendCommand(command);
 
            if (command.m_callback == null) {
                synchronized (command) {
                    ByteBuffer buffer;
 
                    command.wait();
 
                    // System.out.printf("%d: ", command.m_command[3] & 0xff);
                    // for (int i = 0 ; i < command.m_response.length ; i++) {
                    //     System.out.printf("%02x ", command.m_response[i]);
                    // }
                    // System.out.println();
 
                    buffer = ByteBuffer.wrap(command.m_response, 3, command.m_response.length - 3);
                    buffer.order(ByteOrder.LITTLE_ENDIAN);
 
                    return (buffer);
                }
            }
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
 
        return null;
    }
 
    /*
     * Simple command, no arguments
     */
    public void sendCommand(int addr, int commandId) {
        Command command = new Command(addr, commandId, 0);
 
        sendCommand(command);
    }
 
    public void sendCommandToAll(int commandId) {
        for (int i = 0; i < m_devices.size(); i++) {
            Device device = m_devices.get(i);
            Command command = new Command(device.m_addr, commandId, 0);
 
            sendCommand(command);
        }
    }
 
    /*
     * Command with a byte argument
     */
    public void sendCommand(int addr, int commandId, byte byte1) {
        Command command = new Command(addr, commandId, 0, byte1, null, null);
 
        sendCommand(command);
    }
 
    /*
     * Command with a byte and a 16 bit argument
     */
    public void sendCommand(int addr, int commandId, byte byte1, short value) {
        Command command = new Command(addr, commandId, 0, byte1, value);
 
        sendCommand(command);
    }
 
    /*
     * Command with a byte and a 32 bit argument
     */
    public void sendCommand(int addr, int commandId, byte byte1, int value) {
        Command command = new Command(addr, commandId, 0, byte1, value);
 
        sendCommand(command);
    }
 
    /*
     * Command with n bytes of data
     */
    public void sendCommand(int addr, int commandId, byte[] bytes) {
        Command command = new Command(addr, commandId, bytes);
 
        sendCommand(command);
    }
 
    /*
     * Command with 4 bytes of data
     */
    public void sendCommand(int addr, int commandId, byte byte1, byte byte2, byte byte3, byte byte4) {
        Command command = new Command(addr, commandId, 0, byte1, byte2, byte3, byte4);
 
        sendCommand(command);
    }
 
    /*
     * Command with byte and a float (32 bit) arguments
     */
    public void sendCommand(int addr, int commandId, byte byte1, float value) {
        Command command = new Command(addr, commandId, 0, byte1, value);
 
        sendCommand(command);
    }
 
    /*
     * Command with two bytes as arguments
     */
    public void sendCommand(int addr, int commandId, byte byte1, byte byte2) {
        Command command = new Command(addr, commandId, 0, byte1, byte2);
 
        sendCommand(command);
    }
 
    public void sendCommand(int addr, int commandId, int yaw, int x, int y) {
        Command command = new Command(addr, commandId, 0, yaw, x, y);
 
        sendCommand(command);
    }
 
    /*
     * Simple request, no arguments
     * Note: the Arduino will only wait for a response if the commandId is greater
     * than 127
     */
    public ByteBuffer sendRequest(int addr, int commandId, int size) {
        return (sendRequest(new Command(addr, commandId, size)));
    }
 
    /*
     * Takes N one byte arguments and returns N bytes of data
     */
    public ByteBuffer sendRequest(int addr, int commandId, int size, byte[] args, Callback callback) {
        return sendRequest(new Command(addr, commandId, size, args, callback, null));
    }
 
    /*
     * One byte argument
     */
    public ByteBuffer sendRequest(int addr, int commandId, int size, byte byte1) {
        return (sendRequest(new Command(addr, commandId, size, byte1, null, null)));
    }
 
    public void sendRequest(int addr, int commandId, int size, byte byte1, Callback callback, Object object) {
        sendRequest(new Command(addr, commandId, size, byte1, callback, object));
    }
 
    /*
     * Simple request, no arguments, returns byte
     */
    public int sendRequestByte(int addr, int commandId) {
        ByteBuffer buffer = sendRequest(addr, commandId, 1);
 
        return (buffer.get());
    }
 
    /*
     * One byte argument, returns byte
     */
    public int sendRequestByte(int addr, int commandId, byte byte1) {
        ByteBuffer buffer = sendRequest(addr, commandId, 1, byte1);
 
        return (buffer.get());
    }
 
    /*
     * No arguments, returns int (32 bits)
     */
    public int sendRequestInt(int addr, int commandId) {
        ByteBuffer buffer = sendRequest(addr, commandId, 4);
        
//        Logger.log("TwoWire", 1, String.format("sendRequestInt: rem=%d, cap=%d, lim=%d", buffer.remaining(), buffer.capacity(), buffer.limit()));
 
        return (buffer.getInt());
    }
 
    /*
     * Takes N single byte arguments and returns N bytes of data
     */
    public void sendRequestBytes(int addr, int commandId, byte[] args, int responseSize, Callback callback) {
        // ByteBuffer buffer =
        sendRequest(addr, commandId, responseSize, args, callback);
 
        // byte[] ret = new byte[args.length];
 
        // for (int i = 0 ; i < args.length ; i++) {
        // ret[i] = buffer.get();
        // }
 
        // return ret;
        // return null;
    }
 
    /*
     * No arguments, returns long (64 bits)
     */
    public long sendRequestLong(int addr, int commandId) {
        ByteBuffer buffer = sendRequest(addr, commandId, 8);
 
        return (buffer.getLong());
    }
 
    /*
     * One byte argument, returns int (32 bits)
     */
    public int sendRequestInt(int addr, int commandId, byte byte1) {
        ByteBuffer buffer = sendRequest(addr, commandId, 4, byte1);
 
        return (buffer.getInt());
    }
 
    public void sendRequestInt(int addr, int commandId, byte byte1, Callback callback, Object object) {
        sendRequest(addr, commandId, 4, byte1, callback, object);
    }
 
    /*
     * One byte argument, returns short (16 bits)
     */
    public int sendRequestShort(int addr, int commandId, byte byte1) {
        ByteBuffer buffer = sendRequest(addr, commandId, 2, byte1);
 
        return (buffer.getShort());
    }
 
    public boolean checkUsedPin(int addr, int pin) {
        Device device = getDevice(addr);
        
//        Logger.log("TwoWire", 1, String.format("checkPin %d=0x%x", pin, device.getUsedPins()));
 
        return ((device.getUsedPins() & (1L << pin)) != 0);
    }
 
    public void setUsedPin(int addr, int pin) {
        Device device = getDevice(addr);
 
        device.setUsedPin(pin);
    }
 
    public boolean isPinValid(int addr, int pin, robotCore.Device.PinType type) {
        Device device = getDevice(addr);
        
        return device.isPinValid(pin, type);
    }
    
    public int getEncoderRange(int addr) {
        Device device = getDevice(addr);
        
        return device.getEncoderRange();
    }
}