IRLink.java

package lejos.nxt.addon;

import lejos.nxt.I2CPort;
import lejos.nxt.I2CSensor;
import java.util.*;

/*
 * WARNING: THIS CLASS IS SHARED BETWEEN THE classes AND pccomms PROJECTS.
 * DO NOT EDIT THE VERSION IN pccomms AS IT WILL BE OVERWRITTEN WHEN THE PROJECT IS BUILT.
 */

/**
 * Supports for HiTechnic IRLink - see  http://www.hitechnic.com/.
 * 
 * @author Lawrie Griffiths
 *
 */
public class IRLink extends I2CSensor {
	
	//Registers
	private final static byte TX_BUFFER = 0x40; // 40 to 4C
	private final static byte TX_BUFFER_LEN = 0x4D;
	private final static byte TX_MODE = 0x4E;
	private final static byte TX_BUFFER_FLAG = 0x4F;
	
	private final static byte TX_MAX_BUFFER_LEN = 13;
	
	// IRLink transmission modes
	private final static byte TX_MODE_RCX = 0;
	private final static byte TX_MODE_TRAIN = 1;
	private final static byte TX_MODE_PF = 2;
	
	// PF Modes
	public final static byte PF_MODE_COMBO_DIRECT = 1;
	
	// IR PF signal encoding parameters
	private final byte MAX_BITS = TX_MAX_BUFFER_LEN * 8;
	private final byte STOP_START_PAUSE = 7;
	private final byte LOW_BIT_PAUSE = 2;
	private final byte HIGH_BIT_PAUSE = 4;
	
	// PF motor operations
	public static final byte PF_FLOAT = 0;
	public static final byte PF_FORWARD = 1;
	public static final byte PF_BACKWARD = 2;
	public static final byte PF_BRAKE = 3;
	
	private byte toggle = 0;
	
	private BitSet bits = new BitSet(MAX_BITS);
	private int nextBit = 0;

	public IRLink(I2CPort port) {
		super(port);
	}
	
	/**
	 * Send commands to both motors.
	 * Uses PF Combo direct mode.
	 * 
	 * @param channel the channel number (0-3)
	 * @param opA Motor A operation
	 * @param opB Motor B operation
	 */
	public void sendPFComboDirect(int channel, int opA, int opB) {
		sendPFCommand(channel, PF_MODE_COMBO_DIRECT, opB << 2 | opA);
	}

	private void sendPFCommand(int channel, int mode, int data) {
		byte nibble1 = (byte) ((toggle << 3) | channel);
		byte lrc = (byte) (0xF ^ nibble1 ^ mode ^ data);
		int pfData = (nibble1 << 12) | (mode << 8) | (data << 4) | lrc;

		clearBits();
		nextBit = 0;
		setBit(STOP_START_PAUSE); // Start
		for(int i=15;i>=0;i--) {
			setBit(((pfData >> i) & 1) == 0 ? LOW_BIT_PAUSE : HIGH_BIT_PAUSE);
		}
		setBit(STOP_START_PAUSE); // Stop
		toggle ^= 1;
		byte [] pfCommand = new byte[16];
		
		for(int i =0;i<MAX_BITS;i++) {
			boolean bit = bits.get(i);
			int byteIndex = i/8;
			int bitVal = (bit ? 1 : 0);
			pfCommand[byteIndex] |= (bitVal << (7 - i%8));
		}
		
		pfCommand[13] = TX_MAX_BUFFER_LEN;
	    pfCommand[14] = TX_MODE_PF;
	    pfCommand[15] = 1;

		sendData(TX_BUFFER, pfCommand, TX_MAX_BUFFER_LEN+3);
	}
	
	private void setBit(int pause) {
		bits.set(nextBit++);
		nextBit += pause;
	}
	
	private void clearBits() {
		for(int i=0;i<MAX_BITS;i++) bits.clear(i);
	}
}