working i2c pi4j code for Pixy

[rlsutton1]

Thought I’d share the java code to read the frames from the pixy via i2c using pi4j

The sync still needs some work though.

package com.pi4j.gpio.extension.pixy;

import java.io.IOException;
import java.util.LinkedList;
import java.util.List;

import au.com.rsutton.entryPoint.SynchronizedDeviceWrapper;

import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CDevice;
import com.pi4j.io.i2c.I2CFactory;

public class PixyCmu5
{
	private static final int MAX_FRAMES = 10;
	private I2CBus bus;
	private I2CDevice magDevice;

	public void setup() throws IOException
	{

		// create I2C communications bus instance
		bus = I2CFactory.getInstance(1);

		// create I2C device instance
		magDevice = new SynchronizedDeviceWrapper(bus.getDevice(0x54));

	}

	public class Frame
	{
		// 0, 1 0 sync (0xaa55)
		// 2, 3 1 checksum (sum of all 16-bit words 2-6)
		// 4, 5 2 signature number
		// 6, 7 3 x center of object
		// 8, 9 4 y center of object
		// 10, 11 5 width of object
		// 12, 13 6 height of object

		int sync = 0;
		int checksum = 0;
		public int signature;
		public int xCenter;
		public int yCenter;
		public int width;
		public int height;
	}

	public List getFrames() throws IOException
	{

		List frames = new LinkedList();
		byte[] bytes = new byte[14 * MAX_FRAMES];
		for (int i = 0; i < bytes.length; i++)
		{
			bytes[i] = 0;
		}

		// wait for sync byte
		if (magDevice.read() != 0x55)
		{
			return frames;
		}

		if (magDevice.read() != 0xaa)
		{
			return frames;
		}

		// don't lose the sync byte
		// bytes[0] = (byte) 0x55;
		int offset = 0;

		// read frames.
		int read = magDevice.read(bytes, offset, bytes.length);
		for (int r = 0; r < read; r += 14)
		{
			// System.out.print("f ");
			// for (int y = 0;y < 14;y++)
			// {
			// int t = bytes[r+y];
			// if (t < 0)
			// t = t + 256;
			// System.out.print(" " + Integer.toHexString(t));
			// }
			// System.out.println("");
			Frame frame = new Frame();
			frame.sync = convertBytesToInt(bytes[r + 1], bytes[r + 0]);
			// System.out.println("\nsync: "+Integer.toHexString(frame.sync));
			frame.checksum = convertBytesToInt(bytes[r + 3], bytes[r + 2]);
			frame.signature = convertBytesToInt(bytes[r + 5], bytes[r + 4]);
			frame.xCenter = convertBytesToInt(bytes[r + 7], bytes[r + 6]);
			frame.yCenter = convertBytesToInt(bytes[r + 9], bytes[r + 8]);
			frame.width = convertBytesToInt(bytes[r + 11], bytes[r + 10]);
			frame.height = convertBytesToInt(bytes[r + 13], bytes[r + 12]);

			if (frames.size() > MAX_FRAMES)
			{
				break;
			}
			// sync must equal =0x55aa;
			if (frame.sync != 0xaa55)
			{
				// System.out.println("Bad Pixy frame sync = " +
				// frame.sync+" "+frame.checksum);
				break;
			}
			// if the checksum is 0 or the checksum is a sync byte, then there
			// are no more frames.
			if (frame.checksum == 0 || frame.checksum == 0xaa55)
			{
				break;
			}
			frames.add(frame);
			offset = 0;
		}
		return frames;
	}

	public int convertBytesToInt(int msb, int lsb)
	{
		// System.out.println(Integer.toHexString(msb)+" "+Integer.toHexString(lsb));
		if (msb < 0)
			msb += 256;
		int value = msb * 256;

		if (lsb < 0)
		{
			// lsb should be unsigned
			value += 256;
		}
		value += lsb;
		return value;
	}

}

[rlsutton1]

revised code, much better handling of sync

package com.pi4j.gpio.extension.pixy;

import java.io.IOException;
import java.util.LinkedList;
import java.util.List;

import au.com.rsutton.entryPoint.SynchronizedDeviceWrapper;

import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CDevice;
import com.pi4j.io.i2c.I2CFactory;

public class PixyCmu5
{
	private static final int FRAME_SIZE = 14;
	private static final int MAX_FRAMES = 20;
	private I2CBus bus;
	private I2CDevice magDevice;

	public void setup() throws IOException
	{

		// create I2C communications bus instance
		bus = I2CFactory.getInstance(1);

		// create I2C device instance
		magDevice = new SynchronizedDeviceWrapper(bus.getDevice(0x54));

	}

	public class Frame
	{
		// 0, 1 0 sync (0xaa55)
		// 2, 3 1 checksum (sum of all 16-bit words 2-6)
		// 4, 5 2 signature number
		// 6, 7 3 x center of object
		// 8, 9 4 y center of object
		// 10, 11 5 width of object
		// 12, 13 6 height of object

		int sync = 0;
		int checksum = 0;
		public int signature;
		public int xCenter;
		public int yCenter;
		public int width;
		public int height;
	}

	public Frame getFrame(byte[] bytes)
	{
		Frame frame = new Frame();
		frame.sync = convertBytesToInt(bytes[1], bytes[0]);
		// System.out.println("\nsync: "+Integer.toHexString(frame.sync));
		frame.checksum = convertBytesToInt(bytes[3], bytes[2]);

		// if the checksum is 0 or the checksum is a sync byte, then there
		// are no more frames.
		if (frame.checksum == 0 || frame.checksum == 0xaa55)
		{
			return null;
		}
		frame.signature = convertBytesToInt(bytes[5], bytes[4]);
		frame.xCenter = convertBytesToInt(bytes[7], bytes[6]);
		frame.yCenter = convertBytesToInt(bytes[9], bytes[8]);
		frame.width = convertBytesToInt(bytes[11], bytes[10]);
		frame.height = convertBytesToInt(bytes[13], bytes[12]);

		return frame;
	}

	public List getFrames() throws IOException
	{

		List frames = new LinkedList();
		byte[] bytes = new byte[FRAME_SIZE * MAX_FRAMES];

		// read data from pixy
		int bytesRead = magDevice.read(bytes, 0, bytes.length);

		System.out.println("Bytes read " + bytesRead);

		// search for sync
		for (int byteOffset = 0; byteOffset < bytesRead - (FRAME_SIZE - 1);)
		{
			
			int b1 = bytes[byteOffset];
			if (b1 < 0)
			{
				b1+=256;
			}
			int b2 = bytes[byteOffset+1];
			if (b2 < 0)
			{
				b2+=256;
			}

			if (b1 == 0x55 && b2 == 0xaa)
			{
				// found sync
				byte[] tempBytes = new byte[FRAME_SIZE];
				for (int tempByteOffset = 0; tempByteOffset < FRAME_SIZE; tempByteOffset++)
				{
					tempBytes[tempByteOffset] = bytes[byteOffset
							+ tempByteOffset];
				}
				Frame frame = getFrame(tempBytes);
				if (frame != null)
				{
					// it was a valid frame!
					frames.add(frame);
					// skip to next frame -1 as byteOffset will be incremented
					// at the end
					// of the loop block
					byteOffset += FRAME_SIZE - 1;
				} else
				{
					// it wasn't a valid frame, we can skip 2 bytes
					byteOffset++;
				}
			}
			byteOffset++;
		}

		return frames;
	}

	public int convertBytesToInt(int msb, int lsb)
	{
		// System.out.println(Integer.toHexString(msb)+" "+Integer.toHexString(lsb));
		if (msb < 0)
			msb += 256;
		int value = msb * 256;

		if (lsb < 0)
		{
			// lsb should be unsigned
			value += 256;
		}
		value += lsb;
		return value;
	}

}