Permalink
Find file
5525528 Feb 8, 2017
102 lines (96 sloc) 3.31 KB
package org.usfirst.frc2876.Steamworks2017.subsystems;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class PixyI2C {
PixyPacket values;
I2C pixy;
Port port = Port.kOnboard;
PixyPacket[] packets;
PixyException pExc;
String print;
public PixyI2C() {
// pixy = new I2C(port, 0x54);
// packets = new PixyPacket[7];
// pExc = new PixyException(print);
// values = new PixyPacket();
}
public PixyI2C(I2C argPixy, PixyPacket[] argPixyPacket, PixyException argPixyException, PixyPacket argValues){
pixy = argPixy;
packets = argPixyPacket;
pExc = argPixyException;
values = argValues;
}
//This method parses raw data from the pixy into readable integers
public int cvt(byte upper, byte lower) {
return (((int)upper & 0xff) << 8) | ((int)lower & 0xff);
}
/*
public void pixyReset(){
pixy.reset();
}
*/
//This method gathers data, then parses that data, and assigns the ints to global variables
public PixyPacket readPacket(int Signature) throws PixyException { //The signature should be which number object in
int Checksum; //pixymon you are trying to get data for
int Sig;
byte[] rawData = new byte[32];
SmartDashboard.putString("rawData", rawData[0] + " " + rawData[1] + " " + rawData[15] + " " + rawData[31]);
try{
pixy.readOnly(rawData, 32);
} catch (RuntimeException e){
SmartDashboard.putString("Pixy RuntimeException", "Error");
}
if(rawData.length < 32){
System.out.println("byte array length is broken");
return null;
}
for (int i = 0; i <= 16; i++) {
int syncWord = cvt(rawData[i+1], rawData[i+0]); //Parse first 2 bytes
if (syncWord == 0xaa55) { //Check is first 2 bytes equal a "sync word", which indicates the start of a packet of valid data
syncWord = cvt(rawData[i+3], rawData[i+2]); //Parse the next 2 bytes
if (syncWord != 0xaa55){ //Shifts everything in the case that one syncword is sent
i -= 2;
}
//This next block parses the rest of the data
Checksum = cvt(rawData[i+5], rawData[i+4]);
Sig = cvt(rawData[i+7], rawData[i+6]);
if(Sig <= 0 || Sig > packets.length){
break;
}
packets[Sig - 1] = new PixyPacket();
packets[Sig - 1].X = cvt(rawData[i+9], rawData[i+8]);
packets[Sig - 1].Y = cvt(rawData[i+11], rawData[i+10]);
packets[Sig - 1].Width = cvt(rawData[i+13], rawData[i+12]);
packets[Sig - 1].Height = cvt(rawData[i+15], rawData[i+14]);
//Checks whether the data is valid using the checksum *This if block should never be entered*
if (Checksum != Sig + packets[Sig - 1].X + packets[Sig - 1].Y + packets[Sig - 1].Width + packets[Sig - 1].Height) {
packets[Sig - 1] = null;
throw pExc;
}
break;
}
else
SmartDashboard.putNumber("syncword: ", syncWord);
}
//Assigns our packet to a temp packet, then deletes data so that we dont return old data
PixyPacket pkt = packets[Signature - 1];
packets[Signature - 1] = null;
return pkt;
}
public int getX(){
return values.X;
}
public int getY(){
return values.Y;
}
public int getWidth(){
return values.Width;
}
public int getHeight(){
return values.Height;
}
public int getArea(){
return getWidth() * getHeight();
}
}