diff --git a/src/main/java/frc/robot/PixyException.java b/src/main/java/frc/robot/PixyException.java new file mode 100644 index 0000000..c4fd3c0 --- /dev/null +++ b/src/main/java/frc/robot/PixyException.java @@ -0,0 +1,8 @@ +package frc.robot; + +public class PixyException extends Exception { + public PixyException(String message){ + super(message); + } + +} diff --git a/src/main/java/frc/robot/PixyPacket.java b/src/main/java/frc/robot/PixyPacket.java new file mode 100644 index 0000000..d52adfe --- /dev/null +++ b/src/main/java/frc/robot/PixyPacket.java @@ -0,0 +1,9 @@ +package frc.robot; + +public class PixyPacket { + public int X; + public int Y; + public int Width; + public int Height; + +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b50586..f1ad8ec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,7 +19,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Manettes import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - +import frc.robot.command.AllumeLED; // Commands import frc.robot.command.Balayer; import frc.robot.command.GrimpeurDroit; @@ -35,7 +35,9 @@ import frc.robot.subsystem.Balayeuse; import frc.robot.subsystem.Drive; 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 { @@ -57,25 +59,31 @@ public class RobotContainer { //command GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftX); 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)); autoChooser = AutoBuilder.buildAutoChooser(); CameraServer.startAutomaticCapture(); - manette.a().onTrue(guiderBas); - manette.b().onTrue(guiderHaut); + manette.a().whileTrue(guiderBas); + manette.b().whileTrue(guiderHaut); + joystick.button(3).toggleOnTrue(balayer); + configureBindings(); drive.setDefaultCommand(new RunCommand(()->{ drive.drive(-MathUtil.applyDeadband(joystick.getY(),0.2), MathUtil.applyDeadband(-joystick.getX(),0.2), MathUtil.applyDeadband(-joystick.getZ(), 0.2)); },drive)); - + grimpeur.setDefaultCommand(new RunCommand(()->{ + grimpeur.droit(manette.getLeftX());} + ,grimpeur)); + LED.setDefaultCommand(allumeLED); } private void configureBindings() { - joystick.button(3).toggleOnTrue(balayer); - } public Command getAutonomousCommand(){ diff --git a/src/main/java/frc/robot/command/AllumeLED.java b/src/main/java/frc/robot/command/AllumeLED.java new file mode 100644 index 0000000..dd1d09e --- /dev/null +++ b/src/main/java/frc/robot/command/AllumeLED.java @@ -0,0 +1,48 @@ +// 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.Accumulateur; +import frc.robot.subsystem.LED; + +public class AllumeLED extends Command { + private LED led; + private Accumulateur accumulateur; + /** Creates a new AllumeLED. */ + public AllumeLED(LED led) { + this.accumulateur = accumulateur; + this.led = led; + addRequirements(led); + // 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() { + if(accumulateur.limitswitch()){ + led.couleur(0, 255, 0); + } + else{ + led.couleur(255, 0, 0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + led.couleur(255, 0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/GuiderHaut.java b/src/main/java/frc/robot/command/GuiderHaut.java index 1eb052e..74719ca 100644 --- a/src/main/java/frc/robot/command/GuiderHaut.java +++ b/src/main/java/frc/robot/command/GuiderHaut.java @@ -40,6 +40,6 @@ public class GuiderHaut extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return guideur.haut()==true; +return guideur.haut()==true; } } diff --git a/src/main/java/frc/robot/command/LancerNote.java b/src/main/java/frc/robot/command/LancerNote.java index d653184..133492b 100644 --- a/src/main/java/frc/robot/command/LancerNote.java +++ b/src/main/java/frc/robot/command/LancerNote.java @@ -15,6 +15,7 @@ public class LancerNote extends Command { public LancerNote(Lanceur lancer, Accumulateur accumulateur) { this.lancer = lancer; this.accumulateur = accumulateur; + addRequirements(lancer); // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/command/Limelight_tracker.java b/src/main/java/frc/robot/command/Limelight_tracker.java new file mode 100644 index 0000000..78d0f91 --- /dev/null +++ b/src/main/java/frc/robot/command/Limelight_tracker.java @@ -0,0 +1,49 @@ +// 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.Limelight; + +public class Limelight_tracker extends Command { + /** Creates a new Limelight_tracker. */ + private Limelight tracker; + private Drive drive; + + public Limelight_tracker(Limelight tracker, Drive drive) { + this.drive = drive; + this.tracker = tracker; + addRequirements(tracker,drive); + } + + // 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() { + + if (tracker.getv()){ + drive.drive(0,0, tracker.getx()); + } + else{ + drive.drive(0, 0, 0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drive.drive(0, 0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/RobotPixy.java b/src/main/java/frc/robot/command/RobotPixy.java new file mode 100644 index 0000000..652c4c3 --- /dev/null +++ b/src/main/java/frc/robot/command/RobotPixy.java @@ -0,0 +1,40 @@ +// 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; + } +} diff --git a/src/main/java/frc/robot/subsystem/LED.java b/src/main/java/frc/robot/subsystem/LED.java new file mode 100644 index 0000000..8bc91cc --- /dev/null +++ b/src/main/java/frc/robot/subsystem/LED.java @@ -0,0 +1,27 @@ +// 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.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LED extends SubsystemBase { + /** Creates a new LED. */ + public LED() {} + AddressableLED led = new AddressableLED(9); + AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(60); + public void led(){ + led.setData(ledBuffer); + led.start(); + } + public void couleur(int R, int G, int B){ + ledBuffer.setRGB(0, R, G, B); + } + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystem/Limelight.java b/src/main/java/frc/robot/subsystem/Limelight.java index 9fb068b..ab2358f 100644 --- a/src/main/java/frc/robot/subsystem/Limelight.java +++ b/src/main/java/frc/robot/subsystem/Limelight.java @@ -11,27 +11,44 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; - public class Limelight extends SubsystemBase { - /** Creates a new Limelight. */ - public Limelight() { + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); - NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight"); NetworkTableEntry tx = table.getEntry("tx"); NetworkTableEntry ty = table.getEntry("ty"); NetworkTableEntry ta = table.getEntry("ta"); + NetworkTableEntry pipeline = table.getEntry("pipeline"); + NetworkTableEntry tv = table.getEntry("tv"); + NetworkTableEntry camMode = table.getEntry("camMode"); + NetworkTableEntry tid = table.getEntry("tid"); - double x = tx.getDouble(0.0); - double y = ty.getDouble(0.0); - double area = ta.getDouble(0.0); - - SmartDashboard.putNumber("LimelightX", x); - SmartDashboard.putNumber("LimelightY", y); - SmartDashboard.putNumber("LimelightArea", area); - -} + /** Creates a new Limelight. */ + public Limelight() { + } + public double getx(){ + return tx.getDouble(0); + } + public double gety(){ + return ty.getDouble(0); + } + /* public double geta() { + return ta.getDouble(0); + }*/ + public boolean getv(){ + return tv.getBoolean(false); + } + public void setpipeline(){ + pipeline.setNumber(0); + } + public void setcamMode(){ + camMode.setNumber(0); + } + public double getTid(){ + return tid.getDouble(0); + } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystem/Pixy.java b/src/main/java/frc/robot/subsystem/Pixy.java index f56c0d2..8cd59b9 100644 --- a/src/main/java/frc/robot/subsystem/Pixy.java +++ b/src/main/java/frc/robot/subsystem/Pixy.java @@ -1,18 +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