From 335f8b3e893c177be7b4d68979bff186cb66b8bf Mon Sep 17 00:00:00 2001 From: Zelina974 Date: Mon, 20 May 2024 17:08:50 +0200 Subject: [PATCH] update --- fgc2024Pilotage.java | 42 ++++++++++ lastmatch.java | 193 ++++++++++++++++++++++++------------------- 2 files changed, 148 insertions(+), 87 deletions(-) create mode 100644 fgc2024Pilotage.java diff --git a/fgc2024Pilotage.java b/fgc2024Pilotage.java new file mode 100644 index 0000000..8e8683f --- /dev/null +++ b/fgc2024Pilotage.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import com.qualcomm.robotcore.robot.Robot; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.robot.Robot; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; + +@TeleOp(name = "WeRobot: FTC2024 NEW! Carlike", group = "WeRobot") +public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { + + private DcMotorEx rm; + private DcMotorEx lm; + + + @Override + public void runOpMode() throws InterruptedException { + + float x; // abscisse joystick gauche + double y; // ordonnées joystick gauche + + telemetry.addData("Status"," Initialized"); + + + } +} \ No newline at end of file diff --git a/lastmatch.java b/lastmatch.java index 965642f..1f7e171 100644 --- a/lastmatch.java +++ b/lastmatch.java @@ -3,6 +3,7 @@ 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.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.IMU; @@ -22,108 +23,126 @@ import com.qualcomm.robotcore.util.ElapsedTime; @Autonomous -public class Ftc2024_autonome_last extends LinearOpMode { - public enum AutoMode { - B2D, B4D - } +public class lastmatch extends LinearOpMode { - public AutoMode autonomous_mode = B2D; - public DcMotor lm; - public DcMotor rm; + public DcMotorEx lm; + public DcMotorEx rm; public DcMotorEx lmelevator; public DcMotorEx rmelevator; public DcMotor harvestmotor; public IMU imu; public DcMotorEx rotation; + private ElapsedTime timer; + @Override public void runOpMode() { + timer = new ElapsedTime(); + 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"); + rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + 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); - boolean auto = false; - lm = hardwareMap.get(DcMotor.class, "blm"); - rm = hardwareMap.get(DcMotor.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); - 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); - 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); - autonomous_mode = AutoMode.B4D; + telemetry.addData("wait for start", ""); + telemetry.update(); - telemetry.addData("wait for start", ""); - telemetry.update(); + waitForStart(); + telemetry.addData("started", ""); + telemetry.update(); + robotOrientation = imu.getRobotYawPitchRollAngles(); - waitForStart(); - telemetry.addData("started", ""); - telemetry.update(); - robotOrientation = imu.getRobotYawPitchRollAngles(); + while (opModeIsActive()) { + if (gamepad1.a && !auto) { + auto = true; + break; + } + } + if (opModeIsActive()) { + double motor_speed = 1.0; + lmelevator.setVelocity(600); + rmelevator.setVelocity(600); + lmelevator.setTargetPosition(200); + rmelevator.setTargetPosition(200); + rotation.setVelocity(600); + rotation.setTargetPosition(40); + lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + sleep(2000); + + lm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + double total_time = robot.time_for_dist(1 * robot.ground_tiles_width, Math.abs(motor_speed)); + timer.reset(); + while (opModeIsActive() && timer.seconds() < total_time) { + lm.setPower(motor_speed); + rm.setPower(motor_speed); + } + lm.setPower(0); + rm.setPower(0); + + harvestmotor.setPower(-0.6); + + lm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + total_time = robot.time_for_dist(0.7 * robot.ground_tiles_width, Math.abs(motor_speed)); + timer.reset(); + while (opModeIsActive() && timer.seconds() < total_time) { + lm.setPower(-motor_speed); + rm.setPower(-motor_speed); + } + lm.setPower(0); + rm.setPower(0); + + double angle = 90.0; + lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + double perimeter = Math.toRadians(angle)* 37.0/2.0; + int targetPos = (int) Math.floor(perimeter/(9e-2*Math.PI)); + targetPos = targetPos * 20; + rm.setTargetPosition(targetPos); + lm.setTargetPosition(-targetPos); + lm.setVelocity(600); + rm.setVelocity(600); + lm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + // while (Math.abs(lm.getCurrentPosition() - targetPos)>=2){} + + motor_speed= 1; + lm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + total_time = robot.time_for_dist(1.5 * robot.ground_tiles_width, Math.abs(motor_speed)); + timer.reset(); + while (opModeIsActive() && timer.seconds() < total_time) { + lm.setPower(motor_speed); + rm.setPower(motor_speed); + } + lm.setPower(0); + rm.setPower(0); - 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(); - * robot.forward(3.5); - * robot.harvest(-1); - * robot.backward(0.5); - * robot.harvest(0); - * robot.forward(0.5); - */ - break; - case B2D: - robot.posBasse(); - robot.harvest(1); - robot.forward(1); - robot.harvest(0); - /* - * robot.forward(1); - * robot.harvest(-1); - * robot.backward(0.5); - * robot.harvest(0); - * robot.forward(0.5); - */ - break; - } - } + } } }