diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java index 9366d1a..b61d737 100644 --- a/ftc2024-autonome.java +++ b/ftc2024-autonome.java @@ -1,47 +1,36 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.hardware.Gyroscope; -import com.qualcomm.robotcore.hardware.ColorSensor; -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.hardware.DigitalChannel; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.util.ElapsedTime; 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.AngleUnit; + +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.Orientation; -import org.firstinspires.ftc.robotcore.external.navigation.Position; -import org.firstinspires.ftc.robotcore.external.navigation.Velocity; +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; @Autonomous -public class ftc2024_autonome extends LinearOpMode { +public class ftc2024_autonome_test extends LinearOpMode { private DcMotor rm; private DcMotor lm; private IMU imu; - private ElapsedTime runtime = new ElapsedTime(); - - - public double time_for_dist(double speed, double dist){ - return (double) (dist/speed); - } @Override + public void runOpMode() { - lm = hardwareMap.get(DcMotor.class, "blm"); - rm = hardwareMap.get(DcMotor.class, "brm"); + lm = hardwareMap.get (DcMotor.class, "blm"); + rm = hardwareMap.get (DcMotor.class, "brm"); + + rm.setDirection(DcMotor.Direction.REVERSE); imu = hardwareMap.get(IMU.class, "imu"); imu.initialize( @@ -53,132 +42,99 @@ public class ftc2024_autonome extends LinearOpMode { ) ); imu.resetYaw(); - - rm.setDirection(DcMotorSimple.Direction.REVERSE); - telemetry.addData("Status", "Initialized"); - telemetry.update(); - double tour_par_minute = 300.0; - double wheel_width = 9.0e-2; - double wheel_rayon = (wheel_width)/2; - double wheel_perimeter = wheel_rayon*2*Math.PI; - double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second - boolean mode = true; - YawPitchRollAngles robotOrientation; robotOrientation = imu.getRobotYawPitchRollAngles(); - double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES); - double Roll = robotOrientation.getRoll(AngleUnit.DEGREES); + double yaw_sortie = 0.0; - double yaw_sortie; - - // Wait for the game to start (driver presses PLAY) waitForStart(); - runtime.reset(); - if (mode){ - //mode Elina - /*while (opModeIsActive() && Yaw <= -90.0) { - lm.setPower(0.2); - rm.setPower(-0.2); - robotOrientation = imu.getRobotYawPitchRollAngles(); - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - telemetry.addData("Yaw : ", Yaw); - telemetry.update(); - - }*/ - while (opModeIsActive() && Yaw < 90) { - lm.setPower(0.2); - rm.setPower(-0.2); - robotOrientation = imu.getRobotYawPitchRollAngles(); - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - telemetry.addData("Leg 1", runtime.seconds()); - telemetry.addData("Yaw", Yaw); - telemetry.update(); - } - runtime.reset(); - yaw_sortie = Yaw; - - telemetry.update(); - - while (opModeIsActive()) { - lm.setPower(0.1); - rm.setPower(0.1); - robotOrientation = imu.getRobotYawPitchRollAngles(); - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - telemetry.addData("yaw_sortie", yaw_sortie); - telemetry.addData("Yaw", Yaw); - telemetry.update(); - - } - /*while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) { - lm.setPower(0.1); - rm.setPower(0.1); - telemetry.addData("Leg 2", runtime.seconds()); - telemetry.update(); - }*/ - } - else{ - while(opModeIsActive()){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - if(Math.abs(Yaw-90.0)<=0.01){ - break; - } - else if((Yaw - 90.0) <0){ - lm.setPower((Math.abs(Yaw-90.0)/90)*0.5); - rm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); - } - else{ - rm.setPower((Math.abs(Yaw-90.0)/90)*0.5); - lm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); - } - - if(false){ - double[][] operations = { - {-1.0,1.0}, // vectors - {1.0,1.0}, - {-1.0,1.0}, - {-1.0,-1.0}, - {1.0,-1.0} - }; - //mode Aurelien - for(int i = 0; i