ftc2024-robotcode/ftc2024_autonome_api.java

package org.firstinspires.ftc.teamcode;

//import FTC2024WeRobotControl; //a tester car pas sur que ça fonctionne

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;

import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;

import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcode.hardware.HardwareMap;

@Autonomous

public class Ftc2024_autonome_api{
    public enum AutoMode {
        B2D, B4D, B2N, B4N, R2D, R4D, R2N, R4N
    }
    public HardwareMap hardwareMap;
    public AutoMode autonomous_mode;
    public DcMotorEx lm;
    public DcMotorEx rm;
    public DcMotorEx lmelevator;
    public DcMotorEx rmelevator;
    public DcMotor harvestmotor;
    public IMU imu;
    public DcMotorEx rotation;

    public void runOpMode() {

        boolean auto = false;
        lm = hardwareMap.get(DcMotorEx.class, "blm");
        rm = hardwareMap.get(DcMotorEx.class, "brm");
        harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse");
        rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
        lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
        lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv");
        rmelevator.setDirection(DcMotor.Direction.REVERSE);
        rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        rm.setDirection(DcMotor.Direction.REVERSE);
        rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

        imu = hardwareMap.get(IMU.class, "imu");
        imu.initialize(
                new IMU.Parameters(
                        new RevHubOrientationOnRobot(
                                RevHubOrientationOnRobot.LogoFacingDirection.UP,
                                RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)));
        imu.resetYaw();
        YawPitchRollAngles robotOrientation;
        FTC2024WeRobotControl robot = new FTC2024WeRobotControl(this);

        telemetry.addData("wait for start", "");
        telemetry.update();

        while (opModeInInit()){
            imu.resetYaw();
            telemetry.addData("wait", "for start");
            telemetry.addData("Yaw", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
            telemetry.update();
        }
        
        while (opModeIsActive()) {
            if (gamepad1.a && !auto) {
                auto = true;
                break;
            }
        }
        if (opModeIsActive()) {
            /*
             * autonomous_mode differents possibles values respect the next scheme:
             * team_color_shortcode + start_line_index + direct_or_no
             *
             * team_color_shortcode = "b" for blue & "r" for red
             * start_line_index = 4 or 2 see competition manual appendix B Tile location
             * plan
             * direct_or_no = "d" to direct go to pixel deliver zone or "n" to harvest
             * pixels before to go in deliver zone
             *
             * default is "b4d"
             */

            switch (autonomous_mode) {
                default:
                    robot.boxElv();
                    robot.harvest(1);
                    robot.forward(2);
                    robot.harvest(0);
                    robot.rotate((-90));
                    robot.posBasse();
                    while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
                        telemetry.addData("pos", rotation.getCurrentPosition());
                        telemetry.update();
                    }
                    robot.forward(3.5);
                    robot.harvest(-1);
                    robot.backward(0.5);
                    robot.harvest(0);
                    robot.forward(0.5);
                    break;
                case B2D:
                    imu.resetYaw();
                    
                    robot.posBasse();
                    while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
                        telemetry.addData("pos", rotation.getCurrentPosition());
                        telemetry.addData("Yaw", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
                        telemetry.update();
                    }
                    robot.harvest(1);
                    robot.forward(0.5);
                    robot.harvest(0);
                    robot.forward(1);
                    robot.harvest(-1);
                    robot.backward(0.5);
                    robot.harvest(0);
                    robot.forward(0.5);
                    break;
                case R4D:
                    robot.boxElv();
                    robot.harvest(1);
                    robot.forward(1);
                    /*robot.harvest(0);
                    robot.rotate(90);
                    robot.posBasse();
                    while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
                        telemetry.addData("pos", rotation.getCurrentPosition());
                        telemetry.update();
                    }
                    robot.forward(3.5);
                    robot.harvest(-1);
                    robot.backward(0.5);
                    robot.harvest(0);
                    robot.forward(1);*/
                    break;
                case R2D:
                    robot.boxElv();
                    //rebuild
                    //robot.harvest(1);
                    robot.forward(1);
                    //robot.harvest(0);
                    /*robot.rotate(90);
                    robot.posBasse();
                    while (opModeIsActive() && rotation.getCurrentPosition() < rotation.getTargetPosition()) {
                        telemetry.addData("pos", rotation.getCurrentPosition());
                        telemetry.update();
                    }
                    robot.forward(1.5);
                    //robot.harvest(-1);
                    //robot.backward(0.5);
                    //robot.harvest(0);
                    //robot.forward(0.5);*/
                    break;
                case B4N:
                    robot.boxElv();
                    robot.forward(1.5);
                    robot.rotate(90);
                    robot.harvest();
                    robot.forward(3);
                    robot.harvest(0);
                    robot.rotate(180);
                    robot.forward(1);
                    robot.rotate(-90);
                    robot.forward(1);
                    robot.rotate(90);
                    robot.forward(2.5);
                    robot.harvest(-1);
                    robot.backward(1);
                    robot.harvest(0);
                    robot.forward(1);
                    break;
                case B2N:
                    robot.boxElv();
                    robot.forward(1.5);
                    robot.rotate(90);
                    robot.harvest();
                    robot.forward(1);
                    robot.harvest(0);
                    robot.forward(1);
                    robot.rotate(180);
                    robot.forward(1);
                    robot.rotate(-90);
                    robot.forward(1);
                    robot.rotate(90);
                    robot.forward(2.5);
                    robot.harvest(-1);
                    robot.backward(1);
                    robot.harvest(0);
                    robot.forward(1);
                    break;
                case R4N:
                    robot.boxElv();
                    robot.forward(1.5);
                    robot.rotate(-90);
                    robot.harvest();
                    robot.forward(3);
                    robot.harvest(0);
                    robot.rotate(180);
                    robot.forward(1);
                    robot.rotate(90);
                    robot.forward(1);
                    robot.rotate(-90);
                    robot.forward(2.5);
                    robot.harvest(-1);
                    robot.backward(1);
                    robot.harvest(0);
                    robot.forward(1);
                    break;
                case R2N:
                    robot.boxElv();
                    robot.forward(1.5);
                    robot.rotate(-90);
                    robot.harvest();
                    robot.forward(1);
                    robot.harvest(0);
                    robot.forward(1);
                    robot.rotate(180);
                    robot.forward(1);
                    robot.rotate(90);
                    robot.forward(1);
                    robot.rotate(-90);
                    robot.forward(2.5);
                    robot.harvest(-1);
                    robot.backward(1);
                    robot.harvest(0);
                    robot.forward(1);
                    break;
            }
        }
    }
}