diff --git a/src/main/java/frc/robot/Pixy.java b/src/main/java/frc/robot/Pixy.java deleted file mode 100644 index 94c01b5..0000000 --- a/src/main/java/frc/robot/Pixy.java +++ /dev/null @@ -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; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6df4895..9368be5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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)); diff --git a/src/main/java/frc/robot/subsystem/Pixy.java b/src/main/java/frc/robot/subsystem/Pixy.java index 468a421..8cd59b9 100644 --- a/src/main/java/frc/robot/subsystem/Pixy.java +++ b/src/main/java/frc/robot/subsystem/Pixy.java @@ -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; + } +} \ No newline at end of file