From 042b63df572ee07dd0f56336c649744af3e9907d Mon Sep 17 00:00:00 2001
From: Zach3292 <lachancethierry6@gmail.com>
Date: Wed, 17 Jan 2024 20:00:28 -0500
Subject: [PATCH] save

Co-Authored-By: ScootKampfer <125150185+scootkampfer@users.noreply.github.com>
Co-Authored-By: Nahuelpoet <125915501+nahuelpoet@users.noreply.github.com>
---
 src/main/java/frc/robot/RobotContainer.java   |  2 +
 .../commands/GetShooterToSpeedCommand.java    | 38 ++++++++++++++
 .../java/frc/robot/subsystems/Shooter.java    | 51 +++++++++++++++----
 3 files changed, 82 insertions(+), 9 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/GetShooterToSpeedCommand.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a7ac5fe..f77f02a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.ActivateDrivetrainCommand;
 import frc.robot.commands.ActivateMecanumCommand;
+import frc.robot.commands.AvancerXmCommand;
 import frc.robot.commands.ClimberInABoxCommand;
 import frc.robot.commands.OctocanumDrivetrainCommand;
 import frc.robot.commands.ResetGryoCommand;
@@ -64,6 +65,7 @@ private void configureBindings() {
     xboxController.x().onTrue(new ResetGryoCommand(drivetrain));
     xboxController1.rightBumper().onTrue(new ShooterActivateCommand(shooter));
     xboxController1.rightBumper().onFalse(new ShooterDisactivateCommand(shooter));
+    xboxController1.a().onTrue(new AvancerXmCommand(drivetrain, driveTrainSwitch, 1));
   }
 
   /**
diff --git a/src/main/java/frc/robot/commands/GetShooterToSpeedCommand.java b/src/main/java/frc/robot/commands/GetShooterToSpeedCommand.java
new file mode 100644
index 0000000..2ca4c5c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/GetShooterToSpeedCommand.java
@@ -0,0 +1,38 @@
+// 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.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import frc.robot.subsystems.Shooter;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class GetShooterToSpeedCommand extends PIDCommand {
+  /** Creates a new GetShooterToSpeedCommand. */
+  public Shooter shooter;
+  public GetShooterToSpeedCommand(Shooter shooter, double targetRPM) {
+    super(
+        // The controller that the command will use
+        new PIDController(0, 0, 0),
+        // This should return the measurement
+        () -> shooter.getRPM(),
+        // This should return the setpoint (can also be a constant)
+        () -> targetRPM,
+        // This uses the output
+        output -> {
+          shooter.shoot(output);// Use the output here
+        });
+    this.shooter = shooter;
+    addRequirements(shooter);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index eec9867..21ba0a1 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -4,30 +4,63 @@
 
 package frc.robot.subsystems;
 
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj.motorcontrol.Talon;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.OperatorConstants;
 
 public class Shooter extends SubsystemBase {
   /** Creates a new Shooter. */
-  private Talon shooterGauche;
-
-  private Talon shooterDroit;
-
+  private CANSparkMax shooterGauche;
+  private CANSparkMax shooterDroit;
+  private RelativeEncoder shooterDroitEncoder;
+  private RelativeEncoder shooterGaucheEncoder;
   public Shooter() {
     OperatorConstants deviceNumber = new OperatorConstants();
-    shooterGauche = new Talon(deviceNumber.DeviceNumberShooterGauche);
-    shooterDroit = new Talon(deviceNumber.DeviceNumberShooterDroit);
+    shooterGauche = new CANSparkMax(deviceNumber.DeviceNumberShooterGauche, MotorType.kBrushless);
+    shooterDroit = new CANSparkMax(deviceNumber.DeviceNumberShooterDroit, MotorType.kBrushless);
 
     shooterGauche.setInverted(false);
     shooterDroit.setInverted(false);
+
+
   }
 
-  public void shoot(double activated) {
-    shooterGauche.set(activated);
-    shooterDroit.set(activated);
+  public void shoot(double power) {
+    shooterGauche.set(power);
+    shooterDroit.set(power);
   }
+  public double[] getEncoder() {
 
+    double[] encoderValue = {
+      shooterGaucheEncoder.getPosition(),
+      shooterDroitEncoder.getPosition()
+     
+    };
+    return encoderValue;
+  }
+  public double getRPM(){
+    double startposition = shooterGaucheEncoder.getPosition();
+    while(shooterGaucheEncoder.getPosition()< startposition+0.058 || shooterGaucheEncoder.getPosition() > startposition+0.058){
+
+      /* TODO: Regle cela (manque de temps)
+      
+      i = i + 1
+      wait(10)
+      i * 10 * 100 * 60
+      
+      */
+
+
+    }
+
+    //TODO find a way to get speed with rpm
+    return 1.0;
+  }
   @Override
   public void periodic() {