This commit is contained in:
parent
f5919eb7f3
commit
1ef2671fcf
@ -1,8 +0,0 @@
|
||||
package frc.robot;
|
||||
|
||||
public class PixyException extends Exception {
|
||||
public PixyException(String message){
|
||||
super(message);
|
||||
}
|
||||
|
||||
}
|
@ -1,9 +0,0 @@
|
||||
package frc.robot;
|
||||
|
||||
public class PixyPacket {
|
||||
public int X;
|
||||
public int Y;
|
||||
public int Width;
|
||||
public int Height;
|
||||
|
||||
}
|
@ -1,40 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot.command;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc.robot.subsystem.Drive;
|
||||
import frc.robot.subsystem.Pixy;
|
||||
|
||||
public class RobotPixy extends Command {
|
||||
private Pixy pixy;
|
||||
private Drive drive;
|
||||
/** Creates a new RobotPixy. */
|
||||
public RobotPixy(Pixy pixy,Drive drive) {
|
||||
this.drive = drive;
|
||||
this.pixy = pixy;
|
||||
addRequirements(drive,pixy);
|
||||
// Use addRequirements() here to declare subsystem dependencies.
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
}
|
||||
|
||||
// Called once the command ends or is interrupted.
|
||||
@Override
|
||||
public void end(boolean interrupted) {}
|
||||
|
||||
// Returns true when the command should end.
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
}
|
@ -1,75 +0,0 @@
|
||||
package frc.robot.subsystem;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.PixyException;
|
||||
import frc.robot.PixyPacket;
|
||||
import edu.wpi.first.wpilibj.SerialPort;
|
||||
import edu.wpi.first.wpilibj.SerialPort.Port;
|
||||
//Warning: if the pixy is plugged in through mini usb, this code WILL NOT WORK b/c the pixy is smart and detects where it should send data
|
||||
public class Pixy extends SubsystemBase {
|
||||
SerialPort pixy;
|
||||
Port port = Port.kMXP;
|
||||
PixyPacket[] packets;
|
||||
PixyException pExc;
|
||||
String print;
|
||||
|
||||
public Pixy() {
|
||||
pixy = new SerialPort(19200, port);
|
||||
pixy.setReadBufferSize(14);
|
||||
packets = new PixyPacket[7];
|
||||
pExc = new PixyException(print);
|
||||
}
|
||||
//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 {
|
||||
int Checksum;
|
||||
int Sig;
|
||||
byte[] rawData = new byte[32];
|
||||
try{
|
||||
rawData = pixy.read(32);
|
||||
} catch (RuntimeException e){
|
||||
|
||||
}
|
||||
if(rawData.length < 32){
|
||||
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;
|
||||
}
|
||||
}
|
||||
//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;
|
||||
}
|
||||
}
|
Loading…
x
Reference in New Issue
Block a user