Merge branch 'main' of https://git.demerso.net/pls5618/2024/robot

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);
+	}
+
+}

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;
+
+}

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.a().whileTrue(guiderBas);
-    manette.b().onTrue(guiderHaut);
+    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(){

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;
+  }
+}

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; 
   }
 }

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.
   }
 

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;
+  }
+}

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;
+  }
+}

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
+  }
+}

src/main/java/frc/robot/subsystem/Limelight.java

@@ -11,26 +11,43 @@ import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 
-
 public class Limelight extends SubsystemBase {
-  /** Creates a new Limelight. */
+  NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
-  public 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);
+     /** Creates a new Limelight. */
-  double y = ty.getDouble(0.0);
+  public Limelight() {
-  double area = ta.getDouble(0.0);
 
-  SmartDashboard.putNumber("LimelightX", x);
+  }
-  SmartDashboard.putNumber("LimelightY", y);
-  SmartDashboard.putNumber("LimelightArea", area);
-
-}
   
+  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() {

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