PLS5618/robot-2024
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Pixy

Browse Source
This commit is contained in:
samuel desharnais
2024-02-06 19:01:04 -05:00
parent add3237dc5
commit b6e9c303f3
3 changed files with 72 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
src/main/java/frc/robot/Pixy.java
View File

@@ -1,73 +0,0 @@
package frc.robot;
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 {
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;
}
}

2
src/main/java/frc/robot/RobotContainer.java
View File

@@ -37,6 +37,7 @@ import frc.robot.subsystem.Grimpeur;
import frc.robot.subsystem.Guideur;
import frc.robot.subsystem.LED;
import frc.robot.subsystem.Lanceur;
import frc.robot.subsystem.Pixy;
public class RobotContainer {
@@ -59,6 +60,7 @@ public class RobotContainer {
GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getLeftY);
LED LED = new LED();
AllumeLED allumeLED = new AllumeLED(LED);
Pixy pixy = new Pixy();
public RobotContainer() {
NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur));
NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur));

82
src/main/java/frc/robot/subsystem/Pixy.java
View File

@@ -1,17 +1,75 @@
// 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.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 {
/** Creates a new Pixy. */
public Pixy() {}
SerialPort pixy;
Port port = Port.kMXP;
PixyPacket[] packets;
PixyException pExc;
String print;
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
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;
}
}