From 77b894194e825b545c99e0f4f77758c28add4674 Mon Sep 17 00:00:00 2001
From: samuel desharnais <samdes330@gmail.com>
Date: Mon, 13 Feb 2023 19:51:56 -0500
Subject: [PATCH] gjnvb

---
 .../robot/commands/bras/PivoteBrasBas.java    | 35 +++++++++++++++++--
 .../robot/commands/bras/PivoteBrasHaut.java   | 31 ++++++++++++++--
 .../commands/bras/PivoteBrasMilieux.java      | 32 +++++++++++++++--
 3 files changed, 92 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java
index 03a1793..de9ddfb 100644
--- a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java
+++ b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java
@@ -4,12 +4,22 @@
 
 package frc.robot.commands.bras;
 
+
+
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.bras.BrasTelescopique;
+import frc.robot.subsystems.bras.Pivot;
 
 public class PivoteBrasBas extends CommandBase {
+  private BrasTelescopique brasTelescopique;
+  private Pivot pivot;
   /** Creates a new PivoteBrasBas. */
-  public PivoteBrasBas() {
+  public PivoteBrasBas(BrasTelescopique brasTelescopique, Pivot pivot) {
+    this.brasTelescopique = brasTelescopique;
+    this.pivot = pivot;
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(brasTelescopique);
+    addRequirements(pivot);
   }
 
   // Called when the command is initially scheduled.
@@ -18,7 +28,28 @@ public class PivoteBrasBas extends CommandBase {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if(brasTelescopique.distance()<10){
+      brasTelescopique.AvanceRecule(0.5);
+    }
+  else if(brasTelescopique.distance()>11) {
+    brasTelescopique.AvanceRecule(-0.5);
+    }
+    else {
+      brasTelescopique.AvanceRecule(0);
+    }
+    if (pivot.distance()<10){
+      pivot.monteDescendre(0.5);
+    }
+    else if(pivot.distance()>11) {
+      pivot.monteDescendre(-0.5);
+    }
+    else{
+      pivot.monteDescendre(0);
+    }
+    }
+  
+  
 
   // Called once the command ends or is interrupted.
   @Override
diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java b/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java
index 4932ad2..2ba3c74 100644
--- a/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java
+++ b/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java
@@ -5,11 +5,19 @@
 package frc.robot.commands.bras;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.bras.BrasTelescopique;
+import frc.robot.subsystems.bras.Pivot;
 
 public class PivoteBrasHaut extends CommandBase {
+  private BrasTelescopique brasTelescopique;
+  private Pivot pivot;
   /** Creates a new PivoteBrasHaut. */
-  public PivoteBrasHaut() {
+  public PivoteBrasHaut(BrasTelescopique brasTelescopique, Pivot pivot) {
+    this.brasTelescopique = brasTelescopique;
+    this.pivot = pivot;
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(brasTelescopique);
+    addRequirements(pivot);
   }
 
   // Called when the command is initially scheduled.
@@ -18,7 +26,26 @@ public class PivoteBrasHaut extends CommandBase {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if(brasTelescopique.distance()<10){
+      brasTelescopique.AvanceRecule(0.5);
+    }
+  else if(brasTelescopique.distance()>11) {
+    brasTelescopique.AvanceRecule(-0.5);
+    }
+    else {
+      brasTelescopique.AvanceRecule(0);
+    }
+    if (pivot.distance()<10){
+      pivot.monteDescendre(0.5);
+    }
+    else if(pivot.distance()>11) {
+      pivot.monteDescendre(-0.5);
+    }
+    else{
+      pivot.monteDescendre(0);
+    }
+    }
 
   // Called once the command ends or is interrupted.
   @Override
diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java
index be7e44e..da8939b 100644
--- a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java
+++ b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java
@@ -5,11 +5,19 @@
 package frc.robot.commands.bras;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.bras.BrasTelescopique;
+import frc.robot.subsystems.bras.Pivot;
 
 public class PivoteBrasMilieux extends CommandBase {
+  private BrasTelescopique brasTelescopique;
+  private Pivot pivot;
   /** Creates a new PivoteBrasMilieux. */
-  public PivoteBrasMilieux() {
+  public PivoteBrasMilieux(BrasTelescopique brasTelescopique, Pivot pivot) {
+    this.brasTelescopique = brasTelescopique;
+    this.pivot = pivot;
     // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(brasTelescopique);
+    addRequirements(pivot);
   }
 
   // Called when the command is initially scheduled.
@@ -18,7 +26,27 @@ public class PivoteBrasMilieux extends CommandBase {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if(brasTelescopique.distance()<10){
+      brasTelescopique.AvanceRecule(0.5);
+    }
+  else if(brasTelescopique.distance()>11) {
+    brasTelescopique.AvanceRecule(-0.5);
+    }
+    else {
+      brasTelescopique.AvanceRecule(0);
+    }
+    if (pivot.distance()<10){
+      pivot.monteDescendre(0.5);
+    }
+    else if(pivot.distance()>11) {
+      pivot.monteDescendre(-0.5);
+    }
+    else{
+      pivot.monteDescendre(0);
+    }
+    }
+  
 
   // Called once the command ends or is interrupted.
   @Override