Pull code

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeBlueAutoBaseRangee3Uniquement.java

@@ -0,0 +1,327 @@
+package org.firstinspires.ftc.teamcode.pedroPathing.Auto.nonfinalise;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
+import com.pedropathing.util.Timer;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AfficheurLeft;
+import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AfficheurRight;
+import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AngleShooter;
+import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Indexeur;
+import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Intake;
+import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.ServoTireur;
+import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Shooter;
+import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.SpinTurret;
+import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManager;
+
+@Disabled
+@Autonomous (name="bleuCoteBaseRangée3uniquement", group="EnCours")
+public class DecodeBlueAutoBaseRangee3Uniquement extends OpMode {
+
+    private Follower follower;
+    private Timer pathTimer, opModeTimer;
+    //private ElapsedTime pathTimer = new ElapsedTime();
+    //private ElapsedTime opModeTimer = new ElapsedTime();
+    private Shooter shooter;
+    private SpinTurret tourelle;
+    private AngleShooter ServoAngleShoot;
+    private ServoTireur servoTireur;
+    private Indexeur indexeur;
+    private Intake intake;
+
+    private AfficheurLeft afficheurLeft;
+    private AfficheurRight afficheurRight;
+
+    private TireurManager tireurManager;
+    private boolean turretZeroDone = false;
+    private long imuReadySince = 0;
+
+    private boolean shotsTriggered = false;
+
+    public enum PathState {
+        //Start Position -End Position
+        //drive > Movement
+        //Shoot >Attempts to Score the Artifact
+        DRIVE_STARTPOSITIONTOSHOOT,
+        align_RANGEE1Blue,
+        align_rangee2blue,
+        align_rangee3blue,
+        intakeballeRangee1,
+        intakerange2,
+        intakerangee3,
+        PremierTir,
+        DrivedeuxiemeShoot,
+        deuxiemetir,
+        DriveTroisiemeTir,
+        troisiemetir,
+        retourzerotourelle,
+        Drive2Gate,
+        atgate
+    }
+    PathState pathState;
+
+    private final Pose startPose = new Pose(56.00,8.00, Math.toRadians(180));
+    private final Pose firstshootPose = new Pose(56.00,18.00,Math.toRadians(180));
+
+    private final Pose drivetoligne3= new Pose (45.00, 35.00, Math.toRadians(180));
+
+    private final Pose avalerballeRangee3 = new Pose (17.00, 35.00, Math.toRadians(180));
+
+    private final Pose drivetoballesJH= new Pose (8.50, 26.00, Math.toRadians(-90));
+
+    private final Pose avalerballeJH= new Pose (8.50, 8.50, Math.toRadians(-90));
+
+    //private final Pose Gate= new Pose (20, 83, Math.toRadians(180));
+
+    private PathChain driveStartofirstShootPos, driveShoot2pickup1Pos, driveAvalerpremiereLigne, DrivedeuxiemeShoot,drivetorangee2, drivetavalerdeuxiemeligne,driveAvaler2emeLignetotroisemeShoot, drivetroisiemeshoot,DrivetoGate;
+
+    public void buildPaths() {
+        //put the coordinate from start to shooting
+        driveStartofirstShootPos = follower.pathBuilder()
+                .addPath(new BezierLine(startPose, firstshootPose))
+                .setLinearHeadingInterpolation(startPose.getHeading(), firstshootPose.getHeading())
+                .build();
+
+        //du premiershoot à la rangée numéro 1
+        driveShoot2pickup1Pos = follower.pathBuilder()
+                .addPath(new BezierLine(firstshootPose, drivetoligne3))
+                .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoligne3.getHeading())
+                .build();
+
+        //de la l'alignement pickup1 à la derniere balle rangée 1
+        driveAvalerpremiereLigne = follower.pathBuilder()
+                .addPath(new BezierLine(drivetoligne3,avalerballeRangee3))
+                .setLinearHeadingInterpolation(drivetoligne3.getHeading(), avalerballeRangee3.getHeading())
+                .setVelocityConstraint(0.23)
+                .build();
+        //Aller à la zone de Tir apres avoir avaler les balles de la rangée 1
+        DrivedeuxiemeShoot = follower.pathBuilder()
+                .addPath(new BezierLine(avalerballeRangee3,firstshootPose))
+                .setLinearHeadingInterpolation(avalerballeRangee3.getHeading(), firstshootPose.getHeading())
+                .build();
+        //Aller s'aligner à la deuxieme rangée de balle
+        drivetorangee2 = follower.pathBuilder()
+                .addPath(new BezierLine(firstshootPose, drivetoballesJH))
+                .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoballesJH.getHeading())
+                .build();
+
+        //Aller avaler les balles de la rangee 2
+        drivetavalerdeuxiemeligne = follower.pathBuilder()
+                .addPath(new BezierLine(drivetoballesJH, avalerballeJH))
+                .setLinearHeadingInterpolation(drivetoballesJH.getHeading(), avalerballeJH.getHeading())
+                .setVelocityConstraint(0.23)
+                .build();
+
+        //Aller à la zone de Tir apres avoir avaler les balles de la rangée 2
+        driveAvaler2emeLignetotroisemeShoot = follower.pathBuilder()
+                .addPath(new BezierLine(avalerballeJH,firstshootPose))
+                .setLinearHeadingInterpolation(avalerballeJH.getHeading(), firstshootPose.getHeading())
+                .build();
+
+        /*//Aller à la zone de Tir apres avoir avaler les balles de la rangée 2
+        drivetroisiemeshoot = follower.pathBuilder()
+                .addPath(new BezierLine(avalerballeRangee2,Shoot3))
+                .setLinearHeadingInterpolation(avalerballeRangee2.getHeading(), Shoot3.getHeading())
+                .build();*/
+        //Aller à la dernière position
+       /* DrivetoGate= follower.pathBuilder()
+                .addPath(new BezierLine(firstshootPose,Gate))
+                .setLinearHeadingInterpolation(firstshootPose.getHeading(),Gate.getHeading())
+                .build();*/
+
+
+    }
+    public void statePathUpdate(){
+        switch(pathState) {
+            case DRIVE_STARTPOSITIONTOSHOOT:
+                follower.followPath(driveStartofirstShootPos,0.8, true); //true will hold the positon
+                setPathState(PathState.PremierTir); // Reset Timer + make new staet
+                break;
+
+            case PremierTir: // Premier tir en cours
+                //intake.update();
+                //indexeur.update();
+                if (!follower.isBusy()) {
+                    // avons nous deja demandé des tirs :
+
+                    if (!shotsTriggered){
+                        tireurManager.startTirAuto(// Lancer tir automatique
+                                -63,   // angle tourelle (exemple)
+                                0.5,  // angle shooter
+                                4770   // RPM
+                        );
+                        shotsTriggered = true;}
+                    else if (shotsTriggered && !tireurManager.isBusy()){
+                            setPathState(PathState.align_RANGEE1Blue);
+                            shotsTriggered = false;
+                        }
+
+                }
+                break;
+
+
+            case align_RANGEE1Blue: // On va s'aliner avec la rangée 1
+                // check is follow done is path
+                intake.update();
+                indexeur.update();
+                //if (!follower.isBusy() && pathTimer.getElapsedTimeSeconds()>5) {
+                if (!follower.isBusy()) {
+                    telemetry.addLine("Done with Shooting 1, deplacement vers premiere rangée");
+                    // transition to next state
+                    follower.followPath(driveShoot2pickup1Pos ,0.8, true); // chemin d'alignement de la premiere rangée
+                    setPathState(PathState.intakeballeRangee1); // on va a l'étape suivante
+                }
+                break;
+
+            case intakeballeRangee1:
+                // mise à jour des systèmes
+                intake.update();
+                indexeur.update();
+
+                if (!follower.isBusy()) {// attendre que le path soit fini
+                    follower.followPath(driveAvalerpremiereLigne,0.45,true); // on avance doucement pour avaler les balles
+                    setPathState(PathState.DrivedeuxiemeShoot);
+                    }
+                break;
+
+            case DrivedeuxiemeShoot:
+                ;
+                if (!follower.isBusy()) { // Attendre que l'on est fini d'avoir pris toutes les balles
+                    follower.followPath(DrivedeuxiemeShoot,0.8,true);
+                    // Le robot est arrivé en position de tir :
+                    setPathState(PathState.deuxiemetir);
+                }
+                break;
+
+            case deuxiemetir:
+                if (!follower.isBusy()) {
+                    if (!shotsTriggered) { // deuxieme période de tir
+                        tireurManager.startTirAuto(// Lancer tir automatique
+                                -64,   // angle tourelle (exemple)
+                                0.50,  // angle shooter
+                                4770   // RPM
+                        );
+                        shotsTriggered = true;
+                    } else if (shotsTriggered && !tireurManager.isBusy()) {
+                        setPathState(PathState.retourzerotourelle);
+                        shotsTriggered = false;
+                    }
+                }
+                break;
+
+            case retourzerotourelle:
+                if (!follower.isBusy() && pathTimer.getElapsedTimeSeconds()>3) {
+                    tourelle.allerVersAngle(0);
+                    if (tourelle.isAtAngle(0)){
+                        setPathState(PathState.atgate);
+                        shotsTriggered = false;
+                    }
+
+                }
+                break;
+
+            case atgate:
+                telemetry.addLine("C'est fini");
+                break;
+        }
+
+    }
+
+    public void setPathState (PathState newState){
+        pathState = newState;
+        pathTimer.resetTimer();
+        shotsTriggered = false;
+
+    }
+    @Override
+    public void init() {
+        pathState = PathState.DRIVE_STARTPOSITIONTOSHOOT;
+        pathTimer = new Timer();
+        opModeTimer = new Timer();
+        opModeTimer.resetTimer();
+        tourelle = new SpinTurret();
+        tourelle.init(hardwareMap);
+
+        follower = Constants.createFollower(hardwareMap);
+        buildPaths();
+        follower.setPose(startPose);
+
+
+        // --- Initialisation hardware ---
+        shooter = new Shooter();
+        shooter.init(hardwareMap);
+
+
+
+        ServoAngleShoot = new AngleShooter();
+        ServoAngleShoot.init(hardwareMap);
+
+        indexeur = new Indexeur();
+        indexeur.init(hardwareMap);
+
+        afficheurLeft = new AfficheurLeft();
+        afficheurLeft.init(hardwareMap);
+
+        afficheurRight = new AfficheurRight();
+        afficheurRight.init(hardwareMap);
+
+        intake = new Intake(indexeur, afficheurLeft);
+        intake.init(hardwareMap);
+        indexeur.setIntake(intake);
+
+        servoTireur = new ServoTireur(indexeur);
+        servoTireur.init(hardwareMap);
+
+        // --- TireurManager ---
+        tireurManager = new TireurManager(shooter, tourelle, ServoAngleShoot, servoTireur, indexeur, intake, afficheurRight);
+
+        indexeur.setBalles(3);
+        indexeur.resetCompartiments();
+
+
+    }
+
+
+    public void start(){
+        opModeTimer.resetTimer();
+        setPathState(pathState);
+
+    }
+    @Override
+    public void loop (){
+        follower.update();
+        statePathUpdate();
+        intake.update();
+        indexeur.update();
+        tireurManager.update();
+
+        //telemetry.addData("path state", pathState.toString());
+        //telemetry.addData("x",follower.getPose().getX());
+        //telemetry.addData("y",follower.getPose().getY());
+        //telemetry.addData("heading",follower.getPose().getHeading());
+        //telemetry.addData("Path time", pathTimer.getElapsedTimeSeconds());
+        //telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle());
+        //telemetry.addData("RPM", intake.getRPM());
+        //telemetry.addData("DistanceBalle", intake.getCapteurDistance());
+        //telemetry.addData("Lum Indexeur", intake.getLumIndexeur());
+        //telemetry.addData("Score", intake.getScore());
+        //telemetry.addData("État Indexeur", indexeur.getEtat());
+        //telemetry.addData("Pale detectée", indexeur.detectionpale());
+        //telemetry.addData("Nombre de balles", indexeur.getBalles());
+        //for (int i = 0; i < 3; i++) { telemetry.addData("Compartiment " + i, indexeur.getCouleurCompartiment(i)); }
+        //telemetry.addData("État tireur manager", tireurManager.getState());
+        //telemetry.addData("État indexeur", indexeur.getEtat());
+        //telemetry.addData("Etat de l'intake", intake.getEtat());
+        //telemetry.addData("Shooter RPM", shooter.getShooterVelocityRPM());
+        //telemetry.addData("Servo pos", servoTireur.getPosition());
+        //telemetry.addData("Index rotation finie", indexeur.isRotationTerminee());
+
+        telemetry.update();
+    }
+}