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

src/main/java/frc/robot/Constants.java

@@ -43,16 +43,16 @@ public class Constants {
     public static int ArriereGauche = 7;
 
     // Limit switch
-    public static int guideurhaut = 23;
-    public static int guideurbas = 24;
-    public static int limitacc = 25;
-    public static int limitacc2 = 76;
-    public static int limithaut = 28;
-    public static int limitbas = 29;
+    public static int guideurhaut = 0;
+    public static int guideurbas = 1;
+    public static int limitacc = 2;
+    public static int limitacc2 = 3;
+    public static int limithaut = 4;
+    public static int limitbas = 5;
 
     //piston
-    public static int pistondroiteouvre= 30;
-    public static int pistondroiteferme= 31;
-    public static int pistondgaucheouvre= 32;
-    public static int pistondgauchferme= 32;
+    public static int pistondroiteouvre= 0;
+    public static int pistondroiteferme= 1;
+    public static int pistondgaucheouvre= 2;
+    public static int pistondgauchferme= 3;
 }

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

@@ -4,54 +4,88 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 // Manette
+
+// Manettes
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-
-// Subsystem
+import frc.robot.command.AllumeLED;
+// Commands
+import frc.robot.command.Balayer;
+import frc.robot.command.GrimpeurDroit;
+import frc.robot.command.GrimpeurGauche;
+import frc.robot.command.GuiderBas;
+import frc.robot.command.GuiderHaut;
+import frc.robot.command.Lancer;
+import frc.robot.command.LancerNote;
+import frc.robot.command.Lancerampli;
+// Subsystems
 import frc.robot.subsystem.Accumulateur;
 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 {
-  
+  private final SendableChooser<Command> autoChooser;
   Drive drive = new Drive();
   Accumulateur accumulateur = new Accumulateur();
   Balayeuse balayeuse = new Balayeuse();
   Grimpeur grimpeur = new Grimpeur();
   Guideur guideur = new Guideur();
   Lanceur lanceur = new Lanceur();
-
+  Balayer balayer = new Balayer(balayeuse, accumulateur);
+  GuiderHaut guiderHaut = new GuiderHaut(guideur);
+  GuiderBas guiderBas = new GuiderBas(guideur);
+  Lancer lancer = new Lancer(lanceur);
+  LancerNote lancernote = new LancerNote(lanceur, accumulateur);
+  Lancerampli lancerampli = new Lancerampli(lanceur);
    CommandJoystick joystick = new CommandJoystick(0);
    CommandXboxController manette = new CommandXboxController(1);
-  
+   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().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() {
-
-    
-
+  
   }
 
-  public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+  public Command getAutonomousCommand(){
+    return autoChooser.getSelected();
   }
 }

src/main/java/frc/robot/command/AllumeLED.java

@@ -5,42 +5,39 @@
 package frc.robot.command;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystem.Grimpeur;
+import frc.robot.subsystem.Accumulateur;
+import frc.robot.subsystem.LED;
 
-public class GrimpeurBas extends Command {
-  private Grimpeur grimpeur;
-  /** Creates a new GrimpeurBas. */
-  public GrimpeurBas(Grimpeur grimpeur) {
-    this.grimpeur = grimpeur;
-    addRequirements(grimpeur);
+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() {
-    grimpeur.resetencodeurd();
-    grimpeur.resetencodeurg();
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(grimpeur.droite()){
-      grimpeur.resetencodeurd();
-      grimpeur.gauche(0);
-    }
-    if(grimpeur.gauche()){
-      grimpeur.resetencodeurg();
-      grimpeur.gauche(0);
-    }
+   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) {
-    grimpeur.droit(0);
-    grimpeur.gauche(0);
+    led.couleur(255, 0, 0);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/command/Balayer.java

@@ -14,6 +14,7 @@ public class Balayer extends Command {
   /** Creates a new Balayer. */
   public Balayer(Balayeuse balayeuse, Accumulateur accumulateur) {
     this.balayeuse = balayeuse; 
+    this.accumulateur = accumulateur;
     addRequirements(balayeuse, accumulateur);}
     // Use addRequirements() here to declare subsystem dependencies.
   // Called when the command is initially scheduled.
@@ -25,11 +26,11 @@ public class Balayer extends Command {
   public void execute() {
     if(accumulateur.limitswitch()){
     balayeuse.balaye(0);
-    accumulateur.Accumuler(0);
+    accumulateur.Accumuler();
   }
   else{
     balayeuse.balaye(0.6);
-    accumulateur.Accumuler(0.6);
+    accumulateur.Accumuler();
   }
 }
 

src/main/java/frc/robot/command/GrimpeurDroit.java

@@ -4,13 +4,17 @@
 
 package frc.robot.command;
 
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystem.Grimpeur;
 
-public class GrimpeurHautDroit extends Command {
+public class GrimpeurDroit extends Command {
+  private DoubleSupplier doubleSupplier;
   private Grimpeur grimpeur;
   /** Creates a new GrimpeurHaut. */
-  public GrimpeurHautDroit(Grimpeur grimpeur) {
+  public GrimpeurDroit(Grimpeur grimpeur, DoubleSupplier doubleSupplier) {
+    this.doubleSupplier = doubleSupplier;
     this.grimpeur = grimpeur;
     addRequirements(grimpeur);
     // Use addRequirements() here to declare subsystem dependencies.
@@ -20,26 +24,32 @@ public class GrimpeurHautDroit extends Command {
   @Override
   public void initialize() {
     grimpeur.resetencodeurd();
-    grimpeur.resetencodeurg();
+    grimpeur.pistonferme();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    
-  if(grimpeur.encoderd()>261 ){
+    grimpeur.droit(doubleSupplier.getAsDouble());
+  if(grimpeur.encoderd()>261){
     grimpeur.droit(0);
+    
   }
   else if(grimpeur.getpitch()<-15){
-    grimpeur.droit(-0.6);
+    grimpeur.droit(-doubleSupplier.getAsDouble());
     
   }
   else if(grimpeur.getpitch()>15){
-    grimpeur.droit(0.6);
+    grimpeur.droit(doubleSupplier.getAsDouble());
   }
   else{
     grimpeur.droit(0);
   }
+  if(grimpeur.encoderd()>0){
+    grimpeur.resetencodeurd();
+    grimpeur.droit(0);
+    
+  }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/command/GrimpeurGauche.java

@@ -4,13 +4,17 @@
 
 package frc.robot.command;
 
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystem.Grimpeur;
 
-public class GrimpeurHautGauche extends Command {
+public class GrimpeurGauche extends Command {
+  private DoubleSupplier doubleSupplier;
   private Grimpeur grimpeur;
   /** Creates a new GrimpeurHautGauche. */
-  public GrimpeurHautGauche(Grimpeur grimpeur) {
+  public GrimpeurGauche(Grimpeur grimpeur,DoubleSupplier doubleSupplier) {
+    this.doubleSupplier = doubleSupplier;
     this.grimpeur = grimpeur;
     addRequirements(grimpeur);
     // Use addRequirements() here to declare subsystem dependencies.
@@ -26,19 +30,25 @@ public class GrimpeurHautGauche extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(grimpeur.encoderg()>261 ){
+    grimpeur.gauche(doubleSupplier.getAsDouble());
+    if(grimpeur.encoderg()>261){
       grimpeur.gauche(0);
     }
     else if(grimpeur.getpitch()<-15){
-      grimpeur.gauche(0.6);
+      grimpeur.gauche(doubleSupplier.getAsDouble());
       
     }
     else if(grimpeur.getpitch()>15){
-      grimpeur.gauche(-0.6);
+      grimpeur.gauche(-doubleSupplier.getAsDouble());
     }
     else{
       grimpeur.gauche(0);
     }
+    if(grimpeur.encoderd()>0){
+      grimpeur.resetencodeurg();
+      grimpeur.gauche(0);
+      
+    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/command/GuiderHaut.java

@@ -10,7 +10,7 @@ import frc.robot.subsystem.Guideur;
 public class GuiderHaut extends Command {
   private Guideur guideur;
   /** Creates a new GuiderHaut. */
-  public GuiderHaut() {
+  public GuiderHaut(Guideur guideur) {
     this.guideur = guideur;
     addRequirements(guideur);
     // Use addRequirements() here to declare subsystem dependencies.

src/main/java/frc/robot/command/Lancer.java

@@ -4,22 +4,19 @@
 
 package frc.robot.command;
 
-import com.revrobotics.CANSparkMax;
+
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystem.Accumulateur;
+
 import frc.robot.subsystem.Lanceur;
 
 public class Lancer extends Command {
   /** Creates a new Lanceur. */
-
-  private Lanceur lancer;
-  private Lanceur lancer2;
-  private Lanceur lancer3;
-  private Lanceur lancer4;
-  public Lancer() {
+  private Lanceur lanceur;
+  public Lancer(Lanceur lanceur) {
+    this.lanceur = lanceur;
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(lancer,lancer2,lancer3,lancer4);
+    addRequirements(lanceur);
   }
 
   // Called when the command is initially scheduled.
@@ -29,13 +26,13 @@ public class Lancer extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    lancer.lancer(0.3);
+    lanceur.lancerspeaker();
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    lancer.lancer(0);
+    lanceur.lancer(0);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/command/LancerNote.java

@@ -0,0 +1,44 @@
+// 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.Lanceur;
+
+public class LancerNote extends Command {
+  private Lanceur lancer;
+  private Accumulateur accumulateur;
+  /** Creates a new LancerNote. */
+  public LancerNote(Lanceur lancer, Accumulateur accumulateur) {
+    this.lancer = lancer;
+    this.accumulateur = accumulateur;
+    // 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() {
+    double vitesse = 100;
+    lancer.lancerspeaker();
+    if(lancer.vitesse(vitesse)>vitesse){
+      accumulateur.Accumuler(0.6);
+    }
+  }
+
+  // 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/command/Lancerampli.java

@@ -0,0 +1,45 @@
+// 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.Lanceur;
+
+public class Lancerampli extends Command {
+  /** Creates a new Lanceur. */
+
+  private Lanceur lanceur;
+  
+  public Lancerampli(Lanceur lanceur) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(lanceur);
+    this.lanceur = lanceur;
+  }
+
+  // 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() {
+    lanceur.lanceramp();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    lanceur.lancer(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/Accumulateur.java

@@ -6,17 +6,22 @@ package frc.robot.subsystem;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 
 public class Accumulateur extends SubsystemBase {
   /** Creates a new Accumulateur. */
  
-  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard"); 
+  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
+   private GenericEntry vitesse =
+      dashboard.add("vitesseacc", 1)
+         .getEntry(); 
   final WPI_TalonSRX Moteuracc2 = new WPI_TalonSRX(Constants.Moteuracc2);
   final WPI_TalonSRX Moteuracc = new WPI_TalonSRX(Constants.Moteuracc);
   final DigitalInput limitacc = new DigitalInput(Constants.limitacc);
@@ -32,12 +37,16 @@ public class Accumulateur extends SubsystemBase {
     return limitacc.get();
   }
   public Accumulateur() {
+    
     dashboard.addBoolean("limitacc", this::limitswitch);
   }
   public void Accumuler(double vitesse){
     Moteuracc.set(vitesse);
     Moteuracc2.set(vitesse);
   }
+  public void Accumuler(){
+    Accumuler(vitesse.getDouble(0.1));
+  }
 
   @Override
   public void periodic() {

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

@@ -4,7 +4,7 @@
 
 package frc.robot.subsystem;
 
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;

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

@@ -6,8 +6,15 @@ package frc.robot.subsystem;
 
 import java.io.File;
 import java.io.IOException;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import swervelib.SwerveDrive;
@@ -25,7 +32,17 @@ public class Drive extends SubsystemBase {
   }
   
   public Drive() {
-
+    AutoBuilder.configureHolonomic(swerveDrive::getPose, swerveDrive::resetOdometry, swerveDrive::getRobotVelocity, swerveDrive::setChassisSpeeds, new HolonomicPathFollowerConfig(
+      new PIDConstants(5,0,0), new PIDConstants(5,0,0), 4.5,
+      0.275,
+      new ReplanningConfig()
+      ), ()->{
+        var alliance = DriverStation.getAlliance();
+        if(alliance.isPresent()){
+          return alliance.get() == DriverStation.Alliance.Red;
+        }
+        return false;
+         }, this);
      try {
       this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2);
     } catch (IOException e) {

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

@@ -16,7 +16,7 @@ public class Guideur extends SubsystemBase {
   
     final WPI_TalonSRX guideur = new WPI_TalonSRX(Constants.guideur);
     final DigitalInput guideurhaut = new DigitalInput(Constants.guideurhaut);
-    final DigitalInput guideurbas = new DigitalInput(Constants.guideurhaut);
+    final DigitalInput guideurbas = new DigitalInput(Constants.guideurbas);
 
       public void guider(double vitesse){
         guideur.set(vitesse);

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/Lanceur.java

@@ -7,29 +7,49 @@ package frc.robot.subsystem;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
-
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class Lanceur extends SubsystemBase {
   /** Creates a new Lanceur. */
-  
-
-  public Lanceur() {}
-  final CANSparkMax lancer = new CANSparkMax(Constants.lanceur, MotorType.kBrushless);
-  final CANSparkMax lancer2 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless);
-  final CANSparkMax lancer3 = new CANSparkMax(Constants.lancer3, MotorType.kBrushless);
-  final CANSparkMax lancer4 = new CANSparkMax(Constants.lancer4, MotorType.kBrushless);
-
+  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
+   private GenericEntry vitesse =
+      dashboard.add("vitesse", 1)
+         .getEntry();
+         private GenericEntry vitesseamp =
+            dashboard.add("vitesseamp", 1)
+               .getEntry();
+  public Lanceur() {
+    
+  }
+  final CANSparkMax lanceur1 = new CANSparkMax(Constants.lanceur, MotorType.kBrushless);
+  final CANSparkMax lanceur2 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless);
+  final CANSparkMax lanceur3 = new CANSparkMax(Constants.lancer3, MotorType.kBrushless);
+  final CANSparkMax lanceur4 = new CANSparkMax(Constants.lancer4, MotorType.kBrushless);
+  public void pid(){
+    lanceur1.getPIDController();
+  }
   public void lancer(double vitesse){
-    lancer.set(vitesse);
+    lanceur1.set(vitesse);
+  }
+  public void lancerspeaker(){
+    lancer(vitesse.getDouble(0.1));
+  }
+  public void lanceramp(){
+    lancer(vitesseamp.getDouble(0.5));
+  }
+  public double vitesse(double vitesse){
+    return lanceur1.getEncoder().getVelocity();
   }
     public void lanceur(){
-    lancer2.follow(lancer);
-    lancer3.follow(lancer);
-    lancer4.follow(lancer);
-    lancer3.setInverted(true);
-    lancer4.setInverted(true);
+    lanceur2.follow(lanceur1);
+    lanceur3.follow(lanceur1);
+    lanceur4.follow(lanceur1);
+    lanceur3.setInverted(true);
+    lanceur4.setInverted(true);
   }
 
   @Override

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