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