From 926d8299fda617361edbe853ee7054383df81a13 Mon Sep 17 00:00:00 2001
From: Thierry Lachance <101730754+Thierry-Lachance@users.noreply.github.com>
Date: Tue, 6 Feb 2024 23:26:11 +0000
Subject: [PATCH] update code to find find the encoder value based on the
 calculated angle

---
 .../java/frc/robot/commands/SetShooterAngleCommand.java     | 6 +++---
 src/main/java/frc/robot/subsystems/AngleShooter.java        | 5 ++++-
 src/main/java/frc/robot/subsystems/Limelight.java           | 2 +-
 3 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/commands/SetShooterAngleCommand.java b/src/main/java/frc/robot/commands/SetShooterAngleCommand.java
index 941c37b..3c40b0b 100644
--- a/src/main/java/frc/robot/commands/SetShooterAngleCommand.java
+++ b/src/main/java/frc/robot/commands/SetShooterAngleCommand.java
@@ -23,13 +23,13 @@ public class SetShooterAngleCommand extends PIDCommand {
   public SetShooterAngleCommand(AngleShooter angleShooter, Limelight limelight) {
     super(
         // The controller that the command will use
-        new PIDController(0, 0, 0),
+        new PIDController(0, 0, 0),//TODO change value
         // This should return the measurement
         () -> angleShooter.getEncoderValue(),
         // This should return the setpoint (can also be a constant)
         () ->
-            ((limelight.calculateShooterAngle()
-                * 8.57)), // TODO adjust 8.57 based on gearbox and encoder
+            ((limelight.calculateShooterAngle()*(angleShooter.getEncoderMax()-angleShooter.getEncoderMin()))/30)+angleShooter.getEncoderMin()//un produit  croiser pour calculer les valeur d'encoder basee sur l'angle calculer
+                ,
         // This uses the output
         output -> {
           angleShooter.setPower(output);
diff --git a/src/main/java/frc/robot/subsystems/AngleShooter.java b/src/main/java/frc/robot/subsystems/AngleShooter.java
index dc994f9..557f41a 100644
--- a/src/main/java/frc/robot/subsystems/AngleShooter.java
+++ b/src/main/java/frc/robot/subsystems/AngleShooter.java
@@ -29,10 +29,13 @@ public AngleShooter() {
 
   public double getEncoderValue() {
 
-    return shooterAngleEncoder.getAbsolutePosition() - encoderMin;
+    return shooterAngleEncoder.getAbsolutePosition();
   }
 public double getEncoderMax(){
   return encoderMax;
+}
+public double getEncoderMin(){
+  return encoderMin;
 }
   public void setPower(double power) {
     shooterAngle.set(power);
diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
index 2e1184c..aa0ee71 100644
--- a/src/main/java/frc/robot/subsystems/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -187,7 +187,7 @@ public double calculateShooterAngle() {
       if (ty.getDouble(0) < 1234
           && ty.getDouble(0) > -1234
           && tx.getDouble(0) <= 90) { // TODO change range
-        return Math.atan(62.6 / tx.getDouble(0));
+        return Math.atan(62.6 / tx.getDouble(0));//TODO check if relative to the field or camera
       } else {
         return 30;
       }