diff --git a/FtcRobotController/src/main.lnk b/FtcRobotController/src/main.lnk new file mode 100644 index 000000000000..f34861a1c465 Binary files /dev/null and b/FtcRobotController/src/main.lnk differ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/Pedro.zip b/TeamCode/src/main/java/org/firstinspires/ftc/Pedro.zip new file mode 100644 index 000000000000..8069310ad9e4 Binary files /dev/null and b/TeamCode/src/main/java/org/firstinspires/ftc/Pedro.zip differ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeBlueAutoCoteBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeBlueAutoCoteBase.java new file mode 100644 index 000000000000..24e9eb81c5b5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeBlueAutoCoteBase.java @@ -0,0 +1,407 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Auto; + +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.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.TeleOp.TeleOpDecodeBleuCamera; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManager; +import org.firstinspires.ftc.teamcode.pedroPathing.navigation.GlobalPoseStorage; + + +@Autonomous (name="BLEU--BASE", group="Competition") +public class DecodeBlueAutoCoteBase 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 shotsTriggered = false; + private boolean turretZeroDone = false; + private long imuReadySince = 0; + + 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, + Drive2Gate, + atgate + } + + PathState pathState; + + private final Pose startPose = new Pose(46.00, 8.00, Math.toRadians(180)); + private final Pose firstshootPose = new Pose(44.00, 17.30, Math.toRadians(180)); + + private final Pose drivetoligne3 = new Pose(43.00, 33.70, Math.toRadians(180)); + + private final Pose avalerballeRangee3 = new Pose(7.50, 35.00, Math.toRadians(180)); + + private final Pose Shoot2 = new Pose(44.00, 23.00, Math.toRadians(180)); + + private final Pose drivetoligne2 = new Pose(43.00, 55.50, Math.toRadians(180)); + + private final Pose avalerballeRangee2 = new Pose(8.50, 59.50, Math.toRadians(180)); + private final Pose Shoot3 = new Pose(44.00, 77.50, Math.toRadians(180)); + + 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, Shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee3.getHeading(), Shoot2.getHeading()) + .build(); + //Aller s'aligner à la deuxieme rangée de balle + drivetorangee2 = follower.pathBuilder() + .addPath(new BezierLine(Shoot2, drivetoligne2)) + .setLinearHeadingInterpolation(Shoot2.getHeading(), drivetoligne2.getHeading()) + .build(); + + //Aller avaler les balles de la rangee 2 + drivetavalerdeuxiemeligne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne2, avalerballeRangee2)) + .setLinearHeadingInterpolation(drivetoligne2.getHeading(), avalerballeRangee2.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(avalerballeRangee2, Shoot3)) + .setLinearHeadingInterpolation(avalerballeRangee2.getHeading(), Shoot3.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(Shoot2, Gate)) + .setLinearHeadingInterpolation(Shoot2.getHeading(), Gate.getHeading()) + .build(); + + + } + + public void statePathUpdate() { + switch (pathState) { + case DRIVE_STARTPOSITIONTOSHOOT: + follower.followPath(driveStartofirstShootPos, 0.8, true); //true will hold the positon + tireurManager.prespinShooter(4000); + setPathState(PathState.PremierTir); // Reset Timer + make new staet + break; + + case PremierTir: // Premier tir en cours + //intake.update(); + //indexeur.update(); + if (!follower.isBusy() && pathTimer.getElapsedTimeSeconds() > 1) { + // avons nous deja demandé des tirs : + + if (!shotsTriggered) { + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + -67, // angle tourelle (exemple) + 0.58, // angle shooter + 4780 // 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, false); // 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.40, false); // 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 : + tireurManager.prespinShooter(4000); + setPathState(PathState.deuxiemetir); + } + break; + + case deuxiemetir: + if (!follower.isBusy() & pathTimer.getElapsedTimeSeconds() > 1.5) { + if (!shotsTriggered) { // deuxieme période de tir + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + -68, // angle tourelle (exemple) + 0.59, // angle shooter + 4780 // RPM + ); + shotsTriggered = true; + } else if (shotsTriggered && !tireurManager.isBusy()) { + setPathState(PathState.align_rangee2blue); + shotsTriggered = false; + } + } + break; + + case align_rangee2blue: // alignement avec la deuxieme zo + // ne de balle (centrale) + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + //if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>5) { + if (!follower.isBusy()) { + follower.followPath(drivetorangee2, 0.8, false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("alignement ramassage ligne 2"); + // transition to next state + setPathState(PathState.intakerange2); + } + break; + + case intakerange2: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(drivetavalerdeuxiemeligne, 0.40, false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("ramassage 2 terminé"); + // transition to next state + setPathState(PathState.DriveTroisiemeTir); + } + break; + + case DriveTroisiemeTir: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(driveAvaler2emeLignetotroisemeShoot, 0.65, true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + tireurManager.prespinShooter(3000); + telemetry.addLine("Position 3 de tir"); + // transition to next state + setPathState(PathState.troisiemetir); + } + break; + + case troisiemetir: + if (!follower.isBusy() & pathTimer.getElapsedTimeSeconds() > 1.5) { + // le robot est arrivé sur la troisieme position de tir : + + if (!shotsTriggered) { + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + -50, // angle tourelle (exemple) + 0.40, // angle shooter + 3900 // RPM + ); + shotsTriggered = true; + } else if (shotsTriggered && !tireurManager.isBusy()) { + setPathState(PathState.atgate); + shotsTriggered = false; + } + + } + break; + case Drive2Gate: + intake.update(); // mise à jour de nos systemes (constate les balles sont tirées ) + indexeur.update(); + // shoot logique 3eme Tir + if (!follower.isBusy()) { + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Auto Termine & A cote de la porte "); + // transition to next state + setPathState(PathState.atgate); + + } + break; + + case atgate: + tourelle.allerVersAngle(0); + 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(); + + + } + + public void stop() { + + TeleOpDecodeBleuCamera.startingPose = follower.getPose(); + GlobalPoseStorage.turretAngleDeg = tourelle.lectureangletourelle(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeBlueAutoGoal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeBlueAutoGoal.java new file mode 100644 index 000000000000..0472dfc2e921 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeBlueAutoGoal.java @@ -0,0 +1,434 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Auto; + +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.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.TeleOp.TeleOpDecode; +import org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.TeleOpDecodeBleuCamera; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManager; +import org.firstinspires.ftc.teamcode.pedroPathing.navigation.GlobalPoseStorage; + + +@Autonomous (name="BLEU--GOAL", group="Competition") +public class DecodeBlueAutoGoal 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 shotsTriggered = false; + private boolean turretZeroDone = false; + private long imuReadySince = 0; + + 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, + Drive2Gate, + Tourne1, + atgate + } + PathState pathState; + + private final Pose startPose = new Pose(21,125, Math.toRadians(144)); + private final Pose firstshootPose = new Pose(58,84,Math.toRadians(135)); + + private final Pose Tourne1= new Pose (62,89, Math.toRadians(180)); + private final Pose drivetoligne1= new Pose (45.50, 91.00, Math.toRadians(180)); + + private final Pose avalerballeRangee1 = new Pose (20.0, 91.80, Math.toRadians(180)); + + private final Pose Shoot2 = new Pose (51.80, 84.70, Math.toRadians(180)); + + private final Pose Shoot3 = new Pose (51.80, 84.70, Math.toRadians(180)); + + private final Pose drivetoligne2= new Pose (44.40, 73.00, Math.toRadians(180)); + + private final Pose avalerballeRangee2= new Pose (13.5, 66.00, Math.toRadians(180)); + + private final Pose Gate= new Pose (14, 70, Math.toRadians(-90)); + + private PathChain driveStartofirstShootPos,driveTourne1, driveShoot2pickup1Pos, driveAvalerpremiereLigne, DrivedeuxiemeShoot,drivetorangee2, drivetavalerdeuxiemeligne,driveAvaler2emeLignetotroisemeShoot,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(); + + // 1er tir a Virage 1 + driveTourne1 = follower.pathBuilder() + + .addPath(new BezierLine(firstshootPose,Tourne1)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), Tourne1.getHeading()) + .build(); + + + //du premiershoot à la rangée numéro 1 + driveShoot2pickup1Pos = follower.pathBuilder() + .addPath(new BezierLine(firstshootPose, drivetoligne1)) + .setLinearHeadingInterpolation(Tourne1.getHeading(), drivetoligne1.getHeading()) + .build(); + + //de la l'alignement pickup1 à la derniere balle rangée 1 + driveAvalerpremiereLigne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne1,avalerballeRangee1)) + .setLinearHeadingInterpolation(drivetoligne1.getHeading(), avalerballeRangee1.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(avalerballeRangee1,Shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee1.getHeading(), Shoot2.getHeading()) + .build(); + //Aller s'aligner à la deuxieme rangée de balle + drivetorangee2 = follower.pathBuilder() + .addPath(new BezierLine(Shoot2, drivetoligne2)) + .setLinearHeadingInterpolation(Shoot2.getHeading(), drivetoligne2.getHeading()) + .build(); + + //Aller avaler les balles de la rangee 2 + drivetavalerdeuxiemeligne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne2, avalerballeRangee2)) + .setLinearHeadingInterpolation(drivetoligne2.getHeading(), avalerballeRangee2.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(avalerballeRangee2,Shoot3)) + .setLinearHeadingInterpolation(avalerballeRangee2.getHeading(), Shoot3.getHeading()) + .build(); + + + //Aller de la zone du troisieme Tir à la troisieme rangée + DrivetoGate= follower.pathBuilder() + .addPath(new BezierLine(Shoot2,Gate)) + .setLinearHeadingInterpolation(Shoot2.getHeading(),Gate.getHeading()) + .build(); + + + } + public void statePathUpdate(){ + switch(pathState) { + case DRIVE_STARTPOSITIONTOSHOOT: + follower.followPath(driveStartofirstShootPos,0.70, true); //true will hold the positon + setPathState(PathState.PremierTir); // Reset Timer + make new staet + tireurManager.prespinShooter(4000); + break; + + + + case PremierTir: // Premier tir en cours + //intake.update(); + + //indexeur.update(); + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>1.0) { + // avons nous deja demandé des tirs : + + if (!shotsTriggered){ + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + -5, // angle tourelle (exemple) + 0.45, // angle shooter + 4000 // RPM + ); + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){ + setPathState(PathState.align_RANGEE1Blue); + shotsTriggered = false; + } + + } + break; + + + case Tourne1: // on tourne avant de rammaser les balles + intake.update(); + indexeur.update(); + if (!follower.isBusy()) { + //follower.turn(45,false); + //follower.turnDegrees(45,true); + //follower.followPath(driveTourne1 ,0.8, false); + telemetry.addLine("Done with Shooting 1, tourner vers premiere rangée"); + // transition to next state + // chemin d'alignement de la premiere rangée + setPathState(PathState.align_RANGEE1Blue); // on va a l'étape suivante + } + 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, false); // 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.40,false); // 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.60,true); + // Le robot est arrivé en position de tir : + setPathState(PathState.deuxiemetir); + tireurManager.prespinShooter(4000); + } + break; + + case deuxiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>1.5) { + if (!shotsTriggered) { // deuxieme période de tir + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + -8, // angle tourelle (exemple) + 0.48, // angle shooter + 4080 // RP + // M + ); + shotsTriggered = true; + } else if (shotsTriggered && !tireurManager.isBusy()) { + setPathState(PathState.align_rangee2blue); + shotsTriggered = false; + } + } + break; + + case align_rangee2blue: // alignement avec la deuxieme zo + // ne de balle (centrale) + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + //if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>5) { + if (!follower.isBusy()) { + follower.followPath(drivetorangee2,0.7, false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("alignement ramassage ligne 2"); + // transition to next state + setPathState(PathState.intakerange2); + } + break; + + case intakerange2: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(drivetavalerdeuxiemeligne, 0.38 , false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("ramassage 2 terminé"); + // transition to next state + setPathState(PathState.DriveTroisiemeTir); + } + break; + + + case DriveTroisiemeTir: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(driveAvaler2emeLignetotroisemeShoot,0.60, true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Position 3 de tir"); + // transition to next state + setPathState(PathState.troisiemetir); + tireurManager.prespinShooter(4000); + } + break; + + case troisiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>1.5) { + TeleOpDecode.startingPose = follower.getPose(); + // le robot est arrivé sur la troisieme position de tir : + + if (!shotsTriggered){ + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + -12, // angle tourelle (exemple) + 0.48, // angle shooter + 4080 // RPM + ); + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){; + setPathState(PathState.atgate); + shotsTriggered = false; + } + + + + } + break; + case Drive2Gate: // pas utiliser sur la partie Goal + intake.update(); // mise à jour de nos systemes (constate les balles sont tirées ) + indexeur.update(); + // shoot logique 3eme Tir + if (!follower.isBusy()) { + follower.followPath(DrivetoGate,1,true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Auto Termine & A cote de la porte "); + // transition to next state + setPathState(PathState.atgate); + } + break; + + + case atgate: + tourelle.allerVersAngle(35);; + + telemetry.addLine("C'est fini, position enregitrée"); + 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(); + + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setPose(startPose); + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + // --- 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(); + } + + public void stop() { + + TeleOpDecodeBleuCamera.startingPose = follower.getPose(); + GlobalPoseStorage.turretAngleDeg = tourelle.lectureangletourelle(); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeRedAutoCoteBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeRedAutoCoteBase.java new file mode 100644 index 000000000000..be9efa58db20 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeRedAutoCoteBase.java @@ -0,0 +1,391 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Auto; + +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.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; + + +@Autonomous (name="RED--Base", group="Competition") +public class DecodeRedAutoCoteBase 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 boolean turretZeroDone = false; + private long imuReadySince = 0; + private AngleShooter ServoAngleShoot; + private ServoTireur servoTireur; + private Indexeur indexeur; + private Intake intake; + + private AfficheurLeft afficheurLeft; + private AfficheurRight afficheurRight; + + private TireurManager tireurManager; + + 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, + Drive2Gate, + atgate + } + PathState pathState; + + private final Pose startPose = new Pose(90,8.00, Math.toRadians(0)); + private final Pose firstshootPose = new Pose(92,17.00,Math.toRadians(0)); + + private final Pose drivetoligne3= new Pose (100.00, 32.00, Math.toRadians(0)); + + private final Pose avalerballeRangee3 = new Pose (130.00, 35.00, Math.toRadians(0)); + + private final Pose drivetoligne2= new Pose (101.20, 52.90, Math.toRadians(0)); + + private final Pose avalerballeRangee2= new Pose (125.10, 56.90, Math.toRadians(0)); + private final Pose Shoot2 = new Pose (94.00, 78.00, Math.toRadians(0)); + private final Pose avalerballeRangee1 = new Pose (103.00, 104.00, Math.toRadians(0)); + + // private final Pose Gate= new Pose (20, 83, Math.toRadians(0)); + + 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, drivetoligne2)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoligne2.getHeading()) + .build(); + + //Aller avaler les balles de la rangee 2 + drivetavalerdeuxiemeligne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne2, avalerballeRangee2)) + .setLinearHeadingInterpolation(drivetoligne2.getHeading(), avalerballeRangee2.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(avalerballeRangee2,Shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee2.getHeading(), Shoot2.getHeading()) + .build(); + + //Aller à la zone de Tir apres avoir avaler les balles de la rangée 2 + drivetroisiemeshoot = follower.pathBuilder() + .addPath(new BezierLine(Shoot2,avalerballeRangee1)) + .setLinearHeadingInterpolation(Shoot2.getHeading(), avalerballeRangee1.getHeading()) + .build(); + //Aller à la dernière position + /*DrivetoGate= follower.pathBuilder() + .addPath(new BezierLine(Shoot2,Gate)) + .setLinearHeadingInterpolation(Shoot2.getHeading(),Gate.getHeading()) + .build();*/ + + + } + public void statePathUpdate(){ + switch(pathState) { + case DRIVE_STARTPOSITIONTOSHOOT: + follower.followPath(driveStartofirstShootPos,0.8, true); //true will hold the positon + tireurManager.prespinShooter(4000); + setPathState(PathState.PremierTir); // Reset Timer + make new staet + break; + + case PremierTir: // Premier tir en cours + //intake.update(); + //indexeur.update(); + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.7) { + // avons nous deja demandé des tirs : + + if (!shotsTriggered){ + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + 67.0, // angle tourelle (exemple) + 0.59, // angle shooter + 4780 // 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.67,true); + tireurManager.prespinShooter(4000); + // Le robot est arrivé en position de tir : + setPathState(PathState.deuxiemetir); + + } + break; + + case deuxiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.7) { + if (!shotsTriggered) { // deuxieme période de tir + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + 67, // angle tourelle (exemple) + 0.59, // angle shooter + 4780 // RPM + ); + shotsTriggered = true; + } else if (shotsTriggered && !tireurManager.isBusy()) { + setPathState(PathState.align_rangee2blue); + shotsTriggered = false; + } + } + break; + + case align_rangee2blue: // alignement avec la deuxieme zo + // ne de balle (centrale) + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + //if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>5) { + if (!follower.isBusy()) { + follower.followPath(drivetorangee2,0.8, true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("alignement ramassage ligne 2"); + // transition to next state + setPathState(PathState.intakerange2); + } + break; + + case intakerange2: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(drivetavalerdeuxiemeligne, 0.35 , false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("ramassage 2 terminé"); + // transition to next state + setPathState(PathState.DriveTroisiemeTir); + } + break; + + case DriveTroisiemeTir: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(driveAvaler2emeLignetotroisemeShoot,0.9, true); + tireurManager.prespinShooter(4000); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Position 3 de tir"); + // transition to next state + setPathState(PathState.troisiemetir); + } + break; + + case troisiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>1.5) { + // le robot est arrivé sur la troisieme position de tir : + + if (!shotsTriggered){ + tireurManager.stopPrespin(); + + tireurManager.startTirAuto(// Lancer tir automatique + 52, // angle tourelle (exemple) + 0.37, // angle shooter + 3960 // RPM + ); + + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){ + setPathState(PathState.Drive2Gate); + shotsTriggered = false;} + + } + break; + case Drive2Gate: + intake.update(); // mise à jour de nos systemes (constate les balles sont tirées ) + indexeur.update(); + // shoot logique 3eme Tir + if (!follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + setPathState(PathState.atgate); + } + break; + + case atgate: + tourelle.allerVersAngle(0); + 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(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeRedAutoGoal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeRedAutoGoal.java new file mode 100644 index 000000000000..4c1758c3b5f8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeRedAutoGoal.java @@ -0,0 +1,426 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Auto; + +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.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.TeleOp.TeleOpDecode; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManager; + + +@Autonomous (name="Red--GOAL", group="Competition") +public class DecodeRedAutoGoal 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 shotsTriggered = false; + private boolean turretZeroDone = false; + private long imuReadySince = 0; + + 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, + Drive2Gate, + Tourne1, + atgate + } + PathState pathState; + + private final Pose startPose = new Pose(115,125, Math.toRadians(37)); + private final Pose firstshootPose = new Pose(80,84,Math.toRadians(44)); + + private final Pose Tourne1= new Pose (81,89, Math.toRadians(0)); + private final Pose drivetoligne1= new Pose (85.50, 89.00, Math.toRadians(0)); + + private final Pose avalerballeRangee1 = new Pose (106.50, 89.00, Math.toRadians(0)); + + private final Pose Shoot2 = new Pose (72.20, 84.00, Math.toRadians(0)); + + private final Pose Shoot3 = new Pose (72.0, 84.00, Math.toRadians(0)); + + private final Pose drivetoligne2= new Pose (80.90, 67.80, Math.toRadians(0)); + + private final Pose avalerballeRangee2= new Pose (109.50, 62.00, Math.toRadians(0)); + + private final Pose Gate= new Pose (14, 70, Math.toRadians(-90)); + + private PathChain driveStartofirstShootPos,driveTourne1, driveShoot2pickup1Pos, driveAvalerpremiereLigne, DrivedeuxiemeShoot,drivetorangee2, drivetavalerdeuxiemeligne,driveAvaler2emeLignetotroisemeShoot,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(); + + // 1er tir a Virage 1 + driveTourne1 = follower.pathBuilder() + + .addPath(new BezierLine(firstshootPose,drivetoligne1)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoligne1.getHeading()) + .build(); + + + //du premiershoot à la rangée numéro 1 + driveShoot2pickup1Pos = follower.pathBuilder() + .addPath(new BezierLine(firstshootPose, drivetoligne1)) + .setLinearHeadingInterpolation(Tourne1.getHeading(), drivetoligne1.getHeading()) + .build(); + + //de la l'alignement pickup1 à la derniere balle rangée 1 + driveAvalerpremiereLigne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne1,avalerballeRangee1)) + .setLinearHeadingInterpolation(drivetoligne1.getHeading(), avalerballeRangee1.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(avalerballeRangee1,Shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee1.getHeading(), Shoot2.getHeading()) + .build(); + //Aller s'aligner à la deuxieme rangée de balle + drivetorangee2 = follower.pathBuilder() + .addPath(new BezierLine(Shoot2, drivetoligne2)) + .setLinearHeadingInterpolation(Shoot2.getHeading(), drivetoligne2.getHeading()) + .build(); + + //Aller avaler les balles de la rangee 2 + drivetavalerdeuxiemeligne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne2, avalerballeRangee2)) + .setLinearHeadingInterpolation(drivetoligne2.getHeading(), avalerballeRangee2.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(avalerballeRangee2,Shoot3)) + .setLinearHeadingInterpolation(avalerballeRangee2.getHeading(), Shoot3.getHeading()) + .build(); + + + //Aller de la zone du troisieme Tir à la troisieme rangée + DrivetoGate= follower.pathBuilder() + .addPath(new BezierLine(Shoot2,Gate)) + .setLinearHeadingInterpolation(Shoot2.getHeading(),Gate.getHeading()) + .build(); + + + } + public void statePathUpdate(){ + switch(pathState) { + case DRIVE_STARTPOSITIONTOSHOOT: + follower.followPath(driveStartofirstShootPos,0.70, true); //true will hold the positon + tireurManager.prespinShooter(3000); + setPathState(PathState.PremierTir); // Reset Timer + make new staet + + + break; + + case PremierTir: // Premier tir en cours + //intake.update(); + + //indexeur.update(); + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.7) { + // avons nous deja demandé des tirs : + + if (!shotsTriggered){ + tireurManager.stopPrespin(); + + tireurManager.startTirAuto(// Lancer tir automatique + 2, // angle tourelle (exemple) + 0.45, // angle shooter + 4000 // RPM + ); + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){ + setPathState(PathState.Tourne1); + shotsTriggered = false; + } + + } + break; + + + case Tourne1: // on tourne avant de rammaser les balles + if (!follower.isBusy()){ + follower.followPath(driveTourne1 ,0.7, false); + //follower.turn(Math.toRadians(-45),false); + telemetry.addLine("Done with Shooting 1, tourner vers premiere rangée"); + // transition to next state + // chemin d'alignement de la premiere rangée + setPathState(PathState.align_RANGEE1Blue);} // on va a l'étape suivante + 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.7, false); // 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.38,false); // 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.60,true); + // Le robot est arrivé en position de tir : + tireurManager.prespinShooter(3000); + setPathState(PathState.deuxiemetir); + } + break; + + case deuxiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>1.5 + ) { + if (!shotsTriggered) { // deuxieme période de tir + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + 3, // angle tourelle (exemple) + 0.50, // angle shooter + 4090 // RP + // M + ); + shotsTriggered = true; + } else if (shotsTriggered && !tireurManager.isBusy()) { + setPathState(PathState.align_rangee2blue); + shotsTriggered = false; + } + } + break; + + case align_rangee2blue: // alignement avec la deuxieme zo + // ne de balle (centrale) + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + //if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>5) { + if (!follower.isBusy()) { + follower.followPath(drivetorangee2,0.7, false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("alignement ramassage ligne 2"); + // transition to next state + setPathState(PathState.intakerange2); + } + break; + + case intakerange2: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(drivetavalerdeuxiemeligne, 0.36 , false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("ramassage 2 terminé"); + // transition to next state + setPathState(PathState.DriveTroisiemeTir); + } + break; + + + case DriveTroisiemeTir: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + tireurManager.prespinShooter(3000); + follower.followPath(driveAvaler2emeLignetotroisemeShoot,0.60, true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Position 3 de tir"); + // transition to next state + setPathState(PathState.troisiemetir); + } + break; + + case troisiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>1.5) { + TeleOpDecode.startingPose = follower.getPose(); + // le robot est arrivé sur la troisieme position de tir : + + if (!shotsTriggered){ + tireurManager.stopPrespin(); + tireurManager.startTirAuto(// Lancer tir automatique + 4, // angle tourelle (exemple) + 0.50, // angle shooter + 4080 // RPM + ); + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){ + setPathState(PathState.atgate); + shotsTriggered = false; + } + + + + } + break; + case Drive2Gate: // pas utiliser sur la partie Goal + intake.update(); // mise à jour de nos systemes (constate les balles sont tirées ) + indexeur.update(); + + // shoot logique 3eme Tir + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.5) { + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Auto Termine & A cote de la porte "); + // transition to next state + setPathState(PathState.atgate); + + } + break; + + case atgate: + tourelle.allerVersAngle(-35);; + telemetry.addLine("C'est fini, position enregitrée"); + 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(); + + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setPose(startPose); + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + // --- 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(); + } + + public void stop() { + TeleOpDecode.startingPose = follower.getPose(); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeBlueAutoBaseRangee3Uniquement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeBlueAutoBaseRangee3Uniquement.java new file mode 100644 index 000000000000..8ca6f983ca61 --- /dev/null +++ b/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(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeBlueAutoGoalAncien.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeBlueAutoGoalAncien.java new file mode 100644 index 000000000000..080c8a806a25 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeBlueAutoGoalAncien.java @@ -0,0 +1,396 @@ +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.TeleOp.TeleOpDecode; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManager; +@Disabled +@Autonomous (name="bleuGoal", group="Encours") +public class DecodeBlueAutoGoalAncien 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 shotsTriggered = false; + private boolean turretZeroDone = false; + private long imuReadySince = 0; + + 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, + Drive2Gate, + atgate + } + PathState pathState; + + private final Pose startPose = new Pose(33.9416569,135.598599, Math.toRadians(180)); + private final Pose firstshootPose = new Pose(42.00700116,103.0011668,Math.toRadians(180)); + + private final Pose drivetoligne1= new Pose (45.50, 88.00, Math.toRadians(180)); + + private final Pose avalerballeRangee1 = new Pose (22.50, 85.00, Math.toRadians(180)); + + + private final Pose Shoot2 = new Pose (54.60, 80.00, Math.toRadians(180)); + + private final Pose drivetoligne2= new Pose (46.00, 65.00, Math.toRadians(180)); + + private final Pose avalerballeRangee2= new Pose (21.00, 63.00, Math.toRadians(180)); + + private final Pose Gate= new Pose (14, 70, Math.toRadians(-90)); + + private PathChain driveStartofirstShootPos, driveShoot2pickup1Pos, driveAvalerpremiereLigne, DrivedeuxiemeShoot,drivetorangee2, drivetavalerdeuxiemeligne,driveAvaler2emeLignetotroisemeShoot,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, drivetoligne1)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoligne1.getHeading()) + .build(); + + //de la l'alignement pickup1 à la derniere balle rangée 1 + driveAvalerpremiereLigne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne1,avalerballeRangee1)) + .setLinearHeadingInterpolation(drivetoligne1.getHeading(), avalerballeRangee1.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(avalerballeRangee1,Shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee1.getHeading(), Shoot2.getHeading()) + .build(); + //Aller s'aligner à la deuxieme rangée de balle + drivetorangee2 = follower.pathBuilder() + .addPath(new BezierLine(Shoot2, drivetoligne2)) + .setLinearHeadingInterpolation(Shoot2.getHeading(), drivetoligne2.getHeading()) + .build(); + + //Aller avaler les balles de la rangee 2 + drivetavalerdeuxiemeligne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne2, avalerballeRangee2)) + .setLinearHeadingInterpolation(drivetoligne2.getHeading(), avalerballeRangee2.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(avalerballeRangee2,Shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee2.getHeading(), Shoot2.getHeading()) + .build(); + + + //Aller de la zone du troisieme Tir à la troisieme rangée + DrivetoGate= follower.pathBuilder() + .addPath(new BezierLine(Shoot2,Gate)) + .setLinearHeadingInterpolation(Shoot2.getHeading(),Gate.getHeading()) + .build(); + + + } + public void statePathUpdate(){ + switch(pathState) { + case DRIVE_STARTPOSITIONTOSHOOT: + follower.followPath(driveStartofirstShootPos,1, 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()&& pathTimer.getElapsedTimeSeconds()>0.5) { + // avons nous deja demandé des tirs : + + if (!shotsTriggered){ + tireurManager.startTirAuto(// Lancer tir automatique + -44, // angle tourelle (exemple) + 0.33, // angle shooter + 3900 // 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.9, false); // 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.38,false); // 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,1,true); + // Le robot est arrivé en position de tir : + setPathState(PathState.deuxiemetir); + } + break; + + case deuxiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.5) { + if (!shotsTriggered) { // deuxieme période de tir + tireurManager.startTirAuto(// Lancer tir automatique + -49, // angle tourelle (exemple) + 0.40, // angle shooter + 4000 // RPM + ); + shotsTriggered = true; + } else if (shotsTriggered && !tireurManager.isBusy()) { + setPathState(PathState.align_rangee2blue); + shotsTriggered = false; + } + } + break; + + case align_rangee2blue: // alignement avec la deuxieme zo + // ne de balle (centrale) + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + //if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>5) { + if (!follower.isBusy()) { + follower.followPath(drivetorangee2,0.9, false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("alignement ramassage ligne 2"); + // transition to next state + setPathState(PathState.intakerange2); + } + break; + + case intakerange2: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(drivetavalerdeuxiemeligne, 0.285 , false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("ramassage 2 terminé"); + // transition to next state + setPathState(PathState.DriveTroisiemeTir); + } + break; + + + case DriveTroisiemeTir: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(driveAvaler2emeLignetotroisemeShoot,0.8, true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Position 3 de tir"); + // transition to next state + setPathState(PathState.troisiemetir); + } + break; + + case troisiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.5) { + TeleOpDecode.startingPose = follower.getPose(); + // le robot est arrivé sur la troisieme position de tir : + + if (!shotsTriggered){ + + tireurManager.startTirAuto(// Lancer tir automatique + -49, // angle tourelle (exemple) + 0.37, // angle shooter + 4020 // RPM + ); + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){ + + tourelle.allerVersAngle(0); + setPathState(PathState.atgate); + shotsTriggered = false; + } + + + + } + break; + case Drive2Gate: // pas utiliser sur la partie Goal + intake.update(); // mise à jour de nos systemes (constate les balles sont tirées ) + indexeur.update(); + // shoot logique 3eme Tir + if (!follower.isBusy()) { + follower.followPath(DrivetoGate,1,true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Auto Termine & A cote de la porte "); + // transition to next state + setPathState(PathState.atgate); + + } + break; + + case atgate: + telemetry.addLine("C'est fini, position enregitrée"); + 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(); + + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setPose(startPose); + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + // --- 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(); + } + + public void stop() { + TeleOpDecode.startingPose = follower.getPose(); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedAutoCoteBaseV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedAutoCoteBaseV2.java new file mode 100644 index 000000000000..eeb22cd963e4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedAutoCoteBaseV2.java @@ -0,0 +1,374 @@ +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="RedCoteBase V2", group="enCours") +public class DecodeRedAutoCoteBaseV2 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 boolean turretZeroDone = false; + private long imuReadySince = 0; + private Intake intake; + + private AfficheurLeft afficheurLeft; + private AfficheurRight afficheurRight; + + private TireurManager tireurManager; + + 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, + Drive2Gate, + atgate + } + PathState pathState; + + private final Pose startPose = new Pose(90,8.00, Math.toRadians(0)); + private final Pose firstshootPose = new Pose(92,17.00,Math.toRadians(0)); + + private final Pose drivetoligne3= new Pose (100.00, 31.00, Math.toRadians(0)); + + private final Pose avalerballeRangee3 = new Pose (130.00, 35.00, Math.toRadians(0)); + + private final Pose drivetoligne2= new Pose (100.00, 51.00, Math.toRadians(0)); + + private final Pose avalerballeRangee2= new Pose (123.00, 58.10, Math.toRadians(0)); + private final Pose Shoot2 = new Pose (95.00, 78.00, Math.toRadians(0)); + private final Pose avalerballeRangee1 = new Pose (103.00, 104.00, Math.toRadians(0)); + + // private final Pose Gate= new Pose (20, 83, Math.toRadians(0)); + + private PathChain driveStartofirstShootPos, driveShoot2pickup1Pos, driveAvalerpremiereLigne, DrivedeuxiemeShoot,drivetorangee2, drivetavalerdeuxiemeligne,driveAvaler2emeLignetotroisemeShoot, drivetroisiemeshoot,DrivetoGate; + + public void buildPaths() { + //put the coordinate from start to shooting + //du premiershoot à la rangée numéro 1 + driveShoot2pickup1Pos = follower.pathBuilder() + .addPath(new BezierLine(startPose, drivetoligne3)) + .setLinearHeadingInterpolation(startPose.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, drivetoligne2)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoligne2.getHeading()) + .build(); + + //Aller avaler les balles de la rangee 2 + drivetavalerdeuxiemeligne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne2, avalerballeRangee2)) + .setLinearHeadingInterpolation(drivetoligne2.getHeading(), avalerballeRangee2.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(avalerballeRangee2,Shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee2.getHeading(), Shoot2.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()&& pathTimer.getElapsedTimeSeconds()>0.5) { + // avons nous deja demandé des tirs : + + if (!shotsTriggered){ + tireurManager.startTirAuto(// Lancer tir automatique + 69, // angle tourelle (exemple) + 0.60 + , // angle shooter + 4930 // 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, false); // 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.48,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.72,true); + // Le robot est arrivé en position de tir : + setPathState(PathState.deuxiemetir); + } + break; + + case deuxiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.5) { + + if (!shotsTriggered) { // deuxieme période de tir + tireurManager.startTirAuto(// Lancer tir automatique + 72, // angle tourelle (exemple) + 0.62, // angle shooter + 4830 // RPM + ); + shotsTriggered = true; + } else if (shotsTriggered && !tireurManager.isBusy()) { + setPathState(PathState.align_rangee2blue); + shotsTriggered = false; + } + } + + break; + + case align_rangee2blue: // alignement avec la deuxieme zo + // ne de balle (centrale) + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + //if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>5) { + if (!follower.isBusy()) { + follower.followPath(drivetorangee2,0.9, false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("alignement ramassage ligne 2"); + // transition to next state + setPathState(PathState.intakerange2); + } + break; + + case intakerange2: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(drivetavalerdeuxiemeligne, 0.36 , false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("ramassage 2 terminé"); + // transition to next state + setPathState(PathState.DriveTroisiemeTir); + } + break; + + case DriveTroisiemeTir: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(driveAvaler2emeLignetotroisemeShoot,0.9, true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Position 3 de tir"); + // transition to next state + setPathState(PathState.troisiemetir); + } + break; + + case troisiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.5) { + // le robot est arrivé sur la troisieme position de tir : + + if (!shotsTriggered){ + tireurManager.startTirAuto(// Lancer tir automatique + 52, // angle tourelle (exemple) + 0.45, // angle shooter + 3980// RPM + ); + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){ + setPathState(PathState.Drive2Gate); + shotsTriggered = false; + } + + } + break; + case Drive2Gate: + intake.update(); // mise à jour de nos systemes (constate les balles sont tirées ) + indexeur.update(); + // fin retour tourelle + if (!follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + tourelle.allerVersAngle(0); + if (tourelle.isAtAngle(0)){ + shotsTriggered = false; + setPathState(PathState.atgate); + } + + } + 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(); + + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setPose(startPose); + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + // --- 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(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedAutoGoalAncien.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedAutoGoalAncien.java new file mode 100644 index 000000000000..e7527ce733c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedAutoGoalAncien.java @@ -0,0 +1,396 @@ +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.TeleOp.TeleOpDecode; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManager; + +@Disabled +@Autonomous (name="RedGoal", group="EnCours") +public class DecodeRedAutoGoalAncien 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 shotsTriggered = false; + private boolean turretZeroDone = false; + private long imuReadySince = 0; + + 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, + Drive2Gate, + retourzerotourelle, + atgate + } + PathState pathState; + + private final Pose startPose = new Pose(110.50,135.50, Math.toRadians(0)); + private final Pose firstshootPose = new Pose(102.00,104.50,Math.toRadians(0)); + + private final Pose shoot2 = new Pose(107.00,106,Math.toRadians(0)); + + + private final Pose drivetoligne1= new Pose (101.00, 87.50, Math.toRadians(0)); + + + private final Pose avalerballeRangee1 = new Pose (122.80, 84, Math.toRadians(0)); + + private final Pose drivetoligne2= new Pose (102.10, 70.80, Math.toRadians(0)); + + private final Pose avalerballeRangee2= new Pose (119.00, 59.50, Math.toRadians(0)); + + //private final Pose Gate= new Pose (14, 70, Math.toRadians(-90)); + + private PathChain driveStartofirstShootPos, driveShoot2pickup1Pos, driveAvalerpremiereLigne, DrivedeuxiemeShoot,drivetorangee2, drivetavalerdeuxiemeligne,driveAvaler2emeLignetotroisemeShoot,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, drivetoligne1)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoligne1.getHeading()) + .build(); + + //de la l'alignement pickup1 à la derniere balle rangée 1 + driveAvalerpremiereLigne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne1,avalerballeRangee1)) + .setLinearHeadingInterpolation(drivetoligne1.getHeading(), avalerballeRangee1.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(avalerballeRangee1,shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee1.getHeading(), firstshootPose.getHeading()) + .build(); + //Aller s'aligner à la deuxieme rangée de balle + drivetorangee2 = follower.pathBuilder() + .addPath(new BezierLine(shoot2, drivetoligne2)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoligne2.getHeading()) + .build(); + + //Aller avaler les balles de la rangee 2 + drivetavalerdeuxiemeligne = follower.pathBuilder() + .addPath(new BezierLine(drivetoligne2, avalerballeRangee2)) + .setLinearHeadingInterpolation(drivetoligne2.getHeading(), avalerballeRangee2.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(avalerballeRangee2,shoot2)) + .setLinearHeadingInterpolation(avalerballeRangee2.getHeading(), firstshootPose.getHeading()) + .build(); + + + //Aller de la zone du troisieme Tir à la troisieme rangée + /*DrivetoGate= follower.pathBuilder() + .addPath(new BezierLine(Shoot2,Gate)) + .setLinearHeadingInterpolation(Shoot2.getHeading(),Gate.getHeading()) + .build();*/ + + + } + public void statePathUpdate(){ + switch(pathState) { + case DRIVE_STARTPOSITIONTOSHOOT: + follower.followPath(driveStartofirstShootPos,1, 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 + 44, // angle tourelle (exemple) + 0.35, // angle shooter + 3900 // 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.9, 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.385,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,1,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 + 45, // angle tourelle (exemple) + 0.35, // angle shooter + 3900 // RPM + ); + shotsTriggered = true; + } else if (shotsTriggered && !tireurManager.isBusy()) { + setPathState(PathState.align_rangee2blue); + shotsTriggered = false; + } + } + break; + + case align_rangee2blue: // alignement avec la deuxieme zo + // ne de balle (centrale) + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + //if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>5) { + if (!follower.isBusy()) { + follower.followPath(drivetorangee2,0.9, false); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("alignement ramassage ligne 2"); + // transition to next state + setPathState(PathState.intakerange2); + } + + break; + + case intakerange2: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(drivetavalerdeuxiemeligne, 0.33 , true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("ramassage 2 terminé"); + // transition to next state + setPathState(PathState.DriveTroisiemeTir); + } + break; + + + case DriveTroisiemeTir: + intake.update(); // mise à jour de nos systemes (constate que toutes les balles sont parties) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(driveAvaler2emeLignetotroisemeShoot,0.9, true); + // TO DO demarer intake , tourner indexeur des dectetion balles) + telemetry.addLine("Position 3 de tir"); + // transition to next state + setPathState(PathState.troisiemetir); + } + break; + + case troisiemetir: + if (!follower.isBusy()) { + TeleOpDecode.startingPose = follower.getPose(); + // le robot est arrivé sur la troisieme position de tir : + + if (!shotsTriggered){ + + tireurManager.startTirAuto(// Lancer tir automatique + 47, // angle tourelle (exemple) + 0.35, // angle shooter + 3850 // RPM + ); + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){ + + setPathState(PathState.retourzerotourelle); + shotsTriggered = false; + } + + } + break; + + case retourzerotourelle: + if (!follower.isBusy() && pathTimer.getElapsedTimeSeconds()>1) { + tourelle.allerVersAngle(0); + if (tourelle.isAtAngle(0)) { + setPathState(PathState.atgate); + shotsTriggered = false; + } + } + break; + + case atgate: + telemetry.addLine("C'est fini, position enregitrée"); + 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(); + } + + public void stop() { + TeleOpDecode.startingPose = follower.getPose(); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedBaseRangee3Uniquement.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedBaseRangee3Uniquement.java new file mode 100644 index 000000000000..7db26b793bae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/nonfinalise/DecodeRedBaseRangee3Uniquement.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="RedCoteBaseUneRangee3", group="Encours") +public class DecodeRedBaseRangee3Uniquement 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 boolean turretZeroDone = false; + private long imuReadySince = 0; + + private TireurManager tireurManager; + + 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(90,8.00, Math.toRadians(0)); + private final Pose firstshootPose = new Pose(92,17.00,Math.toRadians(0)); + + private final Pose drivetoligne3= new Pose (100.00, 32.00, Math.toRadians(0)); + + private final Pose avalerballeRangee3 = new Pose (130.00, 35.00, Math.toRadians(0)); + + // test ramassage frontal + private final Pose drivetoballeJH= new Pose (40, 10, Math.toRadians(0)); + private final Pose avalerballeJH= new Pose (29, 10, Math.toRadians(0)); + + private final Pose drivetoligne2 = new Pose (104.00, 60.00, Math.toRadians(0)); + + // private final Pose Gate= new Pose (20, 83, Math.toRadians(0)); + + 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, drivetoballeJH)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoballeJH.getHeading()) + .build(); + + //Aller avaler les balles de la rangee 2 + drivetavalerdeuxiemeligne = follower.pathBuilder() + .addPath(new BezierLine(drivetoballeJH, avalerballeJH)) + .setLinearHeadingInterpolation(drivetoballeJH.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(firstshootPose,drivetoligne2)) + .setLinearHeadingInterpolation(firstshootPose.getHeading(), drivetoligne2.getHeading()) + .build(); + //Aller à la dernière position + /*DrivetoGate= follower.pathBuilder() + .addPath(new BezierLine(Shoot2,Gate)) + .setLinearHeadingInterpolation(Shoot2.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()&& pathTimer.getElapsedTimeSeconds()>0.5) { + // avons nous deja demandé des tirs : + + if (!shotsTriggered){ + tireurManager.startTirAuto(// Lancer tir automatique + 70, // angle tourelle (exemple) + 0.62, // angle shooter + 4780 // 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.65,true); + // Le robot est arrivé en position de tir : + setPathState(PathState.deuxiemetir); + } + break; + + case deuxiemetir: + if (!follower.isBusy()&& pathTimer.getElapsedTimeSeconds()>0.5) { + if (!shotsTriggered) { // deuxieme période de tir + tireurManager.startTirAuto(// Lancer tir automatique + 70, // angle tourelle (exemple) + 0.62, // angle shooter + 4780 // 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(); + + follower = Constants.createFollower(hardwareMap); + buildPaths(); + follower.setPose(startPose); + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + // --- 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(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index fa30420ac7cd..b70dadc8276f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -1,19 +1,70 @@ package org.firstinspires.ftc.teamcode.pedroPathing; +import com.pedropathing.control.FilteredPIDFCoefficients; +import com.pedropathing.control.PIDFCoefficients; import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; import com.pedropathing.paths.PathConstraints; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(17.2) + .forwardZeroPowerAcceleration(-30.241270019550853) // old-30.2711203755 + .lateralZeroPowerAcceleration(-66.794486541851) // old -64.5169691986 + .translationalPIDFCoefficients(new PIDFCoefficients(0.06,0,0.01,0.03)) //old (0.1,0,0.01,0.02)) + .headingPIDFCoefficients(new PIDFCoefficients(1.4,0,0.02,0.02)) //old (1.0,0,0.03,0.03)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.025,0.0,0.00001,0.6,0.1)) // old : 1.0,0.0,0.001,0.6,0.25 + .centripetalScaling(0.0005); + //.useSecondaryTranslationalPIDF(true) + //.useSecondaryHeadingPIDF(true) + //.useSecondaryDrivePIDF(true) + + + + public static MecanumConstants driveConstants = new MecanumConstants() + .maxPower(0.9) + + .xVelocity(62.35182346133736) // old 51.015311501 + .yVelocity(46.20394344780389) // old 38.870022270) + .rightFrontMotorName("MotorFrontRight") + .rightRearMotorName("MotorBackRight") + .leftRearMotorName("MotorBackLeft") + .leftFrontMotorName("MotorFrontLeft") + .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) + .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD); - public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + public static PathConstraints pathConstraints = new PathConstraints(0.99, + 100, + 0.5, // old value 1 + 1); + + + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(6.614) + .strafePodX(2.913) + .distanceUnit(DistanceUnit.INCH) + .hardwareMapName("odo") + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED); public static Follower createFollower(HardwareMap hardwareMap) { return new FollowerBuilder(followerConstants, hardwareMap) + .pinpointLocalizer(localizerConstants) .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) + .build(); } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AfficheurLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AfficheurLeft.java new file mode 100644 index 000000000000..b37eb7607e4f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AfficheurLeft.java @@ -0,0 +1,126 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; + +import androidx.annotation.NonNull; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +public class AfficheurLeft { + + private Servo RGBLeft; + private ElapsedTime timer = new ElapsedTime(); + + // Positions des couleurs + private final double vert = 0.500; + private final double bleu = 0.611; + private final double violet = 0.722; + private final double blanc = 1.0; + private final double jaune = 0.388; + private final double rouge = 0.277; + private final double orange = 0.333; + + // États possibles + private enum Etat { + IDLE, + Vert, + Bleu, + Jaune, + Rouge, + Orange, + Violet, + Blanc, + ClignoteOrange, + } + + private Etat etat = Etat.IDLE; + + public void init(@NonNull HardwareMap hwMap) { + RGBLeft = hwMap.get(Servo.class, "RGBLeft"); + } + + public void update() { + switch (etat) { + case IDLE: + RGBLeft(0.0); + break; + case Vert: + RGBLeft(vert); + break; + case Bleu: + RGBLeft(bleu); + break; + case Jaune: + RGBLeft(jaune); + break; + case Rouge: + RGBLeft(rouge); + break; + case Orange: + RGBLeft(orange); + break; + case Violet: + RGBLeft(violet); + break; + case Blanc: + RGBLeft(blanc); + break; + case ClignoteOrange: + if (timer.milliseconds() > 300) { + if (RGBLeft.getPosition() == orange) + setPosition(0.0); + else + setPosition(orange); + timer.reset(); + } + break; + } + } + + // Méthodes publiques pour changer la couleur + public void setIdle() { + etat = Etat.IDLE; + } + + public void setVert() { + etat = Etat.Vert; + } + + public void setBleu() { + etat = Etat.Bleu; + } + + public void setJaune() { + etat = Etat.Jaune; + } + + public void setRouge() { + etat = Etat.Rouge; + } + + public void setOrange() { + etat = Etat.Orange; + } + + public void setViolet() { + etat = Etat.Violet; + } + + public void setBlanc() { + etat = Etat.Blanc; + } + + private void RGBLeft(double pos) { + RGBLeft.setPosition(pos); + } + + //Mode clignotant pour l’intake + public void setClignoteOrange() { + etat = Etat.ClignoteOrange; + timer.reset(); + } + + private void setPosition(double pos) { + RGBLeft.setPosition(pos); + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AfficheurRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AfficheurRight.java new file mode 100644 index 000000000000..2bfc36540fd1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AfficheurRight.java @@ -0,0 +1,89 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; + +import androidx.annotation.NonNull; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +public class AfficheurRight { + + private Servo RGBRight; + private ElapsedTime timer = new ElapsedTime(); + + // Positions des couleurs + private final double vert = 0.500; + private final double bleu = 0.611; + private final double violet = 0.722; + private final double blanc = 1.0; + private final double jaune = 0.388; + private final double rouge = 0.277; + private final double orange = 0.333; + + // États possibles + private enum Etat { + IDLE, + Vert, + Bleu, + Jaune, + Rouge, + Orange, + Violet, + Blanc, + ClignoteVert + } + + private Etat etat = Etat.IDLE; + + public void init(@NonNull HardwareMap hwMap) { + RGBRight = hwMap.get(Servo.class, "RGBRight"); + timer.reset(); + } + + public void update() { + switch (etat) { + + case IDLE: + setPosition(0.0); + break; + + case Vert: setPosition(vert); break; + case Bleu: setPosition(bleu); break; + case Jaune: setPosition(jaune); break; + case Rouge: setPosition(rouge); break; + case Orange: setPosition(orange); break; + case Violet: setPosition(violet); break; + case Blanc: setPosition(blanc); break; + + case ClignoteVert: + if (timer.milliseconds() > 300) { + if (RGBRight.getPosition() == vert) + setPosition(0.0); + else + setPosition(vert); + + timer.reset(); + } + break; + } + } + + // Méthodes publiques pour changer la couleur + public void setIdle() { etat = Etat.IDLE; } + public void setVert() { etat = Etat.Vert; } + public void setBleu() { etat = Etat.Bleu; } + public void setJaune() { etat = Etat.Jaune; } + public void setRouge() { etat = Etat.Rouge; } + public void setOrange(){ etat = Etat.Orange; } + public void setViolet(){ etat = Etat.Violet; } + public void setBlanc() { etat = Etat.Blanc; } + + // Mode clignotant vert pour le tir + public void setClignoteVert() { + etat = Etat.ClignoteVert; + timer.reset(); + } + + public void setPosition(double pos) { + RGBRight.setPosition(pos); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AngleShooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AngleShooter.java new file mode 100644 index 000000000000..ef26afad2a8e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/AngleShooter.java @@ -0,0 +1,67 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; +import androidx.annotation.NonNull; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +public class AngleShooter { + private Servo AngleShoot; + + private double positionInitiale; + private double Positionlow = 0.12; + private double Positionmedium = 0.30; + + private double PositionInterMdHaut = 0.40; + private double Positionhaute = 0.65; + + public void init(@NonNull HardwareMap hwMap) { + AngleShoot = hwMap.get(Servo.class, "AngleShoot"); + AngleShoot.setDirection(Servo.Direction.REVERSE); + // Position de départ sécurisée + positionInitiale = Positionlow; + AngleShoot.setPosition(positionInitiale); + + } + + private enum AngleShooteretat { + + IDLE, //Repos + TirProche, + + TirMoyenneDistance, + TirLoin, + + } + private AngleShooteretat angleshooteretat = AngleShooteretat.IDLE; + private ElapsedTime timeretat = new ElapsedTime(); + + public void update() { + + switch (angleshooteretat) { + case IDLE: + break; + + case TirProche: + angleShoot(Positionlow); + break; + + case TirMoyenneDistance: + angleShoot(Positionmedium); + break; + case TirLoin: + angleShoot(Positionhaute); + break; + } + } + public void angleShoot(double angle) { + AngleShoot.setPosition(angle); + } + + // Methode publique utilisable par Tireur Manager + public void setAngle(double pos) + { AngleShoot.setPosition(pos); } + public boolean isAtAngle(double pos) + { return Math.abs(AngleShoot.getPosition() - pos) < 0.02; } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Camera.java new file mode 100644 index 000000000000..28a1f7322c36 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Camera.java @@ -0,0 +1,53 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; +/* +class Camera { + publi + Camera() { + // initialisation de la caméra, pipeline, etc. + } + + void update() { + // Récupère les données de vision + // Exemple : distance, offset horizontal, vertical, etc. + // Calcule les 3 valeurs utiles + computeShooterSpeed(); + computeShooterAngle(); + computeTurretRotation(); + } + + boolean hasTarget() const { + return targetVisible; + } + + float getShooterSpeed() const { + return shooterSpeed; + } + + float getShooterAngle() const { + return shooterAngle; + } + + float getTurretRotation() const { + return turretRotation; + } + + private: + boolean targetVisible = false; + + float shooterSpeed = 0.0f; + float shooterAngle = 0.0f; + float turretRotation = 0.0f; + + void computeShooterSpeed() { + // Exemple : shooterSpeed = f(distance) + } + + void computeShooterAngle() { + // Exemple : shooterAngle = f(distance) + } + + void computeTurretRotation() { + // Exemple : turretRotation = offsetHorizontal + } +}; +*/ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Indexeur.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Indexeur.java new file mode 100644 index 000000000000..cd30787dd75c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Indexeur.java @@ -0,0 +1,559 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; + +import androidx.annotation.NonNull; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DigitalChannel; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import org.firstinspires.ftc.robotcore.external.JavaUtil; +import com.qualcomm.robotcore.util.ElapsedTime; + +public class Indexeur { + private boolean rotationPourAmassage = false; + private static final boolean HOMING_APRES_BOURRAGE = true; + + // Timer d'erreur (temps passé avec une erreur >= tolérance) + private final ElapsedTime erreurTimer = new ElapsedTime(); + // Durée max erreur avant fin (avancerrapide) + private static final double ERREUR_TIMEOUT_S = 2.0; // 1 à 2 secondes + private boolean rotationPourTir = false; + private String capteurDetecteur = "Aucun"; // "Left", "Right", "Fusion" + private float hueDetectee = -1; // Hue retenue + + + + + private DcMotorEx indexeur; + private Intake intake; + + public void setIntake(Intake intake) { this.intake = intake; } + private DigitalChannel magSensorAv; + private Rev2mDistanceSensor distSensorIndexeur; + private Rev2mDistanceSensor distSensorIndexeur2; + private NormalizedColorSensor ColorLeft; + private NormalizedColorSensor ColorRight; + private String couleurDetecteeTemp = "Inconnue"; + private boolean bourrageInit = false; + double hue; + private static final double TICKS_PER_REV_43 = 3895.9; // GoBilda 5203 43 tours + + private int vitessehomingindexeurRPM = 10; + private int vitesserapideindexeurRPM = 25; + private static final int COMPARTIMENTS = 3; + private int positionLogique = 0; + private int compartimentPhysique = 0; + private static final double TICKS_PAR_COMPARTIMENT = TICKS_PER_REV_43 / COMPARTIMENTS; + private int compartimentActuel = 0; + + int ballComptage = 0; + int MAX_BALLS = 3; + int MIN_BALLS = 0; + + + // Variable interne pour mémoriser l'état précédent + private boolean lastBallDetected = false; + private boolean homingDone = false; + private boolean homingDemarre = false; + private boolean marcheForceeIndexeur = false; + private int consecutiveDetections = 0; + private static final int NB_LECTURES = 5; + + private int targetTicks = 0; + private boolean rotationEnCours = false; + private int erreurindexeur = 30; + + public enum Indexeuretat { + IDLE, + RECHERCHEPALE, + + AVANCERAPIDETIR, + AVANCERAPIDEAMASSAGE, + BOURRAGE, + STABILISATION, + PRETPOURTIR, + + HOMING + } + + private Indexeuretat IndexeurState = Indexeuretat.IDLE; + private ElapsedTime timeretat = new ElapsedTime(); + private ElapsedTime indexeurtimer = new ElapsedTime(); + + private ElapsedTime bourragetimer = new ElapsedTime(); + private int SEUIL_MMDETECTION = 5; //seuil detection capteur distance + + private String[] couleurBalleDansCompartiment = new String[COMPARTIMENTS]; + + public void init(@NonNull HardwareMap hwMap) { + + indexeur = hwMap.get(DcMotorEx.class, "Indexeur"); + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + indexeur.setDirection(DcMotor.Direction.REVERSE); + + magSensorAv = hwMap.get(DigitalChannel.class, "magSensorAv"); + magSensorAv.setMode(DigitalChannel.Mode.INPUT); + + // ColorLeft = hwMap.get(ColorSensor.class, "ColorLeft"); + ColorLeft = hwMap.get(NormalizedColorSensor.class, "ColorLeft"); + ColorRight = hwMap.get(NormalizedColorSensor.class, "ColorRight"); + for (int i = 0; i < COMPARTIMENTS; i++) { + couleurBalleDansCompartiment[i] = "Inconnue"; + } + + } + + public void update() { + // Détection couleur déclenchée par la balle + if (intake.getBalleDetectee()) { + String c = detectBallColor(); + if (!"Inconnue".equals(c)) { + couleurDetecteeTemp = c; // capture anticipée + } + } + + + // --- PRIORITÉ ABSOLUE : lancer le homing une seule fois --- + if (!homingDemarre) { + IndexeurState = Indexeuretat.HOMING; + homingDemarre = true; + } + + updateRotation(); // on accelere la rotation apres son enclenchement + + switch (IndexeurState) { + + case HOMING: + homingIndexeur(); + break; + case IDLE: + indexeur.setPower(0); + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + break; + + case RECHERCHEPALE: + if (!detectionpale() && ballComptage < MAX_BALLS) { + setindexeurTargetRPM(6); + } else if (detectionpale()) { + setindexeurTargetRPM(0.0); + IndexeurState = Indexeuretat.IDLE; + } + if (timeretat.milliseconds() > 500) { + } + break; + + case AVANCERAPIDETIR: + rotationPourAmassage = false; + rotationPourTir = true; + avancerIndexeurRapide(); + break; + case AVANCERAPIDEAMASSAGE: + rotationPourAmassage = true; + rotationPourTir = false; + avancerIndexeurRapide(); + break; + + + case BOURRAGE: + reculerIndexeurbourrage(); + break; + + case STABILISATION: + break; + + case PRETPOURTIR: + break; + + + + + } + } + + public double getindexeurVelocityRPM() { + double ticksPerSec = indexeur.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_43; + } + + public void setindexeurTargetRPM(double targetRPM) { + // Conversion RPM -> ticks/sec + double targetTicksPerSec = (targetRPM * TICKS_PER_REV_43) / 60.0; + indexeur.setVelocity(targetTicksPerSec); + } + + public String detectBallColor() { + // Lecture capteur gauche + NormalizedRGBA colorsLeft = ColorLeft.getNormalizedColors(); + float hueLeft = JavaUtil.colorToHue(colorsLeft.toColor()); + + // Lecture capteur droit + NormalizedRGBA colorsRight = ColorRight.getNormalizedColors(); + float hueRight = JavaUtil.colorToHue(colorsRight.toColor()); + + // Détection individuelle + String couleurLeft = detectHueColor(hueLeft); + String couleurRight = detectHueColor(hueRight); + + // --- FUSION + identification du capteur --- + if (couleurLeft.equals("Vert") || couleurRight.equals("Vert")) { + if (couleurLeft.equals("Vert")) { + capteurDetecteur = "Left"; + hueDetectee = hueLeft; + } else { + capteurDetecteur = "Right"; + hueDetectee = hueRight; + } + return "Vert"; + } + + if (couleurLeft.equals("Violet") || couleurRight.equals("Violet")) { + if (couleurLeft.equals("Violet")) { + capteurDetecteur = "Left"; + hueDetectee = hueLeft; + } else { + capteurDetecteur = "Right"; + hueDetectee = hueRight; + } + return "Violet"; + } + + // Aucun capteur n'a détecté de couleur + capteurDetecteur = "Aucun"; + hueDetectee = -1; + return "Inconnue"; + } + + + // Fonction auxiliaire : convertit Hue en Vert/Violet/Inconnue + private String detectHueColor(float hue) { + if (hue >= 90 && hue <= 150) { + return "Vert"; + } else if (hue >= 230 && hue <= 330) { + return "Violet"; + } else { + return "Inconnue"; + } + } + + // Méthode de homing à appeler au démarrage + public void homingIndexeur() { + //if (homingDone) { //IndexeurState = Indexeuretat.IDLE; + //} + // Toujours en RUN_USING_ENCODER pour le homing + indexeur.setPower(0.2); + // si l’aimant est détecté (via detectionpale) + if (detectionpale()) { + indexeur.setPower(0.0); // arrêt + indexeur.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); // mode normal + homingDone = true; + positionLogique = 0; + compartimentPhysique = 0; + rotationEnCours = false; + rotationPourAmassage = false; + rotationPourTir = false; + bourrageInit = false; + homingDone = true; + + IndexeurState = Indexeuretat.IDLE; + + } + // Sécurité : si ça tourne trop longtemps + //if (timeretat.seconds() > 3.0) { + // indexeur.setPower(0); + // homingDone = true; + + + + + } + + public int getBalles() { + return ballComptage; + } + + // Fast advance by one compartment using RUN_TO_POSITION + + public void avancerIndexeurRapide() { + + if (!homingDone || rotationEnCours) return; + + int current = indexeur.getCurrentPosition(); + int currentSlot = (int) Math.round(current / TICKS_PAR_COMPARTIMENT); + int nextSlot = currentSlot + 1; // avance d’un compartiment + targetTicks = (int) Math.round(nextSlot * TICKS_PAR_COMPARTIMENT); + + // Mets à jour la logique d’après ce nextSlot + positionLogique = nextSlot; + compartimentPhysique = Math.floorMod(positionLogique, COMPARTIMENTS); + + indexeur.setTargetPosition(targetTicks); + indexeur.setTargetPositionTolerance(erreurindexeur); // verifier l'ecat + indexeur.setMode(DcMotor.RunMode.RUN_TO_POSITION); + indexeur.setPower(0.6); + + indexeurtimer.reset(); + erreurTimer.reset(); + rotationEnCours = true; + + } + + public void updateRotation() { + + if (!rotationEnCours) return; + + int posActuelle = indexeur.getCurrentPosition(); + int erreur = Math.abs(posActuelle - targetTicks); + + // --- LECTURE CONTINUE DE LA COULEUR --- + String couleurInstant = detectBallColor(); + if (!"Inconnue".equals(couleurInstant)) { + couleurDetecteeTemp = couleurInstant; + } + + // ========================================================= + // 🚨 NOUVEAU : DETECTION DE BLOCAGE MOTEUR (ANTI-BALLE ÉCRASÉE) + // ========================================================= + double vitesse = Math.abs(indexeur.getVelocity()); + + boolean moteurBloque = + indexeur.isBusy() && + vitesse < 3 && // moteur quasi immobile + indexeurtimer.milliseconds() > 1500; // laisse le temps de démarrer + + if (moteurBloque) { + indexeurtimer.reset(); + indexeur.setPower(0); + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + rotationEnCours = false; + rotationPourAmassage = false; + rotationPourTir = false; + + IndexeurState = Indexeuretat.BOURRAGE; + bourragetimer.reset(); + return; + } + + // ========================================================= + // --- Gestion de la vitesse progressive --- + // ========================================================= + if (erreur > 300) { + indexeur.setPower(1); // loin de la cible → rapide + } else { + indexeur.setPower(0.6); // proche → lent et stable + } + + // --- RESET DU TIMER D’ERREUR SI ON EST DANS LA TOLÉRANCE --- + if (erreur < erreurindexeur) { + erreurTimer.reset(); + indexeurtimer.reset(); + } + + // --- CONDITIONS DE FIN --- + boolean finNormale = !indexeur.isBusy(); + boolean stabiliseDansTol = (erreur < erreurindexeur) + && (erreurTimer.seconds() > 0.15); + boolean finParTimerErreur = (erreur >= erreurindexeur) + && (erreurTimer.seconds() > ERREUR_TIMEOUT_S); + + if (!(finNormale || stabiliseDansTol || finParTimerErreur)) { + return; + } + + // ========================================================= + // --- ROTATION FINIE --- + // ========================================================= + indexeur.setPower(0); + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + int pos = indexeur.getCurrentPosition(); + int logicalSlot = (int) Math.round(pos / TICKS_PAR_COMPARTIMENT); + + positionLogique = logicalSlot; + compartimentPhysique = Math.floorMod(positionLogique, COMPARTIMENTS); + + int slotCapteurs = compartimentPhysique; + int slotTir = (compartimentPhysique + 1) % COMPARTIMENTS; + int slotEntree = (compartimentPhysique + 2) % COMPARTIMENTS; + + String couleurSousCapteurs = couleurDetecteeTemp; + couleurDetecteeTemp = "Inconnue"; + + couleurBalleDansCompartiment[slotCapteurs] = couleurSousCapteurs; + + // --- COMPTAGE --- + if (rotationPourAmassage) { + + ballComptage = Math.min(ballComptage + 1, MAX_BALLS); + + if (ballComptage < 3) { + intake.setetatramasage(); + } else { + intake.setetatIDLE(); + } + + } else if (rotationPourTir) { + + couleurBalleDansCompartiment[slotTir] = "Inconnue"; + } + + rotationPourAmassage = false; + rotationPourTir = false; + rotationEnCours = false; + + IndexeurState = finParTimerErreur ? + Indexeuretat.BOURRAGE : + Indexeuretat.IDLE; + + if (IndexeurState == Indexeuretat.BOURRAGE) { + bourragetimer.reset(); + } + + } + + public void reculerIndexeurbourrage() { + if (!bourrageInit) { + bourrageInit = true; + bourragetimer.reset(); + + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + indexeur.setPower(-0.4); + intake.setetatEJECTION(); + + // Probable perte d'une balle + ballComptage = Math.max(ballComptage - 1, 0); + } + + if (bourragetimer.milliseconds() > 500) { + // Fin du recul : on stoppe et on lance un HOMING + indexeur.setPower(0); + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + rotationEnCours = false; + rotationPourAmassage = false; + rotationPourTir = false; + + // ➜ Homing + lancerHoming(); // passe l'état à HOMING et flag homingDemarre = true + bourrageInit = false; + return; + } + } + + public boolean avanceTerminee() { + boolean blocage = indexeur.isBusy() && Math.abs(indexeur.getVelocity()) < 10; + boolean fini = !indexeur.isBusy() || + // le moteur pense avoir atteint sa cible + Math.abs(indexeur.getCurrentPosition() - indexeur.getTargetPosition()) <= indexeur.getTargetPositionTolerance() || // proche de la cible + indexeurtimer.seconds() > 1.0; // sécurité : timeout 1s + if (fini) { + indexeur.setPower(0.0); + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + return fini; + } + + public boolean detectionpale() { + return !magSensorAv.getState(); + } + public boolean isindexeurBusy() { + return indexeur.isBusy(); } + /** * Getter pour accéder directement au moteur si besoin */ + public DcMotor getindexeurMotor() { + return indexeur;} + private static final int TOLERANCE_TIR = 10; + public boolean indexeurPretPourTir() { + if (indexeur == null) + return false; + boolean fini = !indexeur.isBusy(); + int erreur = Math.abs(indexeur.getTargetPosition() - indexeur.getCurrentPosition()); + boolean dansTol = erreur < TOLERANCE_TIR; + return fini && dansTol; } + + + public String getCouleurCompartiment(int compartiment) { + return couleurBalleDansCompartiment[compartiment]; + } + + + public Indexeuretat getEtat() { + return IndexeurState; + } + + public void setEtat(Indexeuretat nouvelEtat) { + + // Empêche l’intake d'interrompre le homing + if (IndexeurState == Indexeuretat.HOMING && + nouvelEtat == Indexeuretat.AVANCERAPIDEAMASSAGE) { //|| + //nouvelEtat == Indexeuretat.AVANCERAPIDETIR)) { + return; + } + + // Empêche l’intake de spammer pendant une rotation + if (rotationEnCours && + nouvelEtat == Indexeuretat.AVANCERAPIDEAMASSAGE) //|| + //nouvelEtat == Indexeuretat.AVANCERAPIDETIR)) + { + return; + } + + this.IndexeurState = nouvelEtat; + } + // --- Appelé par le TireurManager pour avancer une balle vers le tir --- + public void avancerPourTir() { + rotationPourAmassage = false; + rotationPourTir = true; + setEtat(Indexeuretat.AVANCERAPIDETIR); + } + + // --- Appelé par le TireurManager pour savoir si l'avance est finie --- + public boolean isRotationTerminee() { + // On se base sur la logique déjà existante + return !rotationEnCours || indexeurPretPourTir(); + } + public void forcerHomingTermine() { + homingDone = true; + IndexeurState = Indexeuretat.IDLE; + } + public void lancerHoming() { + IndexeurState = Indexeuretat.HOMING; + homingDemarre = true; + } + public boolean isHomingDone() { return homingDone; } + + public void decrementerBalle() { + ballComptage = Math.max(ballComptage - 1, 0); } + + + public void setBalles(int n) { + ballComptage = Math.max(MIN_BALLS, Math.min(n, MAX_BALLS)); + } + + public void resetCompartiments() { + for (int i = 0; i < COMPARTIMENTS; i++) { + couleurBalleDansCompartiment[i] = "Inconnue"; + } + } + // --- Exposer cible & position réelle --- + public int getTargetTicks() { return targetTicks; } + public int getCurrentTicks() { return indexeur.getCurrentPosition(); } + public int getPositionErreurTicks() { return Math.abs(getCurrentTicks() - getTargetTicks()); } + + // --- Micro-correction douce vers la cible planifiée --- + public void microCorrigerVersCible(double power) { + if (indexeur.isBusy()) return; // ne pas perturber une rotation en cours + indexeur.setTargetPosition(targetTicks); + indexeur.setTargetPositionTolerance(erreurindexeur); // ~30 ticks chez toi + indexeur.setMode(DcMotor.RunMode.RUN_TO_POSITION); + indexeur.setPower(power); // 0.2–0.3 : doux, limite l’overshoot + } + +} + + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Intake.java new file mode 100644 index 000000000000..01da12a7f64d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Intake.java @@ -0,0 +1,209 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import androidx.annotation.NonNull; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.robotcore.hardware.DigitalChannel; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import java.util.*; +import java.util.ArrayList; +import java.util.Random; +public class Intake { + + private DcMotorEx IntakeBall; + private AfficheurLeft afficheurLeft; + private boolean ramassageEnabled = true; + private ElapsedTime statetimer = new ElapsedTime(); + int seuilcapteurmm = 50; + + private enum Intakeetat { + IDLE, + RAMASSAGE, + EJECTION, + ARRET_POUR_TIR + } + private Intakeetat intakeState = Intakeetat.IDLE; + + private double intake_reverse = -1000; + + private double intake_fast = 1000; + + + private final double MINRPM = 0; + + // --- Variables télémétrie / détection + private double rpm = 0; + private float lumIndexeur = 0; + private boolean ralentissement = false; + private int score = 0; + + private final int tempsblocage = 200; + private final double CHUTE_RPM = 0.85; + private final float SEUIL_LUM_INDEXEUR = 0.0800f; + private final float LUM_Indexeursansballes = 0.0577f; + + private Indexeur indexeur; + private NormalizedColorSensor ColorIndexeur, ColorIntake; + private Rev2mDistanceSensor distSensorIndexeur; + + private static final double TICKS_PER_REV_6000 = 145.1; // Moteur 1150 RPM + + public void init(@NonNull HardwareMap hwMap) { + + IntakeBall = hwMap.get(DcMotorEx.class, "IntakeBall"); + IntakeBall.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + IntakeBall.setDirection(DcMotor.Direction.REVERSE); + intakeState = Intakeetat.IDLE; + + ColorIndexeur = hwMap.get(NormalizedColorSensor.class, "ColorIndexeur"); + distSensorIndexeur = hwMap.get(Rev2mDistanceSensor.class, "DistSensorIndexeur"); + } + + public Intake(Indexeur indexeur, AfficheurLeft afficheurLeft) { + this.indexeur = indexeur; + this.afficheurLeft = afficheurLeft;} + + public void update() { + + int ballcomptage = indexeur.getBalles(); + + switch (intakeState) { + + case IDLE: + afficheurLeft.setIdle(); + afficheurLeft.update(); + + setIntakeBallTargetRPM(0); + if (ballcomptage == 0) { + afficheurLeft.setVert(); + if (indexeur.isHomingDone()){ + statetimer.reset(); + intakeState = Intakeetat.RAMASSAGE;} + } + break; + case ARRET_POUR_TIR: + setIntakeBallTargetRPM(0); // moteur arrêté + // pas de ramassage, pas de changement d’état + break; + + + case RAMASSAGE: + + // + setIntakeBallTargetRPM(intake_fast); + // --- Mise à jour des variables --- + rpm = IntakeBall.getVelocity() * 60 / TICKS_PER_REV_6000; + lumIndexeur = ColorIndexeur.getNormalizedColors().alpha; + + ralentissement = rpm < intake_fast * CHUTE_RPM; + boolean indexeurLumiere = detectCapteurIndexeur(); + boolean indexeurdistance= balleDetecteeDistSensor(); + + if (ballcomptage == 3) { + intakeState = Intakeetat.IDLE; + break; + } + + // --- Fusion des signaux --- + score = 0; + //if (ralentissement) score++; + if (indexeurLumiere) score++; + if (indexeurdistance) score++; + + if (score >= 1) { + indexeur.setEtat(Indexeur.Indexeuretat.AVANCERAPIDEAMASSAGE); + statetimer.reset(); + //IntakeBall.setVelocity(0); + } + + // --- Bourrage --- + //if (rpm <= MINRPM && statetimer.milliseconds() > tempsblocage) { + // statetimer.reset(); + // intakeState = Intakeetat.EJECTION; + //} + break; + + case EJECTION: + setIntakeBallTargetRPM(intake_reverse); + afficheurLeft.setOrange(); + if (statetimer.milliseconds() > 400) { + //if (ballcomptage == 3) { + // intakeState = Intakeetat.IDLE; + //} else { + statetimer.reset(); + intakeState = Intakeetat.RAMASSAGE; + } + break; + } + } + + public void setIntakeBallTargetRPM(double targetRPM) { + double targetTicksPerSec = (targetRPM * TICKS_PER_REV_6000) / 60.0; + IntakeBall.setVelocity(targetTicksPerSec); + } + + + private boolean detectCapteurIndexeur() { + return lumIndexeur > SEUIL_LUM_INDEXEUR; + } + + + public boolean balleDetecteeDistSensor() { + int count = 0; + for (int i = 0; i < 2; i++) { + double d1 = distSensorIndexeur.getDistance(DistanceUnit.MM); + boolean capteur1Detecte = Double.isFinite(d1) && d1 > 5 && d1 < seuilcapteurmm; + if (capteur1Detecte) count++; + } + return count >= 1; // au moins une lecture valide + } + + // --- GETTERS --- + public double getRPM() { return rpm; } + + public float getLumIndexeur() { return ColorIndexeur.getNormalizedColors().alpha; } + public boolean getRalentissement() { return ralentissement; } + public int getScore() { return score; } + public double getCapteurDistance() { return distSensorIndexeur.getDistance(DistanceUnit.MM);} + public boolean getBalleDetectee() { return score >= 2; } + public boolean isIdle() { return intakeState == Intakeetat.IDLE; } + + + + + public void enableRamassage() { + ramassageEnabled = true; + } + + public void disableRamassage() { + ramassageEnabled = false; + } + public void arretPourTir() { + intakeState = Intakeetat.ARRET_POUR_TIR; } + + public void repriseApresTir() { + intakeState = Intakeetat.IDLE; } + public Intakeetat getEtat() { + return intakeState; + } + + public void setetatramasage(){ + intakeState = Intakeetat.RAMASSAGE; + } + public void setetatIDLE(){ + intakeState = Intakeetat.IDLE; + } + public void setetatEJECTION(){ + statetimer.reset(); + intakeState = Intakeetat.EJECTION; + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/ServoTireur.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/ServoTireur.java new file mode 100644 index 000000000000..08c45209bb9d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/ServoTireur.java @@ -0,0 +1,82 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Indexeur; +import androidx.annotation.NonNull; + +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import java.util.*; +import java.util.ArrayList; +import java.util.Random; +import org.firstinspires.ftc.robotcore.external.JavaUtil; +import com.qualcomm.robotcore.hardware.DigitalChannel; +import com.qualcomm.robotcore.util.ElapsedTime; + + +public class ServoTireur { + private Servo ServoTir; + private Indexeur indexeur; + private double tirpositionretour = 0.62 + ; // position basse 079 gobilda 1 vers l'exterieur + private double tirpositionhaute = 0.11 + ; // position haute 031 gobila speed + + private ElapsedTime timeretat = new ElapsedTime(); + private boolean tirServoEnCours = false; + + private boolean pauseTir = false; + + + private enum Tireuretat { + IDLE, // servo en position basse + TirPositionhaute // Servo en position haute pour lancer balle + + } + + private Tireuretat tireuretat = Tireuretat.IDLE; + + public void init(@NonNull HardwareMap hwMap) { + ServoTir= hwMap.get(Servo.class, "ServoTir"); + ServoTir.setPosition(tirpositionretour); + + + } + public ServoTireur(Indexeur indexeur) { this.indexeur = indexeur; } + public void update() { + int ballComptage = indexeur.getBalles(); + + switch (tireuretat) { + case IDLE: + tirerAvecServo(tirpositionretour); + break; + + case TirPositionhaute: + tirerAvecServo(tirpositionhaute); + break; + } + } + public void tirerAvecServo(double servoposition) { + ServoTir.setPosition(servoposition); // monter ou descendre le servo + tirServoEnCours = true; // activer le flag + } + // --- Monter le servo pour tirer --- + public void push() { + ServoTir.setPosition(tirpositionhaute); + } + // --- Redescendre le servo --- + public void retract() { + ServoTir.setPosition(tirpositionretour); + } + // --- Vérifier si le servo est bien monté --- + public boolean isPushDone() { + return Math.abs(ServoTir.getPosition() - tirpositionhaute) < 0.02; + } + // --- Vérifier si le servo est bien redescendu --- + public boolean isRetractDone() { + return Math.abs(ServoTir.getPosition() - tirpositionretour) < 0.02; + } + public double getPosition() { return ServoTir.getPosition(); } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Shooter.java new file mode 100644 index 000000000000..dccd946d3774 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Shooter.java @@ -0,0 +1,132 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; +import androidx.annotation.NonNull; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import com.qualcomm.robotcore.util.ElapsedTime; + +public class Shooter { + private static final int TICKS_PER_REV_6000 = 28; // GoBilda 5203 ratio 1:1 + private int Shootpower; + private static final double MAX_RPM = 6000.0; + private DcMotorEx Shooter,Shooter2; + private Indexeur indexeur; + + private enum Shooteretat { + IDLE, //Repos + TirProche, + TirMoyen, + TirLoin, + TirMax, + + } + private Shooteretat shooteretat = Shooteretat.IDLE; + private ElapsedTime timeretat = new ElapsedTime(); + private int shooterlowspeed= 4000; + private int shootermediumspeed = 4500; + private int shootermaxspeed = 5000; + + private double currentTargetVel = 0.0; + + public void init(@NonNull HardwareMap hwMap) { + + double maxVelRPM = 5000.0; + double maxVel = (maxVelRPM * TICKS_PER_REV_6000) / 60.0; + double kF = 15.5; + double kP = 0.002; // Agressif car systeme rapide + double kI = 0.0 ; // nuisible voir inutile avec shooter + double kD = 0.0 ; // amortissement raisonnable + + this.indexeur = indexeur; + Shooter = hwMap.get(DcMotorEx.class, "Shooter"); + Shooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Shooter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + //Shooter.setVelocityPIDFCoefficients(kP, kI, kD, kF); + + //Test avec deux moteurs + Shooter2 = hwMap.get(DcMotorEx.class, "Shooter2"); + Shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Shooter2.setDirection(DcMotor.Direction.REVERSE); + Shooter2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + //Shooter2.setVelocityPIDFCoefficients(kP, kI, kD, kF); + + } + public void update() { + int ballcomptage = indexeur.getBalles(); + + switch (shooteretat) { + case IDLE: + setShooterTargetRPM(0); + if (ballcomptage > 2) { + timeretat.reset(); + shooteretat = shooteretat.TirProche; + } + + break; + case TirProche: + setShooterTargetRPM(shooterlowspeed); + + break; + case TirMoyen: + setShooterTargetRPM(shootermediumspeed); + + break; + case TirLoin: + setShooterTargetRPM(shootermaxspeed); + + break; + } + } + + public void setshooterFullPower(double Shootpower){ + // mettre le moteur a fond pour lancer de balle sans controle + Shooter.setPower(Shootpower); + Shooter2.setPower(Shootpower); + + } + + public void setShooterTargetRPM(double targetRPM) { + + //Conversion RPM -> ticks/sec + double targetTicksPerSec = (targetRPM * TICKS_PER_REV_6000) / 60.0; + Shooter.setVelocity(targetTicksPerSec); + Shooter2.setVelocity(targetTicksPerSec); + } + public void displayShooterVelocity(Telemetry telemetry) { + telemetry.addData("Shooter Speed (RPM)", getShooterVelocityRPM()); + } + + public double getShooter2VelocityRPM() { + double ticksPerSec = Shooter2.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_6000; + } + + // Méthode lire la vitesse du moteur de lancement Balle shooter et affichage + public double getShooterVelocityRPM() { + double v1 = Shooter.getVelocity(); + double v2 = Shooter2.getVelocity(); + + double rpm1 = (v1 * 60.0) / TICKS_PER_REV_6000; + double rpm2 = (v2 * 60.0) / TICKS_PER_REV_6000; + + double diff = Math.abs(rpm1 - rpm2); + + // Si un moteur décroche vraiment, on ignore le plus lent + if (diff > 350) { // seuil adapté pour 3800–4900 RPM + return Math.max(rpm1, rpm2); + } + + // Sinon moyenne normale = vitesse réelle flywheel + return (rpm1 + rpm2) / 2.0; + } + public void setIndexeur(Indexeur indexeur) { + this.indexeur = indexeur; + } + + + + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/SpinTurret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/SpinTurret.java new file mode 100644 index 000000000000..220a56d15550 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/SpinTurret.java @@ -0,0 +1,240 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Hardware; +import androidx.annotation.NonNull; +import com.qualcomm.hardware.rev.Rev9AxisImuOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.hardware.bosch.BNO055IMU; + +public class SpinTurret { + private CRServo SpinTourelle; + private double powerarrettourelle = 0.0; + private double powertournertourelle =1.0; + public IMU imuTourelle; + private boolean turretZeroDone = false; + private long imuReadySince = 0; + double erreur; + + private BNO055IMU boschImu; + + private enum spintourelleetat { + IDLE, + CentrageZeroTourelle, // PositionCentrale + TournerJoystick, + TournerAutoCamera, + TournerAutoBlueSansCamera, + TournerAutoRedSansCamera, + + } + private spintourelleetat Spintourelleetat = spintourelleetat.IDLE; + + public void init(@NonNull HardwareMap hwMap) { + SpinTourelle = hwMap.get(CRServo.class, "SpinTourelle"); + SpinTourelle.setDirection(CRServo.Direction.REVERSE); + //SpinTourelle.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + //Mettre le moteur en mode BRAKE + //SpinTourelle.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + //SpinTourelle.setPower(0); + + imuTourelle = hwMap.get(IMU.class, "imuTourelle"); + IMU.Parameters parameters = new IMU.Parameters( + new Rev9AxisImuOrientationOnRobot( + Rev9AxisImuOrientationOnRobot.LogoFacingDirection.UP, + Rev9AxisImuOrientationOnRobot.I2cPortFacingDirection.LEFT) + ); + imuTourelle.initialize(parameters); + imuTourelle.resetYaw(); + + + + + } + public void update() { + + switch (Spintourelleetat) { + case IDLE: + SpinTourelle.setPower(powerarrettourelle); + break; + + case CentrageZeroTourelle: + if (lectureangletourelle() > 2) { + SpinTourelle.setPower(-powertournertourelle); + } + if (lectureangletourelle() < -2){ + SpinTourelle.setPower(powertournertourelle); + } + if (lectureangletourelle() <= 2 && lectureangletourelle() >= -2){ + SpinTourelle.setPower(powerarrettourelle); + } + break; + case TournerJoystick: + //rotationtourelle(); + break; + + + } + } + // Méthode pour lecture de l'angle gyro de la tourelle et pour faire tourner la tourelle avec un angle max + public double lectureangletourelle() { + double angletourelle = imuTourelle.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + return angletourelle; // en degrés; + } + public void rotationtourelle(double power) { + // Limitation de la puissance maximale + double maxPower = 0.13; // valeur maximale autorisée + power = Math.max(-maxPower, Math.min(power, maxPower)); // clamp entre -1 et +1 + + double angletourelle = lectureangletourelle(); + if ((angletourelle >= 62 && power < 0) || (angletourelle <= -62 && power > 0)) { + SpinTourelle.setPower(0); + } else { + SpinTourelle.setPower(power); + } + } + + public void allerVersAngle(double angleCible) { + double angleActuel = lectureangletourelle(); + erreur = angleActuel - angleCible; + + double absErr = Math.abs(erreur); + double power; + + // 1) Fenêtre d'arrêt : on stoppe complètement + if (absErr < 5.0) { + power = 0.0; // STOP + } + // 2) Zone proche : on bouge lentement mais avec un minimum + else if (absErr < 10.0) { + double minPower = 0.1; // + power = Math.signum(erreur) * minPower; + } + // 3) Zone lointaine : proportionnel normal + else { + double kP = 0.05; + power = kP * erreur; + + power = Math.max(-0.11, Math.min(power, 0.11)); + } + + SpinTourelle.setPower(power); + } + + + + // Connaitre l'angle de la tourelle + public boolean isAtAngle(double angleCible) { + return Math.abs(lectureangletourelle() - angleCible) < 5.0; + } + + public void stopTourelle() { + SpinTourelle.setPower(0); + }// blocage servo } + + public double geterreur(){ + return erreur; + } + + public void resetImuToutelle(){ + imuTourelle.resetYaw(); + } + + + public void tournerAvecTx(double tx) { + + double kP = 0.03; // un peu plus fort + double maxPower = 0.30; // vitesse max + double minPower = 0.12; // seuil mécanique réaliste + + double power = kP * tx; + + // Appliquer une puissance minimale si on est encore loin du centre + if (Math.abs(tx) > 2.0 && Math.abs(power) < minPower) { + power = Math.signum(tx) * minPower; + } + + // Limitation + power = Math.max(-maxPower, Math.min(power, maxPower)); + + // Zone morte finale + if (Math.abs(tx) < 1.0) { + power = 0; + } + + SpinTourelle.setPower(power); + } + + public void tournerAvecTxoffset(double tx, double offset) { + + double kP = 0.03; // gain proportionnel + double maxPower = 0.19; // palier 4 : > 15° + double power5 = 0.179; // palier 1 : 0–5° + double power10 = 0.183; // palier 2 : 5–10° + double power15 = 0.185; // palier 3 : 10–15° + + + // Erreur corrigée selon l’alliance + double erreur = tx - offset; + double absErr = Math.abs(erreur); + + double power; + + // ----------------------------- + // PALIER 1 : zone morte (< 1°) + // ----------------------------- + if (absErr < 1.0) { + power = 0; + } + + // ----------------------------- + // PALIER 2 : 1° à 5° + // puissance minimale + // ----------------------------- + else if (absErr < 5.0) { + power = Math.signum(erreur) * power5; + } + + // ----------------------------- + // PALIER 3 : 5° à 10° + // deuxième palier + // ----------------------------- + else if (absErr < 10.0) { + power = Math.signum(erreur) * power10; + } + + // ----------------------------- + // PALIER 4 : 10° à 15° + // troisième palier + // ----------------------------- + else if (absErr < 15.0) { + power = Math.signum(erreur) * power15; + } + + // ----------------------------- + // PALIER 5 : > 15° + // puissance max + // ----------------------------- + else { + power = Math.signum(erreur) * maxPower; + } + + // Limitation finale + power = Math.max(-maxPower, Math.min(power, maxPower)); + + SpinTourelle.setPower(power); + } + + public boolean isAtTx(double tx, double TxCible) { + return Math.abs(tx - TxCible) < 1.0; + } + + +} + + + + + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/AprilTagLimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/AprilTagLimelightTest.java new file mode 100644 index 000000000000..fe33adc7df43 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/AprilTagLimelightTest.java @@ -0,0 +1,96 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.SpinTurret; + +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +@TeleOp(name="Limelight", group="debug") +public class AprilTagLimelightTest extends OpMode { + + private Limelight3A limelight; + SpinTurret spinTurret; + boolean autoAimActive = false; + boolean isBlueAlliance = true; // ou false selon le match + + public double distanceFromTa(double ta) { + if (ta <= 0 || Double.isNaN(ta)) return -1; + return 186.6697 * Math.pow(ta, -0.6873269); + } + + @Override + public void init() { + spinTurret = new SpinTurret(); + spinTurret.init(hardwareMap); + limelight = hardwareMap.get(Limelight3A.class, "Limelight"); + limelight.pipelineSwitch(0); // pipeline AprilTag + } + + @Override + public void start() { + limelight.start(); + } + + + @Override + public void loop() { + + spinTurret.update(); + LLResult result = limelight.getLatestResult(); + + if (result != null && result.isValid()) { + + double tx = result.getTx(); + double ta = result.getTa(); + double distance = distanceFromTa(ta); + + telemetry.addData("Tx", tx); + telemetry.addData("TA", ta); + telemetry.addData("Distance", distance); + }else{ + telemetry.addData("Tx", 0); + telemetry.addData("TA", 0); + telemetry.addData("Distance", 0); + autoAimActive = false; + spinTurret.stopTourelle(); + } + if (gamepad1.x) { + autoAimActive = true; // on active l’auto-aim + } + +// Définir l’offset selon l’alliance +// +2.0 = côté bleu +// -2.0 = côté rouge + double offset = isBlueAlliance ? 0.0 : -0.0; + + if (autoAimActive && result != null && result.isValid()) { + + double tx = result.getTx(); + + // Appel avec offset + spinTurret.tournerAvecTxoffset(tx, offset); + + // Si on est centré → on coupe l’auto-aim + if (Math.abs(tx - offset) <= 1.0) { + autoAimActive = false; + spinTurret.stopTourelle(); + } + + telemetry.addData("Tx", tx); + telemetry.addData("Offset", offset); + } + + + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/FlywheelPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/FlywheelPIDTuner.java new file mode 100644 index 000000000000..07457f4b0f6d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/FlywheelPIDTuner.java @@ -0,0 +1,168 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Disabled +@TeleOp (name="PIDF Motors", group="debug") +public class FlywheelPIDTuner extends OpMode { + + private static final int TICKS_PER_REV_6000 = 28; // GoBilda 5203 ratio 1:1 + + private static final double MAX_RPM = 6000.0; + private DcMotorEx Shooter, Shooter2; + + private ElapsedTime timeretat = new ElapsedTime(); + private double currentTargetVel = 0.0; + public double highVelocity = 4950; + public double lowVelocity = 3500; + double curTargetVelocity = highVelocity; + double F = 0; + double P = 0; + + double[] stepSizes = {10.0, 1.0, 0.1, 0.001, 0.0001}; + int stepIndex = 1; + + + @Override + public void init() { + + double maxVelRPM = 4700.0; + double maxVel = (maxVelRPM * TICKS_PER_REV_6000) / 60.0; + double kF = 13; + double kP = 1; // Agressif car systeme rapide + double kI = 0.0; // nuisible voir inutile avec shooter + double kD = 0.0002; // amortissement raisonnable + + Shooter = hardwareMap.get(DcMotorEx.class, "Shooter"); + Shooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Shooter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + Shooter2 = hardwareMap.get(DcMotorEx.class, "Shooter2"); + Shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Shooter2.setDirection(DcMotor.Direction.REVERSE); + Shooter2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + PIDFCoefficients pidfCoefficients = new PIDFCoefficients(P, 0, 0, F); + Shooter.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients); + Shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients); + telemetry.addLine("init complete"); + + + } + + //Test avec deux moteurs + //Shooter2 = hardwareMap.get(DcMotorEx.class, "Shooter2"); + //Shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + //Shooter2.setDirection(DcMotor.Direction.REVERSE); + //Shooter2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + //Shooter2.setVelocityPIDFCoefficients(kP, kI, kD, kF);} + // + public void setshooterFullPower(double Shootpower) { + // mettre le moteur a fond pour lancer de balle sans controle + Shooter.setPower(Shootpower); + //Shooter2.setPower(Shootpower); + } + + public void setShooterTargetRPM(double targetRPM) { + + //Conversion RPM -> ticks/sec + double targetTicksPerSec = (targetRPM * TICKS_PER_REV_6000) / 60.0; + Shooter.setVelocity(targetTicksPerSec); + + /*if (targetRPM > 0) { + double ramp = 10.0; //un peu plus nerveux qu'un ascenseur à dubai + + double tolerancemoteur = 25; // ticks/s 50PMx28/60 + + double error = targetTicksPerSec - currentTargetVel; + if (Math.abs(error) > tolerancemoteur) { + double step = Math.copySign(Math.min(Math.abs(error), ramp), error); + currentTargetVel += step; + } else { + currentTargetVel = targetTicksPerSec; // nous sommes assez proche, on reste sur cette zone de vitesse) + } + + Shooter.setVelocity(currentTargetVel);} + //Shooter2.setVelocity(currentTargetVel);} + else{ + currentTargetVel = 0; + Shooter.setVelocity(0.0); + } + */ + + + //setshooterFullPower(1); + + } + + //public double getShooter2VelocityRPM () { + //double ticksPerSec = Shooter2.getVelocity(); + //return (ticksPerSec * 60) / TICKS_PER_REV_6000; + + // Méthode lire la vitesse du moteur de lancement Balle shooter et affichage + public double getShooterVelocityRPM() { + double ticksPerSec = Shooter.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_6000; + } + public double getShooter2VelocityRPM() { + double ticksPerSec = Shooter2.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_6000; + } + + @Override + public void loop() { + if (gamepad1.yWasPressed()) { + if (curTargetVelocity == highVelocity) { + curTargetVelocity = lowVelocity; + } else { + curTargetVelocity = highVelocity; + } + + if (gamepad1.bWasPressed()) { + stepIndex = (stepIndex + 1) % stepSizes.length; + } + + if (gamepad1.dpadLeftWasPressed()) { + F -= stepSizes[stepIndex]; + } + if (gamepad1.dpadRightWasPressed()) { + F += stepSizes[stepIndex]; + } + + if (gamepad1.dpadUpWasPressed()) { + P += stepSizes[stepIndex]; + } + if (gamepad1.dpadDownWasPressed()) { + P -= stepSizes[stepIndex]; + } + //setnew PIDF coefficient + PIDFCoefficients pidfCoefficients = new PIDFCoefficients(P, 0, 0, F); + Shooter.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients); + Shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidfCoefficients); + + Shooter.setVelocity((curTargetVelocity)); + Shooter2.setVelocity((curTargetVelocity)); + + double curVelocity = Shooter.getVelocity(); + double error = curTargetVelocity - curVelocity; + + telemetry.addData("Shooter 1 RPM", getShooterVelocityRPM()); + telemetry.addData("Shooter 2 RPM", getShooter2VelocityRPM()); + telemetry.addData("Target Velocity", curVelocity); + telemetry.addData("Error", error); + telemetry.addData("tuning P", P); + telemetry.addData("Tuning F", F); + telemetry.addData("Step Size", stepSizes[stepIndex]); + + } + } +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TeleOpDecodeTestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TeleOpDecodeTestAuto.java new file mode 100644 index 000000000000..4a1bd651fd69 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TeleOpDecodeTestAuto.java @@ -0,0 +1,232 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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; + +import java.util.function.Supplier; + +@Disabled +@Configurable +@TeleOp (name="AutoTir Test", group="debug") +public class TeleOpDecodeTestAuto extends OpMode { + private Follower follower; + public static Pose startingPose; //See ExampleAuto to understand how to use this + private boolean automatedDrive; + private Supplier pathChain; + private TelemetryManager telemetryM; + private boolean slowMode = false; + private double slowModeMultiplier = 0.5; + private Intake intake; + private Indexeur indexeur; + + private double targetRPM = 0.0; + + private AfficheurLeft afficheurLeft; + private TireurManager tireurManager; + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter ServoAngleShoot; + private ServoTireur servoTireur; + + private AfficheurRight afficheurRight; + private boolean lastX = false; + private boolean lastA = false; + private boolean lastB = false; + private boolean lastY = false; + private boolean lastrightbumper = false ; + private boolean lastleftbumper = false; + int positionAngleshoot =0 ; + boolean anglePresetMode = false; + + int presetIndexShooter = 0; + + + @Override + public void init() { + + tourelle = new SpinTurret(); + tourelle.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + tireurManager = new TireurManager(shooter, tourelle, ServoAngleShoot, servoTireur, indexeur, intake, afficheurRight); + ; + + + + + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(startingPose == null ? new Pose() : startingPose); + follower.update(); + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + tireurManager = new TireurManager(shooter, tourelle, ServoAngleShoot, servoTireur, indexeur, intake, afficheurRight); + ; + + pathChain = () -> follower.pathBuilder() //Lazy Curve Generation + .addPath(new Path(new BezierLine(follower::getPose, new Pose(45, 98)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(45), 0.8)) + .build(); + } + + @Override + public void start() { + //The parameter controls whether the Follower should use break mode on the motors (using it is recommended). + //In order to use float mode, add .useBrakeModeInTeleOp(true); to your Drivetrain Constants in Constant.java (for Mecanum) + //If you don't pass anything in, it uses the default (false) + follower.startTeleopDrive(); + } + + @Override + public void loop() { + //Call this once per loop + follower.update(); + telemetryM.update(); + + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + + //This is the normal version to use in the TeleOp + if (!slowMode) follower.setTeleOpDrive( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x, + false // Robot Centric + ); + + //This is how it looks with slowMode on + else follower.setTeleOpDrive( + -gamepad1.left_stick_y * slowModeMultiplier, + -gamepad1.left_stick_x * slowModeMultiplier, + -gamepad1.right_stick_x * slowModeMultiplier, + false// Robot Centric + ); + } + + //Automated PathFollowing + if (gamepad1.aWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + + //Stop automated following if the follower is done + if (automatedDrive && (gamepad1.bWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + //Slow Mode + if (gamepad1.rightBumperWasPressed()) { + slowMode = !slowMode; + } + + //Optional way to change slow mode strength + if (gamepad1.xWasPressed()) { + slowModeMultiplier += 0.25; + } + + //Optional way to change slow mode strength + //if (gamepad2.yWasPressed()) { + // slowModeMultiplier -= 0.25; + //} + + // --- Déclenchement tir auto --- + if (gamepad2.right_bumper && !lastrightbumper) { + + // Exemple : tir droit devant + double angleTourelle = 0; // à adapter + double angleShooter = 0.12; // à adapter + double vitesseShooter = 3775; + + tireurManager.startTirAuto(angleTourelle, angleShooter, vitesseShooter); + } + + lastrightbumper = gamepad2.right_bumper; + // --- Mise à jour du manager --- + + if (gamepad2.left_bumper && !lastleftbumper) { + + double angleTourelle = 0; // à adapter + double angleShooter = 0.12; // à adapter + double vitesseShooter = 3750; // à adapter + + tireurManager.startTirAuto(angleTourelle, angleShooter, vitesseShooter); + + } + + lastleftbumper = gamepad2.left_bumper; + + //1) gestion des prereglages tourelles avec le bouton X en manuel + + //tourelle.allerVersAngle(45); + + telemetryM.debug("position", follower.getPose()); + telemetryM.debug("velocity", follower.getVelocity()); + telemetryM.debug("automatedDrive", automatedDrive); + intake.update(); + indexeur.update(); + tireurManager.update(); + + telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle()); + telemetry.addData("AngleShoot", positionAngleshoot); + 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(); + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TeleOpTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TeleOpTest.java new file mode 100644 index 000000000000..e68ecda6d7e1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TeleOpTest.java @@ -0,0 +1,147 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Indexeur; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Intake; + + +import java.util.function.Supplier; +@Disabled +@Configurable +@TeleOp (name="TeleOp Test Seul ", group="debug") +public class TeleOpTest extends OpMode { + private Follower follower; + public static Pose startingPose; //See ExampleAuto to understand how to use this + private boolean automatedDrive; + private Supplier pathChain; + private TelemetryManager telemetryM; + private boolean slowMode = false; + private double slowModeMultiplier = 0.5; + private Intake intake; + private Indexeur indexeur; + private Servo ServoTir; + + + @Override + public void init() { + //indexeur = new Indexeur(); + //indexeur.init(hardwareMap); + + //intake = new Intake(indexeur); + //intake.init(hardwareMap); + //indexeur.setIntake(intake); + + ServoTir= hardwareMap.get(Servo.class, "ServoTir"); + ServoTir.setPosition(0.79); + + + follower = Constants.createFollower(hardwareMap); + follower.setStartingPose(startingPose == null ? new Pose() : startingPose); + follower.update(); + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + + pathChain = () -> follower.pathBuilder() //Lazy Curve Generation + .addPath(new Path(new BezierLine(follower::getPose, new Pose(45, 98)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(45), 0.8)) + .build(); + } + + @Override + public void start() { + //The parameter controls whether the Follower should use break mode on the motors (using it is recommended). + //In order to use float mode, add .useBrakeModeInTeleOp(true); to your Drivetrain Constants in Constant.java (for Mecanum) + //If you don't pass anything in, it uses the default (false) + follower.startTeleopDrive(); + } + + @Override + public void loop() { + //Call this once per loop + follower.update(); + telemetryM.update(); + + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + + //This is the normal version to use in the TeleOp + if (!slowMode) follower.setTeleOpDrive( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x, + false // Robot Centric + ); + + //This is how it looks with slowMode on + else follower.setTeleOpDrive( + -gamepad1.left_stick_y * slowModeMultiplier, + -gamepad1.left_stick_x * slowModeMultiplier, + -gamepad1.right_stick_x * slowModeMultiplier, + false// Robot Centric + ); + } + + //Automated PathFollowing + if (gamepad1.aWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + + //Stop automated following if the follower is done + if (automatedDrive && (gamepad1.bWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + //Slow Mode + if (gamepad1.rightBumperWasPressed()) { + slowMode = !slowMode; + } + + if (gamepad2.xWasPressed()) { + ServoTir.setPosition(0.2); + } + + //Optional way to change slow mode strength + if (gamepad1.xWasPressed()) { + slowModeMultiplier += 0.25; + } + + //Optional way to change slow mode strength + if (gamepad2.yWasPressed()) { + slowModeMultiplier -= 0.25; + } + + telemetryM.debug("position", follower.getPose()); + telemetryM.debug("velocity", follower.getVelocity()); + telemetryM.debug("automatedDrive", automatedDrive); + //intake.update(); // très important + //indexeur.update(); // si tu en as un + + //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.update(); + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test1MoteurShooter1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test1MoteurShooter1.java new file mode 100644 index 000000000000..989867e73949 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test1MoteurShooter1.java @@ -0,0 +1,87 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Disabled +@TeleOp (name="Shooter 1 PIDF", group="debug") +public class Test1MoteurShooter1 extends OpMode { + + private static final int TICKS_PER_REV_6000 = 28; // GoBilda 5203 ratio 1:1 + + private static final double MAX_RPM = 6000.0; + private DcMotorEx Shooter, Shooter2; + + private ElapsedTime timeretat = new ElapsedTime(); + private double currentTargetVel = 0.0; + + @Override + public void init () { + + double maxVelRPM = 4700.0; + double maxVel = (maxVelRPM * TICKS_PER_REV_6000) / 60.0; + double kF = 1; + double kP = 6.0; // Agressif car systeme rapide + double kI = 0.0; // nuisible voir inutile avec shooter + double kD = 0.4; // amortissement raisonnable + + //Test avec Shooter 1 left motor + Shooter = hardwareMap.get(DcMotorEx.class, "Shooter"); + Shooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Shooter.setDirection(DcMotor.Direction.REVERSE); + Shooter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + } + + public void setshooterFullPower ( double Shootpower){ + // mettre le moteur a fond pour lancer de balle sans controle + //Shooter.setPower(Shootpower); + Shooter.setPower(Shootpower); + } + + public void setShooterTargetRPM (double targetRPM){ + + //Conversion RPM -> ticks/sec + double targetTicksPerSec = (targetRPM * TICKS_PER_REV_6000) / 60.0; + + if (targetRPM > 0) { + double ramp = 4.0; //un peu plus nerveux qu'un ascenseur à dubai + + double tolerancemoteur = 25; // ticks/s 50PMx28/60 + + double error = targetTicksPerSec - currentTargetVel; + if (Math.abs(error) > tolerancemoteur) { + double step = Math.copySign(Math.min(Math.abs(error), ramp), error); + currentTargetVel += step; + } else { + currentTargetVel = targetTicksPerSec; // nous sommes assez proche, on reste sur cette zone de vitesse) + } + + //Shooter.setVelocity(currentTargetVel); + Shooter.setVelocity(currentTargetVel);} + else { + currentTargetVel = 0; + //Shooter.setVelocity(0.0); + Shooter.setVelocity(0.0);} + //Shooter.setVelocity(targetTicksPerSec); + + } + + + // Méthode lire la vitesse du moteur de lancement Balle shooter et affichage + public double getShooterVelocityRPM () { + double ticksPerSec = Shooter.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_6000; + } + @Override + public void loop () { + + setShooterTargetRPM(4000); + telemetry.addData("telemetry", getShooterVelocityRPM()); + } + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test1MoteurShooter2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test1MoteurShooter2.java new file mode 100644 index 000000000000..69224a57dccc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test1MoteurShooter2.java @@ -0,0 +1,95 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Disabled +@TeleOp (name="Shooter 2 PIDF", group="debug") + +public class Test1MoteurShooter2 extends OpMode { + + private static final int TICKS_PER_REV_6000 = 28; // GoBilda 5203 ratio 1:1 + + private static final double MAX_RPM = 6000.0; + private DcMotorEx Shooter, Shooter2; + + private ElapsedTime timeretat = new ElapsedTime(); + private double currentTargetVel = 0.0; + + @Override + public void init () { + + double maxVelRPM = 4700.0; + double maxVel = (maxVelRPM * TICKS_PER_REV_6000) / 60.0; + double kF = 1; + double kP = 6.0; // Agressif car systeme rapide + double kI = 0.0; // nuisible voir inutile avec shooter + double kD = 0.4; // amortissement raisonnable + + //Shooter = hardwareMap.get(DcMotorEx.class, "Shooter"); + //Shooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + //Shooter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + //Shooter.setVelocityPIDFCoefficients(kP, kI, kD, kF); + + //Test avec deux moteurs + Shooter2 = hardwareMap.get(DcMotorEx.class, "Shooter2"); + Shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Shooter2.setDirection(DcMotor.Direction.REVERSE); + Shooter2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + //Shooter2.setVelocityPIDFCoefficients(kP, kI, kD, kF); + } + + public void setshooterFullPower ( double Shootpower){ + // mettre le moteur a fond pour lancer de balle sans controle + //Shooter.setPower(Shootpower); + Shooter2.setPower(Shootpower); + } + + public void setShooterTargetRPM (double targetRPM){ + + //Conversion RPM -> ticks/sec + double targetTicksPerSec = (targetRPM * TICKS_PER_REV_6000) / 60.0; + + /*if (targetRPM > 0) { + double ramp = 10.0; //un peu plus nerveux qu'un ascenseur à dubai + + double tolerancemoteur = 25; // ticks/s 50PMx28/60 + + double error = targetTicksPerSec - currentTargetVel; + if (Math.abs(error) > tolerancemoteur) { + double step = Math.copySign(Math.min(Math.abs(error), ramp), error); + currentTargetVel += step; + } else { + currentTargetVel = targetTicksPerSec; // nous sommes assez proche, on reste sur cette zone de vitesse) + } + + //Shooter.setVelocity(currentTargetVel); + Shooter2.setVelocity(currentTargetVel);} + else { + currentTargetVel = 0; + //Shooter.setVelocity(0.0); + Shooter2.setVelocity(0.0);} + */ + //Shooter2.setVelocity(targetTicksPerSec); + setshooterFullPower(1); + } + + + // Méthode lire la vitesse du moteur de lancement Balle shooter et affichage + public double getShooterVelocityRPM () { + double ticksPerSec = Shooter2.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_6000; + } + @Override + public void loop () { + + setShooterTargetRPM(4000); + } + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test2MoteurShooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test2MoteurShooter.java new file mode 100644 index 000000000000..7f19aeb569b0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/Test2MoteurShooter.java @@ -0,0 +1,87 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +@Disabled +@TeleOp (name="Test 2 moteurs", group="debug") + +public class Test2MoteurShooter extends OpMode { + + private static final int TICKS_PER_REV_6000 = 28; // GoBilda 5203 ratio 1:1 + + private static final double MAX_RPM = 6000.0; + private DcMotorEx Shooter, Shooter2; + + private ElapsedTime timeretat = new ElapsedTime(); + private double currentTargetVel = 0.0; + + @Override + public void init () { + + double maxVelRPM = 4700.0; + double maxVel = (maxVelRPM * TICKS_PER_REV_6000) / 60.0; + double kF = 1; + double kP = 6.0; // Agressif car systeme rapide + double kI = 0.0; // nuisible voir inutile avec shooter + double kD = 0.4; // amortissement raisonnable + + Shooter = hardwareMap.get(DcMotorEx.class, "Shooter"); + Shooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Shooter.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + //Shooter.setVelocityPIDFCoefficients(kP, kI, kD, kF); + + //Test avec deux moteurs + Shooter2 = hardwareMap.get(DcMotorEx.class, "Shooter2"); + Shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Shooter2.setDirection(DcMotor.Direction.REVERSE); + Shooter2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + //Shooter2.setVelocityPIDFCoefficients(kP, kI, kD, kF); + } + + + public void setshooterFullPower ( double Shootpower){ + // mettre le moteur a fond pour lancer de balle sans controle + Shooter.setPower(Shootpower); + Shooter2.setPower(Shootpower); + } + + public void setShooterTargetRPM ( double targetRPM){ + + //Conversion RPM -> ticks/sec + double targetTicksPerSec = (targetRPM * TICKS_PER_REV_6000) / 60.0; + Shooter.setVelocity(targetTicksPerSec); + Shooter2.setVelocity(targetTicksPerSec); + + } + + + public double getShooter2VelocityRPM () { + double ticksPerSec = Shooter2.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_6000; + } + + // Méthode lire la vitesse du moteur de lancement Balle shooter et affichage + public double getShooterVelocityRPM () { + double ticksPerSec = Shooter.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_6000; + } + @Override + public void loop () { + + setShooterTargetRPM(4000); + + telemetry.addData("vitesse", getShooterVelocityRPM()); + telemetry.addData("vitesseshoot2",getShooter2VelocityRPM()); + + } + } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestAngleShootTourelle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestAngleShootTourelle.java new file mode 100644 index 000000000000..0c6601a7925a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestAngleShootTourelle.java @@ -0,0 +1,120 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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; + +@TeleOp(name="Test AngleShoot", group="debug") + +public class TestAngleShootTourelle extends OpMode { + + // --- Modules --- + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter angleShooter; + private ServoTireur servoTireur; + private Indexeur indexeur; + private Intake intake; + + private AfficheurLeft afficheurLeft; + private AfficheurRight afficheurRight; + + // --- Manager --- + private TireurManager tireurManager; + + // Pour détecter un appui unique + private boolean lastX = false; + private int positionAngleshoot = 0; + double angleDemande = 0; + + @Override + + public void init() { + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + angleShooter = new AngleShooter(); + angleShooter.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + tireurManager = new TireurManager(shooter, tourelle, angleShooter, servoTireur, indexeur, intake, afficheurRight); + ; + telemetry.addLine(">>> Test Tir Auto prêt <<<"); + } + + + + @Override + public void loop() { + //tourelle.allerVersAngle(45); + double[] presets = {0.12, 0.25, 0.48, 0.62, 0.65}; + if (gamepad2.x && !lastX) { + positionAngleshoot = (positionAngleshoot + 1) % presets.length; + angleShooter.angleShoot(presets[positionAngleshoot]); } + lastX = gamepad2.x; + + // --- Déclenchement tir auto --- + //if (gamepad2.x && !lastX) { + + // Exemple : tir droit devant + //double angleTourelle = 0; // à adapter + //double angleShooter = 0; // à adapter + //double vitesseShooter = 0; // à adapter + //tireurManager.startTirAuto(angleTourelle, angleShooter, vitesseShooter); + //} + + + //double powertourelle = gamepad2.left_stick_x; + //tourelle.rotationtourelle(powertourelle); + + // --- Mise à jour du manager --- + + //intake.update(); + //indexeur.update(); + //tireurManager.update(); + + + // --- Telemetry utile --- + telemetry.addLine("=== TIR AUTO ==="); + 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.addData("erreur", tourelle.geterreur()); + telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle()); + telemetry.update(); + } + } + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestLumiere.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestLumiere.java new file mode 100644 index 000000000000..a6eb9b3205be --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestLumiere.java @@ -0,0 +1,129 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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 +@TeleOp(name="Test Lumiere", group="debug") + +public class TestLumiere extends OpMode { + + // --- Modules --- + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter angleShooter; + private ServoTireur servoTireur; + private Indexeur indexeur; + private Intake intake; + + private AfficheurLeft afficheurLeft; + private AfficheurRight afficheurRight; + + // --- Manager --- + private TireurManager tireurManager; + + // Pour détecter un appui unique + private boolean lastX = false; + private int positionAngleshoot = 0; + double angleDemande = 0; + + @Override + + public void init() { + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + angleShooter = new AngleShooter(); + angleShooter.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + tireurManager = new TireurManager(shooter, tourelle, angleShooter, servoTireur, indexeur, intake, afficheurRight); + ; + telemetry.addLine(">>> Test Tir Auto prêt <<<"); + } + + + + @Override + public void loop() { + //tourelle.allerVersAngle(45); + //double[] presets = {0.12, 0.25, 0.30, 0.40, 0.52}; + afficheurRight.setPosition(0.5); + afficheurLeft.setVert(); + afficheurLeft.update(); + + if (gamepad2.x && !lastX) { + afficheurLeft.setVert(); + + } + //positionAngleshoot = (positionAngleshoot + 1) % presets.length; + //angleShooter.angleShoot(presets[positionAngleshoot]); } + lastX = gamepad2.x; + + // --- Déclenchement tir auto --- + //if (gamepad2.x && !lastX) { + + // Exemple : tir droit devant + //double angleTourelle = 0; // à adapter + //double angleShooter = 0; // à adapter + //double vitesseShooter = 0; // à adapter + //tireurManager.startTirAuto(angleTourelle, angleShooter, vitesseShooter); + //} + + + //double powertourelle = gamepad2.left_stick_x; + //tourelle.rotationtourelle(powertourelle); + + // --- Mise à jour du manager --- + + //intake.update(); + //indexeur.update(); + //tireurManager.update(); + + + // --- Telemetry utile --- + telemetry.addLine("=== TIR AUTO ==="); + 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.addData("erreur", tourelle.geterreur()); + telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle()); + telemetry.update(); + } + } + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestServoTireur.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestServoTireur.java new file mode 100644 index 000000000000..a95fa92bee27 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestServoTireur.java @@ -0,0 +1,120 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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; + +@TeleOp(name="Test Servo Tireur", group="debug") + +public class TestServoTireur extends OpMode { + + // --- Modules --- + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter angleShooter; + private ServoTireur servoTireur; + private Indexeur indexeur; + private Intake intake; + + private AfficheurLeft afficheurLeft; + private AfficheurRight afficheurRight; + + // --- Manager --- + private TireurManager tireurManager; + + // Pour détecter un appui unique + + private int positionAngleshoot = 0; + double angleDemande = 0; + + @Override + + public void init() { + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + angleShooter = new AngleShooter(); + angleShooter.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + tireurManager = new TireurManager(shooter, tourelle, angleShooter, servoTireur, indexeur, intake, afficheurRight); + ; + telemetry.addLine(">>> Test Tir Auto prêt <<<"); + } + + + @Override + public void loop() { + //tourelle.allerVersAngle(45); + double[] presets = {0.12, 0.25, 0.30, 0.40, 0.52}; + if (gamepad2.rightBumperWasPressed()) { + servoTireur.push(); } + if (gamepad2.leftBumperWasPressed()) { + servoTireur.retract(); } + + + // --- Déclenchement tir auto --- + //if (gamepad2.x && !lastX) { + + // Exemple : tir droit devant + //double angleTourelle = 0; // à adapter + //double angleShooter = 0; // à adapter + //double vitesseShooter = 0; // à adapter + //tireurManager.startTirAuto(angleTourelle, angleShooter, vitesseShooter); + //} + + + //double powertourelle = gamepad2.left_stick_x; + //tourelle.rotationtourelle(powertourelle); + + // --- Mise à jour du manager --- + + //intake.update(); + //indexeur.update(); + //tireurManager.update(); + + + // --- Telemetry utile --- + telemetry.addLine("=== TIR AUTO ==="); + 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.addData("erreur", tourelle.geterreur()); + telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle()); + telemetry.update(); + } + } + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTirAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTirAuto.java new file mode 100644 index 000000000000..12699f5af4ba --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTirAuto.java @@ -0,0 +1,112 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AfficheurLeft; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AfficheurRight; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Indexeur; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Intake; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Shooter; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.SpinTurret; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AngleShooter; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.ServoTireur; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManager; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +@Disabled +@TeleOp(name="Test Tir Auto", group="debug") + +public class TestTirAuto extends OpMode { + + // --- Modules --- + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter angleShooter; + private ServoTireur servoTireur; + private Indexeur indexeur; + private Intake intake; + + private AfficheurLeft afficheurLeft; + private AfficheurRight afficheurRight; + + // --- Manager --- + private TireurManager tireurManager; + + // Pour détecter un appui unique + private boolean lastX = false; + + @Override + + public void init() { + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + angleShooter = new AngleShooter(); + angleShooter.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + tireurManager = new TireurManager(shooter, tourelle, angleShooter, servoTireur, indexeur, intake, afficheurRight); + ; + telemetry.addLine(">>> Test Tir Auto prêt <<<"); + } + + + + @Override + public void loop() { + + // --- Déclenchement tir auto --- + if (gamepad2.x && !lastX) { + + // Exemple : tir droit devant + double angleTourelle = 0; // à adapter + double angleShooter = 15; // à adapter + double vitesseShooter = 4800; // à adapter + + tireurManager.startTirAuto(angleTourelle, angleShooter, vitesseShooter); + } + + + lastX = gamepad2.x; + // --- Mise à jour du manager --- + + intake.update(); + indexeur.update(); + tireurManager.update(); + + + // --- Telemetry utile --- + telemetry.addLine("=== TIR AUTO ==="); + 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.addData("Nombre de balles", indexeur.getBalles()); + telemetry.update(); + } + } + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTournerTourelle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTournerTourelle.java new file mode 100644 index 000000000000..151c82a3681f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTournerTourelle.java @@ -0,0 +1,117 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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; + +@TeleOp(name="Test Tourelle", group="debug") + +public class TestTournerTourelle extends OpMode { + + // --- Modules --- + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter angleShooter; + private ServoTireur servoTireur; + private Indexeur indexeur; + private Intake intake; + private boolean turretZeroDone = false; + private long imuReadySince = 0; + + private AfficheurLeft afficheurLeft; + private AfficheurRight afficheurRight; + + // --- Manager --- + private TireurManager tireurManager; + + // Pour détecter un appui unique + private boolean lastX = false; + double angleDemande = 0; + + @Override + + public void init() { + + tourelle = new SpinTurret(); + tourelle.init(hardwareMap); + + angleShooter = new AngleShooter(); + angleShooter.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + tireurManager = new TireurManager(shooter, tourelle, angleShooter, servoTireur, indexeur, intake, afficheurRight); + ; + //telemetry.addLine(">>> Test Tir Auto prêt <<<"); + tourelle.resetImuToutelle(); + } + + + + @Override + public void loop() { + tourelle.allerVersAngle(45); + + // --- Déclenchement tir auto --- + if (gamepad2.x && !lastX) { + + // Exemple : tir droit devant + //double angleTourelle = 0; // à adapter + //double angleShooter = 0; // à adapter + //double vitesseShooter = 0; // à adapter + //tireurManager.startTirAuto(angleTourelle, angleShooter, vitesseShooter); + } + + lastX = gamepad2.x; + //double powertourelle = gamepad2.left_stick_x; + //tourelle.rotationtourelle(powertourelle); + + // --- Mise à jour du manager --- + + //intake.update(); + //indexeur.update(); + //tireurManager.update(); + + + // --- Telemetry utile --- + telemetry.addLine("=== TIR AUTO ==="); + 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.addData("erreur", tourelle.geterreur()); + telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle()); + telemetry.update(); + } + } + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecode.java new file mode 100644 index 000000000000..6f8ef4976088 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecode.java @@ -0,0 +1,300 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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.Intake; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Indexeur; + +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +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.TireurManagerTeleop; +import org.firstinspires.ftc.teamcode.pedroPathing.navigation.GlobalPoseStorage; + +import java.util.function.Supplier; + +@Disabled +@Configurable + +@TeleOp (name="TeleOp Competiton Bleu pos fixe", group="Competition") +public class TeleOpDecode extends OpMode { + private Follower follower; + public static Pose startingPose; //See ExampleAuto to understand how to use this + private boolean automatedDrive; + private Supplier pathChain; + private TelemetryManager telemetryM; + private Intake intake; + private Indexeur indexeur; + + private double targetRPM = 0.0; + + private AfficheurLeft afficheurLeft; + private TireurManagerTeleop tireurManager; + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter ServoAngleShoot; + private ServoTireur servoTireur; + + //private boolean automatedDrive = false; + + private AfficheurRight afficheurRight; + private boolean pulseSent = false; + private boolean lastrightbumper = false ; + + private boolean lastleftbumpergamepad1 = false; + + private static final double SLOW_MULT = 0.35; // vitesse par défaut (lent) + private static final double BOOST_MULT = 1.0; // plein régime quand on appuie + + + + @Override + public void init() { + + + tourelle = new SpinTurret(); + tourelle.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + ; + + follower = Constants.createFollower(hardwareMap); + + follower.setStartingPose(new Pose((55), 81, Math.toRadians(0))); + follower.update(); + + Pose p = follower.getPose(); + + telemetry.addData("POSE Pedro", p); + telemetry.update(); + + + + + //follower.setStartingPose(startingPose == null ? new Pose() : startingPose); + //follower.update(); + + // 1. Injecter l'IMU + //double imuHeading = follower.getHeading(); // ex: 0 rad + //Pose p = follower.getPose(); + //follower.setStartingPose(new Pose(p.getX(), p.getY(), imuHeading)); // -> (x_auto, y_auto, 0 rad) + //follower.update(); + + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + tireurManager = new TireurManagerTeleop(shooter, tourelle, ServoAngleShoot, servoTireur, indexeur, intake, afficheurRight); + ; + + + pathChain = () -> follower.pathBuilder() //Lazy Curve Generation + .addPath(new Path(new BezierLine(follower::getPose, new Pose(55, 81)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(0), 0.8)) + .build(); + + + telemetry.addData("Starting pose TeleOp", + "x=%.1f y=%.1f θ=%.1f°", + follower.getPose().getX(), + follower.getPose().getY(), + Math.toDegrees(follower.getPose().getHeading())); + telemetry.update(); + + } + + + @Override + public void start() { + //The parameter controls whether the Follower should use break mode on the motors (using it is recommended). + //In order to use float mode, add .useBrakeModeInTeleOp(true); to your Drivetrain Constants in Constant.java (for Mecanum) + //If you don't pass anything in, it uses the default (false) + follower.startTeleopDrive(); + + } + + + private void fireIfReady(double angle, int rpm, int shots) { + if (!tireurManager.isBusy()) { + if (shots == 1) { + tireurManager.startTirManuel1tir(angle, rpm); + afficheurRight.setJaune(); + } else { + tireurManager.startTirManuel3Tirs(angle, rpm); + afficheurRight.setClignoteVert(); + } + } else { + telemetry.addData("Tir", "IGNORÉ: tireur occupé (BUSY)"); + } + } + + @Override + public void loop() { + //Call this once per loop + follower.update(); + telemetryM.update(); + double t = getRuntime(); + // au bout de 20 secondes : impulsion unique + if (t>=20.0 && !pulseSent){ + gamepad1.rumble(500); + gamepad2.rumble(500); + pulseSent = true; + afficheurRight.setClignoteVert(); + } + + + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + + + // Lecture des sticks + double ly = -gamepad1.left_stick_y; + double lx = -gamepad1.left_stick_x; + double rx = -gamepad1.right_stick_x; + + // Deadband + if (Math.abs(ly) < 0.05) ly = 0; + if (Math.abs(lx) < 0.05) lx = 0; + if (Math.abs(rx) < 0.05) rx = 0; + + + // BOOST avec au bumper droit + boolean boost = gamepad1.right_bumper; + double mult = boost ? BOOST_MULT : SLOW_MULT; + + // Si rotation non ralentie, mettre rotMult = BOOST_MULT + double rotMult = mult; + + follower.setTeleOpDrive( + ly * mult, + lx * mult, + rx * rotMult, + false); + } + + //Automated PathFollowing + if (gamepad1.yWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + + //Stop automated following if the follower is done + if (automatedDrive && (gamepad1.bWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + if (gamepad1.left_bumper && !lastleftbumpergamepad1) { + intake.setetatEJECTION(); + } + lastleftbumpergamepad1 = gamepad1.left_bumper; + + + if (gamepad1.xWasPressed()) { + indexeur.setBalles(0); // reset des balles + // demarre l'intake automatiquement + } + + + double powertourelle = gamepad2.left_stick_x; // Joystick Horizontal + tourelle.rotationtourelle(powertourelle); + + int shotsMode = (gamepad2.left_bumper ? 1 : 3); + + // RB (g2) : position fréquente de tir & en autonome Position 4 tres éloigné + if (gamepad2.right_bumper && !lastrightbumper) { + fireIfReady(0.35, 4400, shotsMode); + } + lastrightbumper = gamepad2.right_bumper; + + // Y : Position 3 + if (gamepad2.yWasPressed()) { + fireIfReady(0.42, 4200, shotsMode);} + + // B : Position 2 proche + if (gamepad2.bWasPressed()) { + fireIfReady(0.30, 3970, shotsMode); + } + + // A : Position 1 tres proche + if (gamepad2.aWasPressed()) { + fireIfReady(0.17, 3750, shotsMode); + } + + // X : collé au mur 1 + if (gamepad2.xWasPressed()) { + fireIfReady(0.1, 3700, shotsMode); + } + // Pad tir de loin + if (gamepad2.dpadUpWasPressed()){ + fireIfReady(0.55, 4850, shotsMode); + } + + //tir tres eloigné. + if (gamepad2.dpadDownWasPressed()){ + fireIfReady(0.55, 4950, shotsMode); + } + + if (gamepad2.dpadRightWasPressed()){ + tireurManager.setState(TireurManagerTeleop.TirState.IDLE); + } + + intake.update(); + indexeur.update(); + tireurManager.update(); + afficheurRight.update(); + afficheurLeft.update(); + + //telemetryM.debug("position", follower.getPose()); + //telemetryM.debug("velocity", follower.getVelocity()); + //telemetryM.debug("automatedDrive", automatedDrive); + //telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle()); + //telemetry.addData("AngleShoot", positionAngleshoot); + //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()); + //for (int i = 0; i < 3; i++) { telemetry.addData("Compartiment " + i, indexeur.getCouleurCompartiment(i)); } + //telemetry.addData("État tireur manager", tireurManager.getState()); + //telemetry.addData("Shooter RPM", shooter.getShooterVelocityRPM()); + //telemetry.addData("Index rotation finie", indexeur.isRotationTerminee()); + + telemetry.update(); + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecodeBleuCamera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecodeBleuCamera.java new file mode 100644 index 000000000000..678e267c5851 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecodeBleuCamera.java @@ -0,0 +1,494 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.pedroPathing.navigation.GlobalPoseStorage; + +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; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManagerTeleop; +import org.firstinspires.ftc.teamcode.pedroPathing.navigation.Camerahusky; +import org.firstinspires.ftc.teamcode.pedroPathing.navigation.CameraLimelight; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.ShooterCalculator; +import java.util.function.Supplier; + +@Configurable +@TeleOp (name="TeleOp - BLUE Bleu Camera", group="Competition") +public class TeleOpDecodeBleuCamera extends OpMode { + //Camerahusky Camera = new Camerahusky(); + + private double ajustementcamera = 1.5; //valeur d'ajustement caméra (erreur) a changer si probleme + + private Follower follower; + private boolean automatedDrive; + private Supplier pathChain; + private TelemetryManager telemetryM; + private Intake intake; + private Indexeur indexeur; + + private double targetRPM = 0.0; + + private AfficheurLeft afficheurLeft; + + private CameraLimelight Camera; + private TireurManagerTeleop tireurManager; + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter ServoAngleShoot; + private double lastAngleCamera = -1; + private ServoTireur servoTireur; + + private boolean homingHoldActive_g1 = false; + private long homingHoldStartMs_g1 = 0; + private static final long HOMING_HOLD_MS = 500; // 0,5 s + private boolean homingWaitRelease_g1 = false; // empêcher un nouveau homing tant que non relâché + + + //private boolean automatedDrive = false; + + private AfficheurRight afficheurRight; + private boolean pulseSent = false; + private boolean lastrightbumper = false ; + + private boolean lastleftbumpergamepad1 = false; + + private double distanceCamerajustee = 0.0 ; + private double distanceCameramesuree = 0.0 ; + private boolean lastLT = false; + + private boolean turretZeroDone = false; + private long imuReadySince = 0; + + public static Pose startingPose; + private ShooterCalculator shooterCalc; + private double angleAuto = 0.0; + + + private static final double SLOW_MULT = 0.35; // vitesse par défaut (lent) + private static final double BOOST_MULT = 1.0; // plein régime quand on appuie + + @Override + public void init() { + + + angleAuto = GlobalPoseStorage.turretAngleDeg; + + // Garde-fous + if (Double.isNaN(angleAuto) || Math.abs(angleAuto) > 360) { + angleAuto = 0.0; // fallback sûr si Auto n’a pas écrit + } + + telemetry.addData("Turret angle (auto→teleop)", angleAuto); + + Camera = new CameraLimelight(); + Camera.init(hardwareMap); + shooterCalc = new ShooterCalculator(); + + tourelle = new SpinTurret(); + tourelle.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + ; + //Camera.init(hardwareMap); + //if (!Camera.isConnected()){ + // telemetry.addData(">>>", "probleme communication caméra");} + //else { + // telemetry.addData(">>>","HuskyLens Prêt"); + //} + follower = Constants.createFollower(hardwareMap); + + follower.setStartingPose(new Pose((55), 81, Math.toRadians(0))); + follower.update(); + + + Pose p = follower.getPose(); + + telemetry.addData("POSE Pedro", p); + + //follower.setStartingPose(startingPose == null ? new Pose() : startingPose); + //follower.update(); + + // 1. Injecter l'IMU + //double imuHeading = follower.getHeading(); // ex: 0 rad + //Pose p = follower.getPose(); + //follower.setStartingPose(new Pose(p.getX(), p.getY(), imuHeading)); // -> (x_auto, y_auto, 0 rad) + //follower.update(); + + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + tireurManager = new TireurManagerTeleop(shooter, tourelle, ServoAngleShoot, servoTireur, indexeur, intake, afficheurRight); + ; + + + pathChain = () -> follower.pathBuilder() //Lazy Curve Generation + .addPath(new Path(new BezierLine(follower::getPose, new Pose(55, 81)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(0), 0.8)) + .build(); + + + telemetry.addData("Starting pose TeleOp", + "x=%.1f y=%.1f θ=%.1f°", + follower.getPose().getX(), + follower.getPose().getY(), + Math.toDegrees(follower.getPose().getHeading())); + telemetry.update(); + + } + + + @Override + public void start() { + //The parameter controls whether the Follower should use break mode on the motors (using it is recommended). + //In order to use float mode, add .useBrakeModeInTeleOp(true); to your Drivetrain Constants in Constant.java (for Mecanum) + //If you don't pass anything in, it uses the default (false) + follower.startTeleopDrive(); + Camera.start(); + + + } + + + private void fireIfReady(double angle, int rpm, int shots) { + if (!tireurManager.isBusy()) { + if (shots == 1) { + tireurManager.startTirManuel1tir(angle, rpm); + afficheurRight.setJaune(); + } else { + tireurManager.startTirManuel3Tirs(angle, rpm); + afficheurRight.setClignoteVert(); + } + } else { + telemetry.addData("Tir", "IGNORÉ: tireur occupé (BUSY)"); + } + } + + private void firefondTerrainAuto(double angletourelle, double angleshooter, int rpm) { + if (!tireurManager.isBusy()) { + tireurManager.startTirAuto(angletourelle,angleshooter,rpm); + } + else { + telemetry.addData("Tir", "IGNORÉ: tireur occupé (BUSY)");} + } + + @Override + public void loop() { + //Call this once per loop + follower.update(); + telemetryM.update(); + + double t = getRuntime(); + // au bout de 20 secondes : impulsion unique + if (t>=20.0 && !pulseSent){ + gamepad1.rumble(500); + gamepad2.rumble(500); + pulseSent = true; + } + + + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + + + // Lecture des sticks + double ly = -gamepad1.left_stick_y; + double lx = -gamepad1.left_stick_x; + double rx = -gamepad1.right_stick_x; + + // Deadband + if (Math.abs(ly) < 0.05) ly = 0; + if (Math.abs(lx) < 0.05) lx = 0; + if (Math.abs(rx) < 0.05) rx = 0; + + + // BOOST avec au bumper droit + boolean boost = gamepad1.right_bumper; + double mult = boost ? BOOST_MULT : SLOW_MULT; + + // Si rotation non ralentie, mettre rotMult = BOOST_MULT + double rotMult = mult; + + follower.setTeleOpDrive( + ly * mult, + lx * mult, + rx * rotMult, + false); + } + + //Automated PathFollowing + if (gamepad1.yWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + + //Stop automated following if the follower is done + if (automatedDrive && (gamepad1.bWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + // === HOMING PILOTE sécurisé : D-pad Gauche (g1) maintenu 0,5 s === + boolean dpadLeftHeld_g1 = gamepad1.dpad_left; // état maintenu du D-pad gauche + +// Si on attend un relâchement, ignorer tant que le bouton est encore appuyé + if (homingWaitRelease_g1) { // << AJOUT + if (!dpadLeftHeld_g1) { // << AJOUT + homingWaitRelease_g1 = false; // << AJOUT + } + } else { // << AJOUT + // Début de maintien + if (dpadLeftHeld_g1 && !homingHoldActive_g1) { + homingHoldActive_g1 = true; + homingHoldStartMs_g1 = System.currentTimeMillis(); + } + + // Reset si relâché + if (!dpadLeftHeld_g1) { + homingHoldActive_g1 = false; + } + + // Si en maintien, vérifier la durée + if (homingHoldActive_g1) { + long held = System.currentTimeMillis() - homingHoldStartMs_g1; + telemetry.addData("Homing (g1)", "Maintiens D-pad Gauche %d / %d ms", held, HOMING_HOLD_MS); + + if (held >= HOMING_HOLD_MS) { + // Sécurités : couper ce qui peut gêner le homing + tireurManager.cancelTir(); + intake.arretPourTir(); + + // Lancer le homing + indexeur.lancerHoming(); + afficheurRight.setBleu(); + afficheurLeft.setBleu(); + telemetry.addData("Indexeur", "HOMING lancé (g1 D-pad Gauche, maintien)"); + + // Anti-spam : attendre relâchement avant de pouvoir relancer + homingHoldActive_g1 = false; + homingWaitRelease_g1 = true; // << AJOUT + } + } + } // << AJOUT + + if (gamepad1.left_bumper && !lastleftbumpergamepad1) { + intake.setetatEJECTION(); + } + lastleftbumpergamepad1 = gamepad1.left_bumper; + + if (gamepad1.xWasPressed()) { + indexeur.setBalles(0); + intake.setetatramasage(); // demarre l'intake automatiquement + } + + if (gamepad1.dpadRightWasPressed()){ + indexeur.reculerIndexeurbourrage(); + } + boolean tirautoactif = tireurManager.isTirAutoActif(); + if (!tirautoactif) { + double powertourelle = gamepad2.left_stick_x; // Joystick Horizontal + tourelle.rotationtourelle(powertourelle); + } + + // Deadzone pour considérer que le pilote "veut" bouger la tourelle + //final double TURRET_DEADZONE = 0.02; + //double powertourelle = gamepad2.left_stick_x; + // Si le pilote bouge vraiment le stick -> priorité au manuel pour CETTE boucle + //if (Math.abs(powertourelle) > TURRET_DEADZONE) { + // tourelle.rotationtourelle(powertourelle); + //} + + int shotsMode = (gamepad2.left_bumper ? 1 : 3); + + // RB (g2) : position fréquente de tir & en autonome Position 4 tres éloigné + //if (gamepad2.right_bumper && !lastrightbumper) { + // fireIfReady(0.35, 4400, shotsMode); + // } + // lastrightbumper = gamepad2.right_bumper; + + // Y : Position 1 distance 1 robot de la zone de tir + if (gamepad2.yWasPressed()) { + + fireIfReady(0.30, 3970, shotsMode); + } + + // B : Position 3 milieu de terrain bouton 2 droite + if (gamepad2.bWasPressed()) { + fireIfReady(0.42, 4200, shotsMode); + } + + + // A : Position 4 la plus loin bouton 1 le plus proche co driver + if (gamepad2.aWasPressed()) { + //fireIfReady(0.17, 3750, shotsMode); + fireIfReady(0.47, 4400, shotsMode); + } + + // X : Bouton Gauche Boutton 0 reset de l'IMU de la tourelle + if (gamepad2.xWasPressed()) { + //fireIfReady(0.1, 3700, shotsMode); + tourelle.resetImuToutelle(); + afficheurLeft.setViolet(); + } + // Pad tir de loin + if (gamepad2.dpadUpWasPressed()){ + fireIfReady(0.56, 4780, shotsMode); + } + + + //tir tres eloigné. + if (gamepad2.dpadDownWasPressed()){ + fireIfReady(0.56, 4750, shotsMode); + } + + if (gamepad2.dpadLeftWasPressed()){ + tirautoactif = true; + double angletourelleajustee = -55 - angleAuto; + firefondTerrainAuto(angletourelleajustee,0.58,4780); + } + + boolean ltPressed = gamepad2.left_trigger > 0.6; // seuil pour éviter bruit + boolean prespinOn = tireurManager.isPrespinActif(); + + if (ltPressed && !lastLT) { // front montant + if (tireurManager.getState() == TireurManagerTeleop.TirState.IDLE) { + + if (!prespinOn) { + // PRESHOOTER ON — 3500 rpm + tireurManager.prespinShooter(3500); + afficheurLeft.setViolet(); // optionnel + } else { + // PRESHOOTER OFF + tireurManager.stopPrespin(); + afficheurLeft.setIdle(); // optionnel + } + } + } + lastLT = ltPressed; + + + if (gamepad2.dpadRightWasPressed()){ + tireurManager.cancelTir(); + + } + + if (Camera.hasTag() == true){ + afficheurRight.setBleu(); + distanceCamerajustee = Camera.getDistanceFiableCm(); + distanceCameramesuree = Camera.getDistanceFiableCm(); + //telemetry.addData("Distancemesurée", distanceCameramesuree); + telemetry.addData("Distance_camera_apriltag", distanceCamerajustee); + telemetry.update(); + //if (distanceCamerajustee > 0 && distanceCamerajustee < 70) { + // afficheurRight.setVert(); + //} + //if (distanceCamerajustee > 68 && distanceCamerajustee < 110) { + // afficheurRight.setJaune(); + + //} + + //if (distanceCamerajustee > 110 && distanceCamerajustee < 150) { + // afficheurRight.setOrange(); + + //} + //if (distanceCamerajustee > 150 && distanceCamerajustee < 187) { + // afficheurRight.setRouge(); + //} + //if (distanceCamerajustee > 187 && distanceCamerajustee < 200) { + // afficheurRight.setViolet(); + //} + afficheurRight.update(); + distanceCamerajustee = 0; + distanceCameramesuree =0; + } + + if(!Camera.hasTag() == true){ + afficheurRight.setIdle(); + afficheurRight.update(); + } + + // Tir automatique caméra + + if (gamepad2.rightBumperWasPressed()) { + + if (!Camera.hasTag()) { + afficheurRight.setRouge(); + afficheurRight.update(); + return; // on ne tire pas + } + + // Ici on est sûr qu'il y a un tag + double distanceCm = Camera.getDistanceFiableCm(); + ShooterCalculator.ShooterResult r = shooterCalc.compute(distanceCm); + lastAngleCamera = r.angle; + fireIfReady(r.angle, r.rpm, shotsMode); + } + + intake.update(); + indexeur.update(); + Double tx = Camera.getTx(); // peut être null + boolean hasTag = Camera.hasTag(); // bool + tireurManager.updateCameraData(tx, hasTag); + tireurManager.update(); + afficheurRight.update(); + afficheurLeft.update(); + + + telemetry.addData("ShotsMode", shotsMode); + telemetry.addData("Angle envoyé Angleshoot", lastAngleCamera); + //telemetryM.debug("position", follower.getPose()); + //telemetryM.debug("velocity", follower.getVelocity()); + //telemetryM.debug("automatedDrive", automatedDrive); + //telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle()); + //telemetry.addData("AngleShoot", positionAngleshoot); + //telemetry.addData("RPM intake", 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()); + //for (int i = 0; i < 3; i++) { telemetry.addData("Compartiment " + i, indexeur.getCouleurCompartiment(i)); } + //telemetry.addData("État tireur manager", tireurManager.getState()); + telemetry.addData("Shooter RPM", shooter.getShooterVelocityRPM()); + //telemetry.addData("Index rotation finie", indexeur.isRotationTerminee()); + + telemetry.update(); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecodeRedCamera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecodeRedCamera.java new file mode 100644 index 000000000000..9bdc850aef7e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecodeRedCamera.java @@ -0,0 +1,444 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp; + +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.HeadingInterpolator; +import com.pedropathing.paths.Path; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +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.TireurManagerTeleop; +import org.firstinspires.ftc.teamcode.pedroPathing.navigation.Camerahusky; + +import java.util.function.Supplier; + +@Configurable +@TeleOp (name="TeleOp -- RED ROUGE -- Camera", group="Competition") +public class TeleOpDecodeRedCamera extends OpMode { + Camerahusky Camera = new Camerahusky(); + private double ajustementcamera = 1.5; //valeur d'ajustement caméra (erreur) a changer si probleme + + private Follower follower; + public static Pose startingPose; //See ExampleAuto to understand how to use this + private boolean automatedDrive; + private Supplier pathChain; + private TelemetryManager telemetryM; + private Intake intake; + private Indexeur indexeur; + + private double targetRPM = 0.0; + + private AfficheurLeft afficheurLeft; + private TireurManagerTeleop tireurManager; + private Shooter shooter; + private SpinTurret tourelle; + private AngleShooter ServoAngleShoot; + private ServoTireur servoTireur; + + //private boolean automatedDrive = false; + + private AfficheurRight afficheurRight; + private boolean pulseSent = false; + private boolean lastrightbumper = false ; + + private boolean lastleftbumpergamepad1 = false; + + private double distanceCamerajustee = 0.0 ; + private double distanceCameramesuree = 0.0 ; + + private boolean turretZeroDone = false; + private long imuReadySince = 0; + private boolean lastLT = false; + + + private static final double SLOW_MULT = 0.35; // vitesse par défaut (lent) + private static final double BOOST_MULT = 1.0; // plein régime quand on appuie + + @Override + public void init() { + + + tourelle = new SpinTurret(); + tourelle.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); + + shooter = new Shooter(); + shooter.setIndexeur(indexeur); // ✔️ on passe l’indexeur + shooter.init(hardwareMap); // ✔️ on initialise le hardware + + + servoTireur = new ServoTireur(indexeur); // ✔️ constructeur correct + servoTireur.init(hardwareMap); // ✔️ initialisation du servo + ; + Camera.init(hardwareMap); + if (!Camera.isConnected()){ + telemetry.addData(">>>", "probleme communication caméra");} + else { + telemetry.addData(">>>","HuskyLens Prêt"); + } + follower = Constants.createFollower(hardwareMap); + + follower.setStartingPose(new Pose((55), 81, Math.toRadians(0))); + follower.update(); + + Pose p = follower.getPose(); + + telemetry.addData("POSE Pedro", p); + + //follower.setStartingPose(startingPose == null ? new Pose() : startingPose); + //follower.update(); + + // 1. Injecter l'IMU + //double imuHeading = follower.getHeading(); // ex: 0 rad + //Pose p = follower.getPose(); + //follower.setStartingPose(new Pose(p.getX(), p.getY(), imuHeading)); // -> (x_auto, y_auto, 0 rad) + //follower.update(); + + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + tireurManager = new TireurManagerTeleop(shooter, tourelle, ServoAngleShoot, servoTireur, indexeur, intake, afficheurRight); + ; + + + pathChain = () -> follower.pathBuilder() //Lazy Curve Generation + .addPath(new Path(new BezierLine(follower::getPose, new Pose(55, 81)))) + .setHeadingInterpolation(HeadingInterpolator.linearFromPoint(follower::getHeading, Math.toRadians(0), 0.8)) + .build(); + + + telemetry.addData("Starting pose TeleOp", + "x=%.1f y=%.1f θ=%.1f°", + follower.getPose().getX(), + follower.getPose().getY(), + Math.toDegrees(follower.getPose().getHeading())); + telemetry.update(); + + } + + + @Override + public void start() { + //The parameter controls whether the Follower should use break mode on the motors (using it is recommended). + //In order to use float mode, add .useBrakeModeInTeleOp(true); to your Drivetrain Constants in Constant.java (for Mecanum) + //If you don't pass anything in, it uses the default (false) + follower.startTeleopDrive(); + + } + + + private void fireIfReady(double angle, int rpm, int shots) { + if (!tireurManager.isBusy()) { + if (shots == 1) { + tireurManager.startTirManuel1tir(angle, rpm); + afficheurRight.setJaune(); + } else { + tireurManager.startTirManuel3Tirs(angle, rpm); + afficheurRight.setClignoteVert(); + } + } else { + telemetry.addData("Tir", "IGNORÉ: tireur occupé (BUSY)"); + } + } + + private void firefondTerrainAuto(double angletourelle, double angleshooter, int rpm) { + if (!tireurManager.isBusy()) { + tireurManager.startTirAuto(angletourelle,angleshooter,rpm); + } + else { + telemetry.addData("Tir", "IGNORÉ: tireur occupé (BUSY)");} + } + + @Override + public void loop() { + //Call this once per loop + follower.update(); + telemetryM.update(); + + double t = getRuntime(); + // au bout de 20 secondes : impulsion unique + if (t>=20.0 && !pulseSent){ + gamepad1.rumble(500); + gamepad2.rumble(500); + pulseSent = true; + } + + + if (!automatedDrive) { + //Make the last parameter false for field-centric + //In case the drivers want to use a "slowMode" you can scale the vectors + + + // Lecture des sticks + double ly = -gamepad1.left_stick_y; + double lx = -gamepad1.left_stick_x; + double rx = -gamepad1.right_stick_x; + + // Deadband + if (Math.abs(ly) < 0.05) ly = 0; + if (Math.abs(lx) < 0.05) lx = 0; + if (Math.abs(rx) < 0.05) rx = 0; + + + // BOOST avec au bumper droit + boolean boost = gamepad1.right_bumper; + double mult = boost ? BOOST_MULT : SLOW_MULT; + + // Si rotation non ralentie, mettre rotMult = BOOST_MULT + double rotMult = mult; + + follower.setTeleOpDrive( + ly * mult, + lx * mult, + rx * rotMult, + false); + } + + //Automated PathFollowing + if (gamepad1.yWasPressed()) { + follower.followPath(pathChain.get()); + automatedDrive = true; + } + + //Stop automated following if the follower is done + if (automatedDrive && (gamepad1.bWasPressed() || !follower.isBusy())) { + follower.startTeleopDrive(); + automatedDrive = false; + } + + if (gamepad1.left_bumper && !lastleftbumpergamepad1) { + intake.setetatEJECTION(); + } + lastleftbumpergamepad1 = gamepad1.left_bumper; + + if (gamepad1.xWasPressed()) { + indexeur.setBalles(0); // reset des balles + // demarre l'intake automatiquement + } + + if (gamepad1.dpadRightWasPressed()){ + indexeur.reculerIndexeurbourrage(); + } + boolean tirautoactif = tireurManager.isTirAutoActif(); + if (!tirautoactif) { + double powertourelle = gamepad2.left_stick_x; // Joystick Horizontal + tourelle.rotationtourelle(powertourelle); + } + + // Deadzone pour considérer que le pilote "veut" bouger la tourelle + //final double TURRET_DEADZONE = 0.02; + //double powertourelle = gamepad2.left_stick_x; + // Si le pilote bouge vraiment le stick -> priorité au manuel pour CETTE boucle + //if (Math.abs(powertourelle) > TURRET_DEADZONE) { + // tourelle.rotationtourelle(powertourelle); + //} + + int shotsMode = (gamepad2.left_bumper ? 1 : 3); + + // RB (g2) : position fréquente de tir & en autonome Position 4 tres éloigné + //if (gamepad2.right_bumper && !lastrightbumper) { + // fireIfReady(0.35, 4400, shotsMode); + // } + // lastrightbumper = gamepad2.right_bumper; + + // Y : Position 1 distance 1 robot de la zone de tir + if (gamepad2.yWasPressed()) { + + fireIfReady(0.30, 3970, shotsMode); + } + + // B : Position 3 milieu de terrain bouton 2 droite + if (gamepad2.bWasPressed()) { + fireIfReady(0.42, 4200, shotsMode); + } + + + // A : Position 4 la plus loin bouton 1 le plus proche co driver + if (gamepad2.aWasPressed()) { + //fireIfReady(0.17, 3750, shotsMode); + fireIfReady(0.47, 4400, shotsMode); + } + + // X : Bouton Gauche Boutton 0 reset de l'IMU de la tourelle + if (gamepad2.xWasPressed()) { + //fireIfReady(0.1, 3700, shotsMode); + tourelle.resetImuToutelle(); + afficheurLeft.setViolet(); + } + // Pad tir de loin + if (gamepad2.dpadUpWasPressed()){ + fireIfReady(0.56, 4850, shotsMode); + } + + //tir tres eloigné. + if (gamepad2.dpadDownWasPressed()){ + fireIfReady(0.56, 4950, shotsMode); + } + + if (gamepad2.dpadLeftWasPressed()){ + tirautoactif = true; + firefondTerrainAuto(55.0,0.58,4820); + } + + + if (gamepad2.dpadRightWasPressed()){ + tireurManager.cancelTir(); + } + + boolean ltPressed = gamepad2.left_trigger > 0.6; // seuil pour éviter bruit + boolean prespinOn = tireurManager.isPrespinActif(); + + if (ltPressed && !lastLT) { // front montant + if (tireurManager.getState() == TireurManagerTeleop.TirState.IDLE) { + + if (!prespinOn) { + // PRESHOOTER ON — 3500 rpm + tireurManager.prespinShooter(3500); + afficheurLeft.setViolet(); // optionnel + } else { + // PRESHOOTER OFF + tireurManager.stopPrespin(); + afficheurLeft.setIdle(); // optionnel + } + } + } + lastLT = ltPressed; + + + if (Camera.hasTag() == true){ + afficheurRight.setBleu(); + distanceCamerajustee = Camera.getDistanceFiableCm(); + distanceCameramesuree = Camera.getEstimatedDistanceCm(); + telemetry.addData("Distancemesurée", distanceCameramesuree); + telemetry.addData("Distance_camera_apriltag", distanceCamerajustee); + telemetry.update(); + //if (distanceCamerajustee > 0 && distanceCamerajustee < 70) { + // afficheurRight.setVert(); + //} + //if (distanceCamerajustee > 68 && distanceCamerajustee < 110) { + // afficheurRight.setJaune(); + + //} + + //if (distanceCamerajustee > 110 && distanceCamerajustee < 150) { + // afficheurRight.setOrange(); + + //} + //if (distanceCamerajustee > 150 && distanceCamerajustee < 187) { + // afficheurRight.setRouge(); + //} + //if (distanceCamerajustee > 187 && distanceCamerajustee < 200) { + // afficheurRight.setViolet(); + //} + afficheurRight.update(); + distanceCamerajustee = 0; + distanceCameramesuree =0; + } + + if(!Camera.hasTag() == true){ + afficheurRight.setIdle(); + afficheurRight.update(); + } + + // Tir automatique caméra + if (gamepad2.rightBumperWasPressed()) { + distanceCamerajustee = 0; + + + if (Camera.hasTag() == true) { + distanceCamerajustee = (Camera.getDistanceFiableCm())/100; + double rpmDouble = 551.62 * distanceCamerajustee + 3210.34;//ajout 20 + int rpm = (int) Math.round(rpmDouble); + + + telemetry.addData("RPM dynamique", rpm); + telemetry.update(); + + if (distanceCamerajustee > 0 && distanceCamerajustee < 0.68) { + fireIfReady(0.1,rpm, shotsMode); //3700 + distanceCamerajustee = 0; + distanceCameramesuree =0; + rpm = 0; + + } + if (distanceCamerajustee > 0.68 && distanceCamerajustee < 1.10) { + fireIfReady(0.22, rpm, shotsMode); //3750 + distanceCamerajustee = 0; + distanceCameramesuree =0; + rpm = 0; + } + + if (distanceCamerajustee > 1.10 && distanceCamerajustee < 1.50) { + fireIfReady(0.38, rpm, shotsMode); //3900 + distanceCamerajustee = 0; + distanceCameramesuree =0; + rpm = 0; + + } + + if (distanceCamerajustee > 1.50 && distanceCamerajustee < 2.50) { + fireIfReady(0.49, rpm, shotsMode); //4200 + distanceCamerajustee = 0; + distanceCameramesuree =0; + rpm =0; + } + + } + else {afficheurRight.setRouge(); + afficheurRight.update();} + + } + + + intake.update(); + indexeur.update(); + tireurManager.update(); + afficheurRight.update(); + afficheurLeft.update(); + + //telemetryM.debug("position", follower.getPose()); + //telemetryM.debug("velocity", follower.getVelocity()); + //telemetryM.debug("automatedDrive", automatedDrive); + //telemetry.addData("angle Tourelle actuel", tourelle.lectureangletourelle()); + //telemetry.addData("AngleShoot", positionAngleshoot); + //telemetry.addData("RPM intake", 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()); + //for (int i = 0; i < 3; i++) { telemetry.addData("Compartiment " + i, indexeur.getCouleurCompartiment(i)); } + //telemetry.addData("État tireur manager", tireurManager.getState()); + telemetry.addData("Shooter RPM", shooter.getShooterVelocityRPM()); + //telemetry.addData("Index rotation finie", indexeur.isRotationTerminee()); + + telemetry.update(); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/ShooterCalculator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/ShooterCalculator.java new file mode 100644 index 000000000000..6408bbdbfa8a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/ShooterCalculator.java @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.logique; + +public class ShooterCalculator { + + /** Conteneur du résultat du calcul de tir */ + public static class ShooterResult { + public boolean shouldShoot = false; + /** Angle du volet (unité servo 0.0–1.0) */ + public double angle = 0.0; + /** Vitesse du shooter (RPM) */ + public int rpm = 0; + } + + /** + * Calcule l’angle du volet et le RPM à partir de la distance caméra. + * @param distanceCm distance au tag en centimètres (doit être > 0) + * @return ShooterResult (shouldShoot=false si distance invalide) + */ + public ShooterResult compute(double distanceCm) { + ShooterResult r = new ShooterResult(); + + if (distanceCm <= 0) { + // distance invalide : on ne tire pas + return r; + } + + double distanceM = distanceCm / 100.0; + + // 1) RPM proportionnel (à ajuster selon ton robot si besoin) + r.rpm = (int) Math.round(551.62 * distanceM + 3210.34); + + // 2) ANGLE (volet) + // Base + double angleBase = 0.30 * distanceM; + + // Correction 1 : +0.05 entre 1.10 m et 1.75 m + double bonus = 0.0; + if (distanceM >= 1.10 && distanceM <= 1.65) { + bonus = 0.05; + } + + // Correction 2 : réduction progressive entre 1.75 m et 2.00 m (jusqu’à -0.05) + double reduction = 0.0; + if (distanceM > 1.65) { + // (distanceM - 1.75) varie de 0 à 0.25 -> interpolation linéaire vers 0.05 + reduction = 0.05; + } + if (distanceM > 2.00) { + // (distanceM - 1.75) varie de 0 à 0.25 -> interpolation linéaire vers 0.05 + reduction = 0.00; + } + + double angle = angleBase + bonus - reduction; + + + // Clamp mécanique (sécurité servo) + if (angle < 0.10) angle = 0.10; + if (angle > 0.62) angle = 0.62; + + r.angle = angle; + r.shouldShoot = true; + return r; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManager.java new file mode 100644 index 000000000000..437fde0b9f3a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManager.java @@ -0,0 +1,381 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.logique; + +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Shooter; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.SpinTurret; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AngleShooter; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Indexeur; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.ServoTireur; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Intake; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AfficheurRight; + +public class TireurManager { + + // --- Modules contrôlés --- + private final Shooter shooter; + private static final int TOL_STRICT_TICKS = 50; + boolean tirDeclenche = false; + private boolean tirEnCours = false; + private final SpinTurret tourelle; + private final AngleShooter ServoAngleShoot; + private final ServoTireur servoTireur; + private final Indexeur indexeur; + private final Intake intake; + + private double toleranceVelocityMax; + private double toleranceVelocityMin; + + private final AfficheurRight afficheurRight; + + // --- Machine à états --- + public enum TirState { + IDLE, + SHOOTER_SPINUP, + SPINUP_CONTROL, + TURRET_POSITION, + ANGLE_POSITION, + SERVO_PUSH, + SERVO_RETRACT, + INDEX_ADVANCE, + AFTERWAIT_INDEX, + WAIT_AFTER_INDEX + } + + private TirState state = TirState.IDLE; + private final ElapsedTime timer = new ElapsedTime(); + private int tirsEffectues = 0; + + private int shotsRemaining = 0; + + private double Min_shooterRPM = 4000; + private double TargetFlyWheelRPM = 4700; + + private double shootermaxspintime = 2; + + // --- Cibles dynamiques --- + private double angleCibleTourelle = 0; + private boolean kickActiveShooter = false; + private final ElapsedTime kickTimerShooter = new ElapsedTime(); + private static final double KICK_MIN_RPM = 900; + private static final double KICK_TIME_MS = 80; + private double angleCibleShooter = 0; + private double vitesseCibleShooter = 0; + private double rampedShooterRPM = 0; + private boolean prespinActif = false; + private double prespinRPM = 0; + + private final ElapsedTime dtShooter = new ElapsedTime(); + + public TireurManager(Shooter shooter, + SpinTurret tourelle, + AngleShooter ServoAngleShoot, + ServoTireur servoTireur, + Indexeur indexeur, Intake intake, AfficheurRight afficheurRight) { + + this.shooter = shooter; + this.tourelle = tourelle; + this.ServoAngleShoot = ServoAngleShoot; + this.servoTireur = servoTireur; + this.indexeur = indexeur; + this.intake = intake; + this.afficheurRight = afficheurRight; + + dtShooter.reset(); + kickActiveShooter = false; + rampedShooterRPM = 0; + } + + private void checkIndexeurCoherence() { + if (!indexeur.isHomingDone()) return; // pas de check tant que non calé + if (indexeur.isindexeurBusy()) return; // laisse finir la rotation + + int err = indexeur.getPositionErreurTicks(); + if (err > TOL_STRICT_TICKS) { + indexeur.microCorrigerVersCible(0.25); // correction douce vers cible planifiée + } + } + + public void update() { + + boolean tirActif = (state != TirState.IDLE); + if (tirActif) { + afficheurRight.setClignoteVert(); + } else { + afficheurRight.setIdle(); + } + + afficheurRight.update(); + + switch (state) { + + case IDLE: + if (prespinActif) { + shooter.setShooterTargetRPM(rampShooterRPM(prespinRPM)); + } else { + shooter.setShooterTargetRPM(0); + } + break; + + // --- 1) Shooter spin-up --- + case SHOOTER_SPINUP: + intake.arretPourTir(); + if (shotsRemaining > 0) { + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + intake.disableRamassage(); + //tourelle.allerVersAngle(angleCibleTourelle); + if (!indexeur.isHomingDone()) { + indexeur.lancerHoming(); + return; + } + if (indexeur.isHomingDone()) { + state = TirState.ANGLE_POSITION; + timer.reset(); + } + } else { + state = TirState.IDLE; + shooter.setShooterTargetRPM(0); + } + break; + + case ANGLE_POSITION: + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + ServoAngleShoot.setAngle(angleCibleShooter); + + // 1) Indexeur pas prêt → correction → rester dans ANGLE_POSITION + if (!indexeur.isHomingDone()) { + indexeur.lancerHoming(); + return; + } + if (!indexeur.indexeurPretPourTir()) { + indexeur.microCorrigerVersCible(0.20); + return; // rester tant que l'indexeur n'est pas calé + } + + // 2) Volet pas encore calé → attendre ici + if (!ServoAngleShoot.isAtAngle(angleCibleShooter)) { + return; + } + + // 3) Les deux sont prêts → on peut avancer + timer.reset(); + state = TirState.SPINUP_CONTROL; + break; + + // --- 3) Contrôle de la montée en régime --- + case SPINUP_CONTROL: + rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + + if (vitesseCibleShooter < 4000) { + toleranceVelocityMin = 0.94 * vitesseCibleShooter; + toleranceVelocityMax = 1.08 * vitesseCibleShooter; + } + else if (vitesseCibleShooter < 4400) { + toleranceVelocityMin = 0.93 * vitesseCibleShooter; + toleranceVelocityMax = 1.07 * vitesseCibleShooter; + } + else { + toleranceVelocityMin = 0.93 * vitesseCibleShooter; + toleranceVelocityMax = 1.05 * vitesseCibleShooter; + } + + if ((shooter.getShooterVelocityRPM() > toleranceVelocityMin) + && (shooter.getShooterVelocityRPM() < toleranceVelocityMax) + && !indexeur.isindexeurBusy()) { + timer.reset(); + state = TirState.TURRET_POSITION; + } + + // NOTE : timeout laissé inchangé (comportement d'origine) + if (timer.milliseconds() > 2500) { + timer.reset(); + state = TirState.SERVO_RETRACT; + indexeur.decrementerBalle(); + } + break; + + // --- 2) Positionnement tourelle --- + case TURRET_POSITION: + rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + tourelle.allerVersAngle(angleCibleTourelle); + + if (tourelle.isAtAngle(angleCibleTourelle)) { + timer.reset(); // reset à l'entrée de SERVO_PUSH + state = TirState.SERVO_PUSH; + } + break; + + // --- 4) Pousser la balle --- + case SERVO_PUSH: { + rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + + servoTireur.push(); + + // Attendre 200 ms avant de décrémenter et passer à RETRACT + if (timer.milliseconds() > 200) { + indexeur.decrementerBalle(); // décrémentation unique, au bon moment + timer.reset(); // reset à l'entrée de SERVO_RETRACT + state = TirState.SERVO_RETRACT; + } + break; + } + + // --- 5) Rétracter le servo --- + case SERVO_RETRACT: + servoTireur.retract(); + if (timer.milliseconds() > 150) { + timer.reset(); + shotsRemaining--; + tirsEffectues++; + state = TirState.INDEX_ADVANCE; + } + break; + + // --- 6) Avancer l'indexeur ou finir --- + case INDEX_ADVANCE: + if (shotsRemaining == 0) { + shooter.setShooterTargetRPM(0); + intake.repriseApresTir(); + timer.reset(); + state = TirState.IDLE; + + } else { + indexeur.avancerPourTir(); + timer.reset(); + state = TirState.WAIT_AFTER_INDEX; + } + break; + + // --- 7) Petite pause avant tir suivant --- + case WAIT_AFTER_INDEX: + if (timer.milliseconds() > 10) { + state = TirState.AFTERWAIT_INDEX; + } + break; + + case AFTERWAIT_INDEX: { + // Cohérence stricte (comme TeleOp) + checkIndexeurCoherence(); + if (indexeur.isRotationTerminee()) { + if (shotsRemaining > 0) { + // Fenêtre stricte : ne pas perdre un tir + if (!indexeur.indexeurPretPourTir()) { + indexeur.microCorrigerVersCible(0.20); + return; // rester ici jusqu'au calage + } + timer.reset(); // reset à l'entrée de SERVO_PUSH + state = TirState.SERVO_PUSH; + } else { + shooter.setShooterTargetRPM(0); + intake.repriseApresTir(); + timer.reset(); + state = TirState.IDLE; + } + } + break; + } + } + } + + // --- Lancer un tir automatique --- + public void startTirAuto(double angleTourelle, double angleShooter, double vitesseShooter) { + intake.arretPourTir(); + tirEnCours = true; + shotsRemaining = 3; + indexeur.setBalles(3); + prespinActif = false; + this.angleCibleTourelle = angleTourelle; + this.angleCibleShooter = angleShooter; + this.vitesseCibleShooter = vitesseShooter; + + tirsEffectues = 0; + + shooter.setShooterTargetRPM(vitesseCibleShooter); // Démarre immédiatement + state = TirState.SHOOTER_SPINUP; + } + + public TirState getState() { + return state; + } + + public boolean isBusy() { + return state != TirState.IDLE; + } + + public boolean isTirEnCours() { + return tirEnCours; + } + + private double rampShooterRPM(double targetRPM) { + + double dt = dtShooter.seconds(); + dtShooter.reset(); + + double currentRPM = shooter.getShooterVelocityRPM(); + + // PAS DE RAMPE AU-DESSUS DE 4800 (commentaire aligné avec la condition) + if (targetRPM >= 4800) { + rampedShooterRPM = targetRPM; // reset rampe pour éviter dérive + kickActiveShooter = false; + return targetRPM; + } + + // STOP DIRECT SI ON COUPE + if (targetRPM <= 0) { + rampedShooterRPM = 0; + kickActiveShooter = false; + return 0; + } + + // KICK de démarrage bas régime + if (currentRPM < KICK_MIN_RPM && rampedShooterRPM < KICK_MIN_RPM * 0.8) { + if (!kickActiveShooter) { + kickActiveShooter = true; + kickTimerShooter.reset(); + } + } + + double commanded = targetRPM; + + if (kickActiveShooter) { + commanded = Math.max(targetRPM, KICK_MIN_RPM); + if (kickTimerShooter.milliseconds() > KICK_TIME_MS || currentRPM >= KICK_MIN_RPM * 0.9) { + kickActiveShooter = false; + } + } + + // RAMPE UNIQUEMENT BASSE VITESSE + double slewUp = 3500; // montée douce pour ~3800 + double slewDown = 12000; // descente rapide + + double maxUp = slewUp * dt; + double maxDown = slewDown * dt; + + double delta = commanded - rampedShooterRPM; + + if (delta > maxUp) delta = maxUp; + if (delta < -maxDown) delta = -maxDown; + + rampedShooterRPM += delta; + + return rampedShooterRPM; + } + + public void prespinShooter(double rpm) { + prespinRPM = rpm; + prespinActif = true; + } + + public void stopPrespin() { + prespinActif = false; + } + + public void shotsRemaining(int n) { shotsRemaining = n; } +} + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManagerTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManagerTeleop.java new file mode 100644 index 000000000000..887293de6650 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManagerTeleop.java @@ -0,0 +1,436 @@ + +package org.firstinspires.ftc.teamcode.pedroPathing.logique; + +import com.qualcomm.robotcore.util.ElapsedTime; + +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; + +public class TireurManagerTeleop { + + // --- Modules contrôlés --- + private final Shooter shooter; + private boolean tirEnCours = false; + private final SpinTurret tourelle; + private final AngleShooter ServoAngleShoot; + private final ServoTireur servoTireur; + private final Indexeur indexeur; + private final Intake intake; + private double angleCibleTourelle = 0; + private boolean tirAutoActif = false; + private static final int TOL_STRICT_TICKS = 50; + + // Données caméra éventuelles (injetées depuis le TeleOp) + private Double txCamera = null; + private boolean cameraHasTag = false; + + private final AfficheurRight afficheurRight; + + // --- Machine à états --- + public enum TirState { + IDLE, + SHOOTER_SPINUP, + TURRET_POSITION, + ANGLE_POSITION, + align_tourelletermine, + SERVO_PUSH, + SERVO_RETRACT, + INDEX_ADVANCE, + WAIT_AFTER_INDEX, + AVANCE1TIR, + align_camera, + AFTERWAIT_INDEX + } + + private TirState state = TirState.IDLE; + private final ElapsedTime timer = new ElapsedTime(); + + private int tirsEffectues = 0; + private int shotsRemaining = 0; + + private double Min_shooterRPM = 4000; + private double toleranceVelocityMax; + private double toleranceVelocityMin; + private boolean prespinActif = false; + private double prespinRPM = 0; + private double TargetFlyWheelRPM = 4700; + + // --- Rampe pour le shooter (sans modifier PIDF) --- + private double rampedShooterRPM = 0; + private final ElapsedTime dtShooter = new ElapsedTime(); + + // Kick anti-collage + private boolean kickActiveShooter = false; + private final ElapsedTime kickTimerShooter = new ElapsedTime(); + private static final double KICK_MIN_RPM = 900; + private static final double KICK_TIME_MS = 80; + + private double shootermaxspintime = 2; + + // --- Cibles dynamiques --- + private double angleCibleShooter = 0; + private double vitesseCibleShooter = 0; + + public TireurManagerTeleop(Shooter shooter, + SpinTurret tourelle, + AngleShooter ServoAngleShoot, + ServoTireur servoTireur, + Indexeur indexeur, Intake intake, AfficheurRight afficheurRight) { + + this.shooter = shooter; + this.tourelle = tourelle; + this.ServoAngleShoot = ServoAngleShoot; + this.servoTireur = servoTireur; + this.indexeur = indexeur; + this.intake = intake; + this.afficheurRight = afficheurRight; + + dtShooter.reset(); + kickActiveShooter = false; + rampedShooterRPM = 0; + } + + public void update() { + + boolean tirActif = (state != TirState.IDLE); + if (tirActif) { + //afficheurRight.setClignoteVert(); + } else { + //afficheurRight.setIdle(); + } + + afficheurRight.update(); + + switch (state) { + + case IDLE: { + if (prespinActif) { + shooter.setShooterTargetRPM(rampShooterRPM(prespinRPM)); + } else { + shooter.setShooterTargetRPM(0); + } + break; + } + + case TURRET_POSITION: { + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + + tourelle.allerVersAngle(angleCibleTourelle); + if (tourelle.isAtAngle(angleCibleTourelle)) { + timer.reset(); + tirAutoActif = false; + state = TirState.ANGLE_POSITION; + } + break; + } + + case AVANCE1TIR: { + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + stopPrespin(); + + if (!indexeur.isHomingDone()) { + indexeur.lancerHoming(); + return; + } + if (indexeur.isHomingDone()) { + indexeur.avancerPourTir(); + timer.reset(); + state = TirState.WAIT_AFTER_INDEX; + } + break; + } + + case ANGLE_POSITION: { + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + + // 1) Commande du volet + ServoAngleShoot.setAngle(angleCibleShooter); + + // 2) Vérification indexeur : s’il n’est PAS calé → correction → rester ici + if (!indexeur.indexeurPretPourTir()) { + indexeur.microCorrigerVersCible(0.20); + return; // rester dans ANGLE_POSITION + } + + // 3) Vérification du volet : s’il n’est PAS calé → attendre → rester ici + if (!ServoAngleShoot.isAtAngle(angleCibleShooter)) { + return; // on attend qu’il se cale + } + + // 4) Les DEUX sont calés → on peut avancer + timer.reset(); + state = TirState.SHOOTER_SPINUP; + break; + } + + case SHOOTER_SPINUP: { + stopPrespin(); + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + + // tolérances RPM dynamiques + + if (vitesseCibleShooter < 4000) { + toleranceVelocityMin = 0.94 * vitesseCibleShooter; + toleranceVelocityMax = 1.08 * vitesseCibleShooter; + } + else if (vitesseCibleShooter < 4400) { + toleranceVelocityMin = 0.93 * vitesseCibleShooter; + toleranceVelocityMax = 1.07 * vitesseCibleShooter; + } + else { + toleranceVelocityMin = 0.93 * vitesseCibleShooter; + toleranceVelocityMax = 1.05 * vitesseCibleShooter; + } + + if ((shooter.getShooterVelocityRPM() > toleranceVelocityMin) + && (shooter.getShooterVelocityRPM() < toleranceVelocityMax) + && !indexeur.isindexeurBusy()) { + + // Alignement indexeur OK → on entre dans SERVO_PUSH + timer.reset(); // reset à l’entrée de SERVO_PUSH + state = TirState.SERVO_PUSH; + } + break; + } + + case SERVO_PUSH: { + // Timer a été reset AVANT d'entrer dans cet état + servoTireur.push(); + + // 200 ms → décrémenter et passer en retract + if (timer.milliseconds() > 200) { + indexeur.decrementerBalle(); // décrémentation UNIQUE + timer.reset(); // reset à l’entrée de SERVO_RETRACT + state = TirState.SERVO_RETRACT; + } + break; + } + + case SERVO_RETRACT: { + // Timer reset à l’entrée de l'état précédent + servoTireur.retract(); + if (timer.milliseconds() > 150) { + timer.reset(); // reset à l’entrée de l’état suivant + if (shotsRemaining > 0) shotsRemaining--; + tirsEffectues++; + state = TirState.INDEX_ADVANCE; + } + break; + } + + case INDEX_ADVANCE: { + if (shotsRemaining == 0) { + shooter.setShooterTargetRPM(0); + intake.repriseApresTir(); + state = TirState.IDLE; + } else { + indexeur.avancerPourTir(); + timer.reset(); + state = TirState.WAIT_AFTER_INDEX; + } + break; + } + + case WAIT_AFTER_INDEX: { + if (timer.milliseconds() > 10) { + state = TirState.AFTERWAIT_INDEX; + } + break; + } + + case AFTERWAIT_INDEX: { + checkIndexeurCoherence(); + if (indexeur.isRotationTerminee()) { + if (shotsRemaining > 0) { + + // fenêtre stricte + if (!indexeur.indexeurPretPourTir()) { + indexeur.microCorrigerVersCible(0.20); + return; // correctif : évite de perdre un tir + } + timer.reset(); // reset à l’entrée de SERVO_PUSH + state = TirState.SERVO_PUSH; + + } else { + shooter.setShooterTargetRPM(0); + intake.repriseApresTir(); + tirEnCours = false; + state = TirState.IDLE; + } + } + break; + } + } + } + + // --- Lancer un tir manuel 3 tirs --- + public void startTirManuel3Tirs(double angleShooter, double vitesseShooter) { + intake.arretPourTir(); + tirEnCours = true; + shotsRemaining = 3; + tirsEffectues = 0; + this.angleCibleShooter = angleShooter; + this.vitesseCibleShooter = vitesseShooter; + + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + + timer.reset(); + state = TirState.ANGLE_POSITION; + } + + public void startTirManuel1tir(double angleShooter, double vitesseShooter) { + intake.arretPourTir(); + tirEnCours = true; + shotsRemaining = 1; + prespinActif = false; + this.angleCibleShooter = angleShooter; + this.vitesseCibleShooter = vitesseShooter; + tirsEffectues = 0; + + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + + timer.reset(); + state = TirState.AVANCE1TIR; + } + + public void startTirAuto(double angleTourelle, double angleShooter, double vitesseShooter) { + intake.arretPourTir(); + tirAutoActif = true; + tirEnCours = true; + prespinActif = false; + shotsRemaining = 3; + this.angleCibleTourelle = angleTourelle; + this.angleCibleShooter = angleShooter; + this.vitesseCibleShooter = vitesseShooter; + + tirsEffectues = 0; + + timer.reset(); + + double rpmCmd = rampShooterRPM(vitesseCibleShooter); + shooter.setShooterTargetRPM(rpmCmd); + state = TirState.TURRET_POSITION; + } + + public TirState getState() { + return state; + } + + public boolean isBusy() { + return state != TirState.IDLE; + } + + public boolean isTirEnCours() { return tirEnCours; } + + public void setState(TirState state) { this.state = state; } + + public boolean isTirAutoActif() { return tirAutoActif; } + + // --- Rampe rapide + kick (sans toucher au PIDF) --- + private double rampShooterRPM(double targetRPM) { + + double dt = dtShooter.seconds(); + dtShooter.reset(); + + double currentRPM = shooter.getShooterVelocityRPM(); + + // 🚀 PAS DE RAMPE AU-DESSUS DE 4000 + if (targetRPM >= 4000) { + rampedShooterRPM = targetRPM; // reset rampe pour éviter dérive + kickActiveShooter = false; + return targetRPM; + } + + // STOP DIRECT SI ON COUPE + if (targetRPM <= 0) { + rampedShooterRPM = 0; + kickActiveShooter = false; + return 0; + } + + // KICK de démarrage bas régime + if (currentRPM < KICK_MIN_RPM && rampedShooterRPM < KICK_MIN_RPM * 0.8) { + if (!kickActiveShooter) { + kickActiveShooter = true; + kickTimerShooter.reset(); + } + } + + double commanded = targetRPM; + + if (kickActiveShooter) { + commanded = Math.max(targetRPM, KICK_MIN_RPM); + if (kickTimerShooter.milliseconds() > KICK_TIME_MS || currentRPM >= KICK_MIN_RPM * 0.9) { + kickActiveShooter = false; + } + } + + // RAMPE UNIQUEMENT BASSE VITESSE + double slewUp = 3500; // montée douce pour 3800 + double slewDown = 12000; // descente rapide + + double maxUp = slewUp * dt; + double maxDown = slewDown * dt; + + double delta = commanded - rampedShooterRPM; + + if (delta > maxUp) delta = maxUp; + if (delta < -maxDown) delta = -maxDown; + + rampedShooterRPM += delta; + + return rampedShooterRPM; + } + + public void shotsRemaining(int nb) { shotsRemaining = nb; } + + public void cancelTir() { + shotsRemaining = 0; + tirEnCours = false; + prespinActif = false; + servoTireur.retract(); + shooter.setShooterTargetRPM(0); + intake.repriseApresTir(); + state = TirState.IDLE; + } + + public void prespinShooter(double rpm) { + prespinRPM = rpm; + prespinActif = true; + } + + public void stopPrespin() { + prespinActif = false; + } + + private void checkIndexeurCoherence() { + if (!indexeur.isHomingDone()) return; // pas de check tant que non calé + if (indexeur.isindexeurBusy()) return; // laisse finir la rotation + + int err = indexeur.getPositionErreurTicks(); + if (err > TOL_STRICT_TICKS) { + // Correction douce vers la cible planifiée + indexeur.microCorrigerVersCible(0.25); + } + } + + public boolean isPrespinActif() { + return prespinActif; + } + + public void updateCameraData(Double tx, boolean hasTag) { + this.txCamera = tx; + this.cameraHasTag = hasTag; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/CameraLimelight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/CameraLimelight.java new file mode 100644 index 000000000000..8ea8269f350c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/CameraLimelight.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.navigation; + +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class CameraLimelight { + + private Limelight3A limelight; + + // --- Buffer circulaire pour filtrer la distance --- + private double[] buffer = new double[3]; + private int index = 0; + private int count = 0; + + // Constructeur vide (comme HuskyLens) + public CameraLimelight() {} + + // Initialisation (comme HuskyLens.init()) + public void init(HardwareMap hardwareMap) { + limelight = hardwareMap.get(Limelight3A.class, "Limelight"); + limelight.pipelineSwitch(0); + } + + public void start() { + limelight.start(); + } + + /** Retourne le dernier résultat brut */ + public LLResult getResult() { + return limelight.getLatestResult(); + } + + /** Retourne TX ou null si pas de tag */ + public Double getTx() { + LLResult result = getResult(); + if (result == null || !result.isValid()) return null; + return result.getTx(); + } + + /** Retourne TA ou null si pas de tag */ + public Double getTa() { + LLResult result = getResult(); + if (result == null || !result.isValid()) return null; + return result.getTa(); + } + + /** Distance rectifiée sur sur TA */ + public Double getDistance() { + Double ta = getTa(); + if (ta == null || ta <= 0) return null; + + return 186.6697 * Math.pow(ta, -0.6873269); + } + + + /** Distance filtrée (comme HuskyLens.getDistanceFiableCm()) */ + public double getDistanceFiableCm() { + + Double dist = getDistance(); + if (dist == null || dist < 0) return -1; + + + // Ajout dans buffer circulaire + buffer[index] = dist; + index = (index + 1) % 3; + if (count < 3) count++; + + if (count < 3) return -1; + + // Recherche d’un groupe cohérent + for (int i = 0; i < 3; i++) { + int c = 1; + double somme = buffer[i]; + + for (int j = 0; j < 3; j++) { + if (i != j && Math.abs(buffer[j] - buffer[i]) <= 5.0) { + somme += buffer[j]; + c++; + } + } + + if (c >= 3) { + return somme / c; + } + } + + return -1; + } + public boolean hasTag() { + LLResult result = getResult(); + return result != null && result.isValid(); + } + +} + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/Camerahusky.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/Camerahusky.java new file mode 100644 index 000000000000..4cc20bf19aea --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/Camerahusky.java @@ -0,0 +1,108 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.navigation; + +import com.qualcomm.hardware.dfrobot.HuskyLens; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.internal.system.Deadline; +import java.util.concurrent.TimeUnit; + +public class Camerahusky { + HuskyLens.Block tag; + + + private final int READ_PERIOD = 1; + private final double TAG_REAL_WIDTH_CM = 20.5; + private final double FOV_DEGREES = 60.0; + private final int IMAGE_WIDTH = 320; + private final double K = 400.0; + + private HuskyLens huskyLensDevice; + private Deadline rateLimit; + + private double[] buffer = new double[5]; + private int index = 0; + private int count = 0; + + + public void init(HardwareMap hardwareMap) { + huskyLensDevice = hardwareMap.get(HuskyLens.class, "huskylens"); + rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS); + rateLimit.expire(); + huskyLensDevice.selectAlgorithm(HuskyLens.Algorithm.TAG_RECOGNITION); + } + + public boolean isConnected() { + return huskyLensDevice != null && huskyLensDevice.knock(); + } + + public boolean canRead() { + return rateLimit.hasExpired(); + } + + public void resetRateLimit() { + rateLimit.reset(); + } + + public boolean hasTag() { + return huskyLensDevice.blocks().length > 0; + } + + public double getEstimatedDistanceCm() { + HuskyLens.Block[] blocks = huskyLensDevice.blocks(); + if (blocks.length > 0) { + int tagWidthPixels = blocks[0].width; + return (K * TAG_REAL_WIDTH_CM) / tagWidthPixels; + } + return -1; + } + + public double getEstimatedAngleDegrees() { + HuskyLens.Block[] blocks = huskyLensDevice.blocks(); + if (blocks.length > 0) { + int tagX = blocks[0].x; + int pixelOffset = tagX - (IMAGE_WIDTH / 2); + return (pixelOffset * FOV_DEGREES) / IMAGE_WIDTH; + } + return 0; + } + + public double getDistanceFiableCm() { + double cam = getEstimatedDistanceCm(); + if (cam < 0) return -1; + + // Correction en cm (calibration) + double reel = 0.7078 * cam - 13.25; + //double reel = 0.5096995 * Math.pow(cam, 1.023373); + + // Buffer circulaire de 5 valeurs + buffer[index] = reel; + index = (index + 1) % 5; + if (count < 5) count++; + + // Pas encore 5 valeurs → pas fiable + if (count < 5) return -1; + + // Cherche un groupe de ≥3 mesures dans ±5 cm + for (int i = 0; i < 5; i++) { + int c = 1; + double somme = buffer[i]; + + for (int j = 0; j < 5; j++) { + if (i != j && Math.abs(buffer[j] - buffer[i]) <= 5.0) { + somme += buffer[j]; + c++; + } + } + + if (c >= 3) { + return somme / c; // Renvoie UN SEUL double + } + } + return -1; + } +} + + + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/GlobalPoseStorage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/GlobalPoseStorage.java new file mode 100644 index 000000000000..b45c4d67c251 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/navigation/GlobalPoseStorage.java @@ -0,0 +1,11 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.navigation; + +import com.pedropathing.geometry.Pose; + +import java.net.PortUnreachableException; + +public class GlobalPoseStorage { + public static Pose currentPose = new Pose(); + public static double turretAngleDeg = 0.0; + +}