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/DecodeBlueAutoFond.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeBlueAutoFond.java new file mode 100644 index 000000000000..580049492654 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Auto/DecodeBlueAutoFond.java @@ -0,0 +1,368 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.Auto; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.follower.Follower; +import com.pedropathing.util.Timer; + +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AfficheurLeft; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.AfficheurRight; +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.logique.TireurManager; +import org.firstinspires.ftc.teamcode.pedroPathing.Hardware.Intake; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; + + +@Autonomous (name="bleuFond", group="Competition") +public class DecodeBlueAutoFond 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; + + 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(137)); + + private final Pose drivetoligne1= new Pose (48.3920653, 82.8378063, Math.toRadians(180)); + + private final Pose avalerballeRangee1 = new Pose (28.4830805, 84.1820303, Math.toRadians(180)); + + private final Pose Shoot2 = new Pose (60.3220536, 84.0140023, Math.toRadians(137)); + + private final Pose drivetoligne2= new Pose (47.71995332555426, 59.649941656942815, Math.toRadians(180)); + + private final Pose avalerballeRangee2= new Pose (18.4830805, 59.649941656942815, 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()) + .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) { + intake.update(); + indexeur.update(); + case DRIVE_STARTPOSITIONTOSHOOT: + follower.followPath(driveStartofirstShootPos, 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 + 0, // angle tourelle (exemple) + 0.28, // angle shooter + 3880 // 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 , 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.3,true); // on avance doucement pour avaler les balles + setPathState(PathState.DrivedeuxiemeShoot); + } + break; + + case DrivedeuxiemeShoot: + intake.update(); + indexeur.update(); + ; + if (!follower.isBusy()) { // Attendre que l'on est fini d'avoir pris toutes les balles + follower.followPath(DrivedeuxiemeShoot, true); + // Le robot est arrivé en position de tir : + } + break; + + case deuxiemetir: + intake.update(); + indexeur.update(); + if (!shotsTriggered) { // deuxieme période de tir + tireurManager.startTirAuto(// Lancer tir automatique + 0, // angle tourelle (exemple) + 0.28, // angle shooter + 3880 // RPM + ); + shotsTriggered = true;} + else if (shotsTriggered && !tireurManager.isBusy()){ + setPathState(PathState.align_rangee2blue); + shotsTriggered = false; + } + break; + + case align_rangee2blue: // alignement avec la deuxieme zone 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, 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.30 , 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 les balles ) + indexeur.update(); + if (!follower.isBusy()) { + follower.followPath(driveAvaler2emeLignetotroisemeShoot, , 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: + intake.update(); // mise à jour de nos systemes (constate les balles ) + indexeur.update(); + if (!follower.isBusy()) { + // le robot est arrivé sur la troisieme position de tir : + + if (!shotsTriggered){ + tireurManager.startTirAuto(// Lancer tir automatique + 0, // angle tourelle (exemple) + 0.28, // angle shooter + 3880 // 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()) { + follower.followPath(DrivetoGate,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"); + 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); + + + // --- Initialisation hardware --- + shooter = new Shooter(); + shooter.init(hardwareMap); + + 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); + + 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()); + + } +} 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..b112de91d672 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) + .forwardZeroPowerAcceleration(-30.2711203755) + .lateralZeroPowerAcceleration(-64.5169691986) + .translationalPIDFCoefficients(new PIDFCoefficients(0.1,0,0.01,0.02)) + .headingPIDFCoefficients(new PIDFCoefficients(1.0,0,0.03,0.03)) + .drivePIDFCoefficients(new FilteredPIDFCoefficients(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.8) + + .xVelocity(51.015311501) + .yVelocity(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, + 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..00d2b04b945f --- /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.52; + + 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..28572cf7ac84 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Indexeur.java @@ -0,0 +1,469 @@ +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; + + // 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 = 1.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"; + 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 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(); + 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; + 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) return; + if (rotationEnCours) return; + + positionLogique++; + compartimentPhysique = positionLogique % COMPARTIMENTS; + + targetTicks = (int) (positionLogique * TICKS_PAR_COMPARTIMENT); + + indexeur.setTargetPosition(targetTicks); + indexeur.setMode(DcMotor.RunMode.RUN_TO_POSITION); + indexeur.setPower(0.5); + indexeurtimer.reset(); // timer général + erreurTimer.reset(); // timer d'erreur (au-dessus de tolérance) + + rotationEnCours = true; + } + + public void updateRotation() { + + if (!rotationEnCours) return; + + int erreur = Math.abs(indexeur.getCurrentPosition() - targetTicks); + + // --- LECTURE CONTINUE DE LA COULEUR (NOUVEAU) --- + // On lit la couleur pendant la rotation, dès qu'une couleur valide apparaît + String couleurInstant = detectBallColor(); + + if (!"Inconnue".equals(couleurInstant)) { + couleurDetecteeTemp = couleurInstant; // on garde la meilleure couleur vue + } + + // --- Gestion de la vitesse progressive --- + if (erreur > 300) { + indexeur.setPower(0.9); // loin de la cible → rapide + } else { + indexeur.setPower(0.5); // proche de la cible → lent + } + + boolean finNormale = !indexeur.isBusy() || (erreur < erreurindexeur); + boolean finParTimerErreur = (erreur >= erreurindexeur) && (erreurTimer.seconds() > ERREUR_TIMEOUT_S); + + if (!(finNormale || finParTimerErreur)) { + return; + } + + // --- ROTATION FINIE --- + indexeur.setPower(0); + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + int slotCapteurs = compartimentPhysique; + int slotTir = (compartimentPhysique + 1) % COMPARTIMENTS; + int slotEntree = (compartimentPhysique + 2) % COMPARTIMENTS; + + // --- UTILISATION DE LA MEILLEURE COULEUR DÉTECTÉE --- + String couleurSousCapteurs = couleurDetecteeTemp; + couleurDetecteeTemp = "Inconnue"; // reset pour la prochaine rotation + + couleurBalleDansCompartiment[slotCapteurs] = couleurSousCapteurs; + + // --- Mise à jour du comptage --- + if (rotationPourAmassage) { + ballComptage = Math.min(ballComptage + 1, MAX_BALLS); + //if (!"Inconnue".equals(couleurSousCapteurs)) { + // ballComptage = Math.min(ballComptage + 1, MAX_BALLS); + //} + } else if (rotationPourTir) { + String couleurAuTir = couleurBalleDansCompartiment[slotTir]; + //if (couleurAuTir != null && !"Inconnue".equals(couleurAuTir)) { + // ballComptage = Math.max(ballComptage - 1, MIN_BALLS); + //} + //ballComptage = Math.max(ballComptage - 1, MIN_BALLS); + couleurBalleDansCompartiment[slotTir] = "Inconnue"; + } + + rotationPourAmassage = false; + rotationPourTir = false; + rotationEnCours = false; + + IndexeurState = finParTimerErreur ? Indexeuretat.IDLE : Indexeuretat.IDLE; //Retrait Bourrage + } + + + + public void reculerIndexeurbourrage() { + int positionbourrage = indexeur.getCurrentPosition(); + int delta = (int) (TICKS_PER_REV_43 * 0.05); // environ 18° + int target = positionbourrage - delta; + indexeur.setTargetPosition(target); + indexeur.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // Power ceiling controls max speed in + indexeur.setPower(0.2); // augmente si besoin, attention au couple, pas besoin de mettre puissance negative car calcul auto } + int erreur = Math.abs(target - indexeur.getCurrentPosition()); + if (erreur < 15) { // tolérance de 15 ticks // Stop net + indexeur.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + indexeur.setPower(0); + } + } + + + 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 = 15; + 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"; + } + } + + +} + + + + 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..8ce766b6bded --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Intake.java @@ -0,0 +1,200 @@ +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 = -1800; + + private double intake_fast = 2000; + + + + 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 = 300; + private final double CHUTE_RPM = 0.85; + private final float SEUIL_LUM_INDEXEUR = 0.250f; + + private Indexeur indexeur; + private NormalizedColorSensor ColorIndexeur, ColorIntake; + private Rev2mDistanceSensor distSensorIndexeur; + + private static final int TICKS_PER_REV_6000 = 28; + + 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(); + boolean intakeEnMarche = (intakeState == Intakeetat.RAMASSAGE); + if (intakeEnMarche) { + afficheurLeft.setClignoteOrange(); + } else { + afficheurLeft.setIdle(); + } + + afficheurLeft.update(); + + + + switch (intakeState) { + + case IDLE: + setIntakeBallTargetRPM(0); + if (ballcomptage == 0) { + 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); + } + + // --- Bourrage --- + if (rpm < MINRPM && statetimer.milliseconds() > tempsblocage) { + intakeState = Intakeetat.EJECTION; + } + break; + + case EJECTION: + setIntakeBallTargetRPM(intake_reverse); + if (statetimer.milliseconds() > 500) { + 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; + } +} + 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..a53a58426f3b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/ServoTireur.java @@ -0,0 +1,80 @@ +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.79; // position basse + private double tirpositionhaute = 0.31; // position haute + + 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..a911cc36a543 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/Shooter.java @@ -0,0 +1,129 @@ +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 = 4700.0; + double maxVel = (maxVelRPM * TICKS_PER_REV_6000) / 60.0; + double kF = 32767.0 / maxVel; + double kP = 6.0; // Agressif car systeme rapide + double kI = 0.0 ; // nuisible voir inutile avec shooter + double kD = 0.4 ; // 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; + + double ramp = 600.0; //un peu plus nerveux qu'un ascenseur à dubai + + double error = targetTicksPerSec - currentTargetVel; + if (Math.abs(error)>200) { + 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); + } + 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 ticksPerSec = Shooter.getVelocity(); + return (ticksPerSec * 60) / TICKS_PER_REV_6000; + } + 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..05a10d1d70e8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Hardware/SpinTurret.java @@ -0,0 +1,132 @@ +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; + +public class SpinTurret { + private CRServo SpinTourelle; + private double powerarrettourelle = 0.0; + private double powertournertourelle =1.0; + public IMU imuTourelle; + double erreur; + + 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 = 1; // valeur maximale autorisée + power = Math.max(-maxPower, Math.min(power, maxPower)); // clamp entre -1 et +1 + + double angletourelle = lectureangletourelle(); + if ((angletourelle >= 50 && power < 0) || (angletourelle <= -50 && 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 < 20.0) { + double minPower = 0.10; // + power = Math.signum(erreur) * minPower; + } + // 3) Zone lointaine : proportionnel normal + else { + double kP = 0.10; + power = kP * erreur; + + power = Math.max(-0.3, Math.min(power, 0.3)); + } + + 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; + } + + +} \ 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..e4fb6ffc0ebe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TeleOpTest.java @@ -0,0 +1,146 @@ +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.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; + +@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/TestAngleShootTourelle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestAngleShootTourelle.java new file mode 100644 index 000000000000..9ea5a0cdacf9 --- /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.30, 0.40, 0.52}; + 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..b71126ed66fd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestLumiere.java @@ -0,0 +1,127 @@ +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 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/TestTirAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTirAuto.java new file mode 100644 index 000000000000..2d568b53a231 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTirAuto.java @@ -0,0 +1,110 @@ +package org.firstinspires.ftc.teamcode.pedroPathing.TeleOp.Debug; + +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; + +@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..3b73a6f7403a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/Debug/TestTournerTourelle.java @@ -0,0 +1,114 @@ +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 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 <<<"); + } + + + + @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..bae29a1a7f2c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp/TeleOpDecode.java @@ -0,0 +1,264 @@ +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.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.TireurManager; +import org.firstinspires.ftc.teamcode.pedroPathing.logique.TireurManagerTeleop; + +import java.util.function.Supplier; + +@Configurable +@TeleOp (name="TeleOp Competiton Bleu", 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 boolean slowMode = false; + private double slowModeMultiplier = 0.5; + 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 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 TireurManagerTeleop(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 TireurManagerTeleop(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); + double[] presetsAngleshoot = {0.12, 0.25, 0.30, 0.40, 0.52}; + if (gamepad2.x && !lastX) { + positionAngleshoot = (positionAngleshoot + 1) % presetsAngleshoot.length; + ServoAngleShoot.angleShoot(presetsAngleshoot[positionAngleshoot]); + } + lastX = gamepad2.x; + + double powertourelle = gamepad2.left_stick_x; // Joystick Horizontal + tourelle.rotationtourelle(powertourelle); + + + double[] presetsVitesseShooter = {0.0, 3800, 3900, 4000, 4600}; + if (gamepad2.b && !lastB) { + presetIndexShooter = (presetIndexShooter + 1) % presetsVitesseShooter.length; + shooter.setShooterTargetRPM(presetsVitesseShooter[presetIndexShooter]); + } + lastB = gamepad2.b; + + + + if (gamepad2.right_bumper && !lastrightbumper) { + + tireurManager.startTirManuel3Tirs(); + } + lastrightbumper = gamepad2.right_bumper; + + if (gamepad2.left_bumper && !lastleftbumper) { + + tireurManager.startTirManuel1tir(); + } + + lastleftbumper = gamepad2.left_bumper; + + + 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/logique/TireurManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManager.java new file mode 100644 index 000000000000..19eeef38bde6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManager.java @@ -0,0 +1,224 @@ +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 boolean tirEnCours = false; + private final SpinTurret tourelle; + private final AngleShooter ServoAngleShoot; + private final ServoTireur servoTireur; + private final Indexeur indexeur; + private final Intake intake; + + private final AfficheurRight afficheurRight; + + // --- Machine à états --- + public enum TirState { + IDLE, + SHOOTER_SPINUP, + 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 double angleCibleShooter = 0; + private double vitesseCibleShooter = 0; + + 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; + } + + public void update() { + + boolean tirActif = (state != TirState.IDLE); + if (tirActif) { + afficheurRight.setClignoteVert(); + } else { + afficheurRight.setIdle(); + } + + afficheurRight.update(); + + switch (state) { + + + case IDLE: + shooter.setShooterTargetRPM(0); + break; + + // --- 1) Shooter spin-up --- + case SHOOTER_SPINUP: + if (shotsRemaining >0) { + shooter.setShooterTargetRPM(vitesseCibleShooter); + intake.disableRamassage(); + tourelle.allerVersAngle(angleCibleTourelle); + if (!indexeur.isHomingDone()) { + indexeur.lancerHoming(); + return; + } + if (indexeur.isHomingDone()) { + state = TirState.TURRET_POSITION; + timer.reset(); + } + } + else { state = TirState.IDLE; + shooter.setShooterTargetRPM(0);} + break; + + // --- 2) Positionnement tourelle --- + case TURRET_POSITION: + shooter.setShooterTargetRPM(vitesseCibleShooter); + tourelle.allerVersAngle(angleCibleTourelle); + + if (tourelle.isAtAngle(angleCibleTourelle)) { + timer.reset(); + state = TirState.ANGLE_POSITION; + } + break; + + // --- 3) Positionnement angle shooter --- + case ANGLE_POSITION: + shooter.setShooterTargetRPM(vitesseCibleShooter); + ServoAngleShoot.setAngle(angleCibleShooter); + //timer.reset(); + + if (ServoAngleShoot.isAtAngle(angleCibleShooter)) { + timer.reset(); + state = TirState.SERVO_PUSH; + } + break; + + // --- 4) Pousser la balle --- + case SERVO_PUSH: + shooter.setShooterTargetRPM(vitesseCibleShooter); + double toleranceVelocityMax = 1.04 * vitesseCibleShooter; + double toleranceVelocityMin = 0.94 * vitesseCibleShooter; + if ((shooter.getShooterVelocityRPM() > toleranceVelocityMin) && (shooter.getShooterVelocityRPM() < toleranceVelocityMax)){; + servoTireur.push(); + timer.reset(); + state = TirState.SERVO_RETRACT; + indexeur.decrementerBalle(); + }; + + if (timer.milliseconds() > 1000) { + timer.reset(); + state = TirState.SERVO_RETRACT; + } + break; + + // --- 5) Rétracter le servo --- + case SERVO_RETRACT: + servoTireur.retract(); + if (timer.milliseconds() > 300) { + timer.reset(); + shotsRemaining--; // retrait d'un tir + tirsEffectues++; // Tir réellement terminé ici + state = TirState.INDEX_ADVANCE; + } + break; + + // --- 6) Attendre fin rotation indexeur --- + case INDEX_ADVANCE: + if ((timer.milliseconds() > 300) && shotsRemaining == 0) { + shooter.setShooterTargetRPM(0); + intake.repriseApresTir(); + timer.reset(); + state = TirState.IDLE; + + } + if (!(shotsRemaining == 0) && (timer.milliseconds() > 300)) { + indexeur.avancerPourTir(); + timer.reset(); + state = TirState.WAIT_AFTER_INDEX; + } + + break; + + // --- 7) Petite pause avant tir suivant --- + case WAIT_AFTER_INDEX: + if (timer.milliseconds() > 190){ + state = TirState.AFTERWAIT_INDEX; + } + break; + + + case AFTERWAIT_INDEX: + if (indexeur.isRotationTerminee()) { + tourelle.allerVersAngle(angleCibleTourelle); + if (tourelle.isAtAngle(angleCibleTourelle)) { + timer.reset(); + state = TirState.SERVO_PUSH; + } + //tirEnCours = false; + //intake.enableRamassage(); + } + break; + } + } + + // --- Lancer un tir automatique --- + public void startTirAuto(double angleTourelle, double angleShooter, double vitesseShooter) { + intake.arretPourTir(); + tirEnCours = true; + shotsRemaining = 3; + 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; } +} 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..a2c832ffc46e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/logique/TireurManagerTeleop.java @@ -0,0 +1,187 @@ +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 final AfficheurRight afficheurRight; + + // --- Machine à états --- + public enum TirState { + IDLE, + SHOOTER_SPINUP, + TURRET_POSITION, + ANGLE_POSITION, + SERVO_PUSH, + SERVO_RETRACT, + INDEX_ADVANCE, + WAIT_AFTER_INDEX, + AVANCE1TIR, + 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 TargetFlyWheelRPM = 4700; + + private double shootermaxspintime = 2; + + // --- Cibles dynamiques --- + private double angleCibleTourelle = 0; + 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; + } + + public void update() { + + boolean tirActif = (state != TirState.IDLE); + if (tirActif) { + afficheurRight.setClignoteVert(); + } else { + afficheurRight.setIdle(); + } + + afficheurRight.update(); + + switch (state) { + + case IDLE: + break; + + // --- 4) Pousser la balle --- + + case AVANCE1TIR: + indexeur.avancerPourTir(); + timer.reset(); + state = TirState.WAIT_AFTER_INDEX; + break; + + case SERVO_PUSH: + servoTireur.push(); + if (timer.milliseconds() > 280) { + timer.reset(); + indexeur.decrementerBalle(); + state = TirState.SERVO_RETRACT; + } + break; + + // --- 5) Rétracter le servo --- + case SERVO_RETRACT: + servoTireur.retract(); + if (timer.milliseconds() > 280) { + timer.reset(); + shotsRemaining--; // retrait d'un tir + tirsEffectues++; + state = TirState.INDEX_ADVANCE;// Tir réellement terminé ici + } + break; + + // --- 6) Attendre fin rotation indexeur --- + case INDEX_ADVANCE: + if (shotsRemaining == 0) { + shooter.setShooterTargetRPM(0); + intake.repriseApresTir(); + state = TirState.IDLE; + + } + if (!(shotsRemaining == 0)){ + indexeur.avancerPourTir(); + timer.reset(); + state = TirState.WAIT_AFTER_INDEX; + } + break; + + // --- 7) Petite pause avant tir suivant --- + case WAIT_AFTER_INDEX: + if (timer.milliseconds() > 190){ + state = TirState.AFTERWAIT_INDEX; + } + break; + + case AFTERWAIT_INDEX: + + if (indexeur.isRotationTerminee()){ + timer.reset(); + state = TirState.SERVO_PUSH; + } + break; + } + } + + // --- Lancer un tir automatique --- + public void startTirManuel3Tirs() { + intake.arretPourTir(); + tirEnCours = true; + shotsRemaining = 3; + tirsEffectues = 0; + timer.reset(); + state = TirState.SERVO_PUSH; + } + + public void startTirManuel1tir() { + intake.arretPourTir(); + tirEnCours = true; + shotsRemaining = 1; + //this.vitesseCibleShooter = vitesseShooter; + + tirsEffectues = 0; + + //shooter.setShooterTargetRPM(vitesseShooter); // Démarre immédiatement + timer.reset(); + state = TirState.AVANCE1TIR; + + + //if ((shooter.getShooterVelocityRPM() >= vitesseShooter) && (indexeur.isHomingDone())) { + // state = TirState.SERVO_PUSH; + // timer.reset(); + //} + } + + public TirState getState() { + return state; + } + + public boolean isBusy(){ + return state != TirState.IDLE; + } + + public boolean isTirEnCours() { + return tirEnCours; } +}