From 26055358992c0a2f168c040142841a684117d51e Mon Sep 17 00:00:00 2001 From: Zelina974 <155895303+Zelina974@users.noreply.github.com> Date: Sun, 25 Feb 2024 16:03:04 +0100 Subject: [PATCH 01/20] Create IMU.java --- IMU.java | 184 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 184 insertions(+) create mode 100644 IMU.java diff --git a/IMU.java b/IMU.java new file mode 100644 index 0000000..6d21439 --- /dev/null +++ b/IMU.java @@ -0,0 +1,184 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + + +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.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +@TeleOp(name="Yale Holo", group="WeRobot") +public class YaleHolo extends LinearOpMode { + + float rotate_angle = 0; + double reset_angle = 0; + + private DcMotor front_left_wheel = null; + private DcMotor back_left_wheel = null; + private DcMotor back_right_wheel = null; + private DcMotor front_right_wheel = null; + + IMU imu; + @Override + public void runOpMode() { + front_left_wheel = hardwareMap.dcMotor.get("flm"); + back_left_wheel = hardwareMap.dcMotor.get("blm"); + back_right_wheel = hardwareMap.dcMotor.get("brm"); + front_right_wheel = hardwareMap.dcMotor.get("frm"); + + front_left_wheel.setDirection(DcMotor.Direction.REVERSE); + back_left_wheel.setDirection(DcMotor.Direction.REVERSE); + front_right_wheel.setDirection(DcMotor.Direction.FORWARD); + back_right_wheel.setDirection(DcMotor.Direction.FORWARD); + + front_left_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + back_left_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + front_right_wheel.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + back_right_wheel.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(); + + while(!opModeIsActive()){} + + while(opModeIsActive()){ + drive(); + resetAngle(); + //driveSimple(); + telemetry.update(); + } + } + public void driveSimple(){ + double power = .5; + if(gamepad1.dpad_up){ //Forward + front_left_wheel.setPower(-power); + back_left_wheel.setPower(-power); + back_right_wheel.setPower(-power); + front_right_wheel.setPower(-power); + } + else if(gamepad1.dpad_left){ //Left + front_left_wheel.setPower(power); + back_left_wheel.setPower(-power); + back_right_wheel.setPower(power); + front_right_wheel.setPower(-power); + } + else if(gamepad1.dpad_down){ //Back + front_left_wheel.setPower(power); + back_left_wheel.setPower(power); + back_right_wheel.setPower(power); + front_right_wheel.setPower(power); + } + else if(gamepad1.dpad_right){ //Right + front_left_wheel.setPower(-power); + back_left_wheel.setPower(power); + back_right_wheel.setPower(-power); + front_right_wheel.setPower(power); + } + else if(Math.abs(gamepad1.right_stick_x) > 0){ //Rotation + front_left_wheel.setPower(-gamepad1.right_stick_x); + back_left_wheel.setPower(-gamepad1.right_stick_x); + back_right_wheel.setPower(gamepad1.right_stick_x); + front_right_wheel.setPower(gamepad1.right_stick_x); + } + else{ + front_left_wheel.setPower(0); + back_left_wheel.setPower(0); + back_right_wheel.setPower(0); + front_right_wheel.setPower(0); + } + } + public void drive() { + double Protate = gamepad1.right_stick_x/4; + double stick_x = gamepad1.left_stick_x * Math.sqrt(Math.pow(1-Math.abs(Protate), 2)/2); //Accounts for Protate when limiting magnitude to be less than 1 + double stick_y = gamepad1.left_stick_y * Math.sqrt(Math.pow(1-Math.abs(Protate), 2)/2); + double theta = 0; + double Px = 0; + double Py = 0; + + double gyroAngle = getHeading() * Math.PI / 180; //Converts gyroAngle into radians + if (gyroAngle <= 0) { + gyroAngle = gyroAngle + (Math.PI / 2); + } else if (0 < gyroAngle && gyroAngle < Math.PI / 2) { + gyroAngle = gyroAngle + (Math.PI / 2); + } else if (Math.PI / 2 <= gyroAngle) { + gyroAngle = gyroAngle - (3 * Math.PI / 2); + } + gyroAngle = -1 * gyroAngle; + + if(gamepad1.right_bumper){ //Disables gyro, sets to -Math.PI/2 so front is defined correctly. + gyroAngle = -Math.PI/2; + } + + //Linear directions in case you want to do straight lines. + if(gamepad1.dpad_right){ + stick_x = 0.5; + } + else if(gamepad1.dpad_left){ + stick_x = -0.5; + } + if(gamepad1.dpad_up){ + stick_y = -0.5; + } + else if(gamepad1.dpad_down){ + stick_y = 0.5; + } + + + //MOVEMENT + theta = Math.atan2(stick_y, stick_x) - gyroAngle - (Math.PI / 2); + Px = Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2)) * (Math.sin(theta + Math.PI / 4)); + Py = Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2)) * (Math.sin(theta - Math.PI / 4)); + + telemetry.addData("Stick_X", stick_x); + telemetry.addData("Stick_Y", stick_y); + telemetry.addData("Magnitude", Math.sqrt(Math.pow(stick_x, 2) + Math.pow(stick_y, 2))); + telemetry.addData("Front Left", Py - Protate); + telemetry.addData("Back Left", Px - Protate); + telemetry.addData("Back Right", Py + Protate); + telemetry.addData("Front Right", Px + Protate); + + front_left_wheel.setPower(Py - Protate); + back_left_wheel.setPower(Px - Protate); + back_right_wheel.setPower(Py + Protate); + front_right_wheel.setPower(Px + Protate); + } + public void resetAngle(){ + if(gamepad1.a){ + reset_angle = getHeading() + reset_angle; + } + } + public double getHeading(){ + + + double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + if(heading < -180) { + heading = heading + 360; + } + else if(heading > 180){ + heading = heading - 360; + } + heading = heading - reset_angle; + return heading; + } +} + From ff8e4bf295e2e1293cf9d67b234d52f1e57d5167 Mon Sep 17 00:00:00 2001 From: Zelina974 Date: Sun, 25 Feb 2024 16:06:12 +0100 Subject: [PATCH 02/20] fixe errors --- ftc2024-autonome.java | 84 ++++++++++++++++++++++--------------------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java index ad0ac98..6dda8d5 100644 --- a/ftc2024-autonome.java +++ b/ftc2024-autonome.java @@ -17,6 +17,7 @@ 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.AxesOrder; import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; @@ -88,7 +89,7 @@ public class ftc2024_autonome extends LinearOpMode { telemetry.update(); yaw_sortie = Yaw; } - telemetry.addData("yaw_sortie", yaw_sortie) + telemetry.addData("yaw_sortie", yaw_sortie); runtime.reset(); while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) { lm.setPower(0.1); @@ -98,50 +99,50 @@ public class ftc2024_autonome extends LinearOpMode { } } 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); - } - + 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} + 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 Date: Sun, 25 Feb 2024 16:11:37 +0100 Subject: [PATCH 03/20] change --- ftc2024-autonome.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java index 6dda8d5..ebc6510 100644 --- a/ftc2024-autonome.java +++ b/ftc2024-autonome.java @@ -63,7 +63,7 @@ public class ftc2024_autonome extends LinearOpMode { 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; + boolean mode = false; YawPitchRollAngles robotOrientation; robotOrientation = imu.getRobotYawPitchRollAngles(); @@ -119,7 +119,7 @@ public class ftc2024_autonome extends LinearOpMode { {1.0,1.0}, {-1.0,1.0}, {-1.0,-1.0}, - {1.0,-1.0} + {1.0,-1.0} }; //mode Aurelien for(int i = 0; i Date: Mon, 26 Feb 2024 13:27:58 +0100 Subject: [PATCH 04/20] Create test.java --- test.java | 1 + 1 file changed, 1 insertion(+) create mode 100644 test.java diff --git a/test.java b/test.java new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/test.java @@ -0,0 +1 @@ + From 94118433a0ec0d0e1cbb7d0d8dd8c08b3d63f471 Mon Sep 17 00:00:00 2001 From: Zelina974 Date: Mon, 26 Feb 2024 14:10:42 +0100 Subject: [PATCH 05/20] change --- ftc2024-autonome.java | 132 ++++++++++++++++++++++++------------------ test.java | 68 ++++++++++++++++++++++ 2 files changed, 145 insertions(+), 55 deletions(-) diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java index ebc6510..9366d1a 100644 --- a/ftc2024-autonome.java +++ b/ftc2024-autonome.java @@ -17,7 +17,6 @@ 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.AxesOrder; import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; @@ -63,7 +62,7 @@ public class ftc2024_autonome extends LinearOpMode { 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 = false; + boolean mode = true; YawPitchRollAngles robotOrientation; robotOrientation = imu.getRobotYawPitchRollAngles(); @@ -72,7 +71,7 @@ public class ftc2024_autonome extends LinearOpMode { double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES); double Roll = robotOrientation.getRoll(AngleUnit.DEGREES); - double yaw_sortie; + double yaw_sortie; // Wait for the game to start (driver presses PLAY) waitForStart(); @@ -80,69 +79,92 @@ public class ftc2024_autonome extends LinearOpMode { runtime.reset(); if (mode){ //mode Elina - while (opModeIsActive() && Yaw <= 90.0) { - lm.setPower(0.5); - rm.setPower(-0.5); - robotOrientation = imu.getRobotYawPitchRollAngles(); + /*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(); - yaw_sortie = Yaw; + + }*/ + 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(); } - telemetry.addData("yaw_sortie", yaw_sortie); runtime.reset(); - while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) { + 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); - } + 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 Date: Mon, 26 Feb 2024 15:28:02 +0100 Subject: [PATCH 06/20] update --- test.java | 85 +++++++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 80 insertions(+), 5 deletions(-) diff --git a/test.java b/test.java index 22d1a8b..6939d97 100644 --- a/test.java +++ b/test.java @@ -45,25 +45,100 @@ public class ftc2024_autonome_test extends LinearOpMode { YawPitchRollAngles robotOrientation; double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double yaw_sortie1; - double yaw_sortie2; - double yaw_sortie3; - double yaw_sortie4; + double yaw_sortie; + double a; + double a1; + double b; + double b1; + double k; + double k1; waitForStart(); while (opModeIsActive()){ double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1}; double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1}; - for(int i = 0; i< p_t_g.length; i++){ + for(int i = 0; i< lm_p.length; i++){ while (opModeIsActive() && Yaw < 90){ lm.setPower = lm_p[i]; rm.setPower = rm_p[i]; Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); telemetry.addData("Yaw : ", Yaw); telemetry.update(); + yaw_sortie = Yaw; } + telemetry.addData("Yaw sortie", yaw_sortie); + telemetry.update(); + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double [] x = Yaw - yaw_sortie; + + + + /*if (i = 0) { + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + a = yaw_sortie; + a1 = Yaw; + } + if (i = 1){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + b = yaw_sortie; + b1 = Yaw; + } + if (i = 2){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double c = yaw_sortie; + double c1 = Yaw; + } + if (i = 3){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double d = yaw_sortie; + double d1 = Yaw; + } + if (i = 4){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double e = yaw_sortie; + double e1 = Yaw; + } + if (i = 5){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double f = yaw_sortie; + double f1 = Yaw; + } + if (i = 6){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double g = yaw_sortie; + double g1 = Yaw; + } + if (i = 7){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double h = yaw_sortie; + double h1 = Yaw; + } + if (i = 8){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + double j = yaw_sortie; + double j1 = Yaw; + } + if (i = 9){ + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + k = yaw_sortie; + k1 = Yaw; + } + */ + imu.resetYaw(); } + + + telemetry.addData("0.1", x[0]); + telemetry.addData("0.2", x[1]); + telemetry.addData("0.3", x[2]); + telemetry.addData("0.4", x[3]); + telemetry.addData("0.5", x[4]); + telemetry.addData("0.6", x[5]); + telemetry.addData("0.7", x[6]); + telemetry.addData("0.8", x[7]); + telemetry.addData("0.9", x[8]); + telemetry.addData("1", x[9]); } } } \ No newline at end of file From df33d5290cc4bc5b501c40b83cac19ee4c7d9a32 Mon Sep 17 00:00:00 2001 From: Zelina974 Date: Mon, 26 Feb 2024 16:01:15 +0100 Subject: [PATCH 07/20] update --- test.java | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/test.java b/test.java index 6939d97..3b5d6c9 100644 --- a/test.java +++ b/test.java @@ -46,18 +46,13 @@ public class ftc2024_autonome_test extends LinearOpMode { YawPitchRollAngles robotOrientation; double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); double yaw_sortie; - double a; - double a1; - double b; - double b1; - double k; - double k1; waitForStart(); while (opModeIsActive()){ double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1}; double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1}; + double [] x = new double[lm_p.length]; for(int i = 0; i< lm_p.length; i++){ while (opModeIsActive() && Yaw < 90){ lm.setPower = lm_p[i]; @@ -70,7 +65,7 @@ public class ftc2024_autonome_test extends LinearOpMode { telemetry.addData("Yaw sortie", yaw_sortie); telemetry.update(); Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double [] x = Yaw - yaw_sortie; + x [i]= Yaw - yaw_sortie; From da49742d7ce3e2d30187bbc3bc91e34168100e7d Mon Sep 17 00:00:00 2001 From: Zelina974 Date: Mon, 26 Feb 2024 16:06:04 +0100 Subject: [PATCH 08/20] fix --- test.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/test.java b/test.java index 3b5d6c9..80a4baa 100644 --- a/test.java +++ b/test.java @@ -30,7 +30,7 @@ public class ftc2024_autonome_test extends LinearOpMode { lm = hardwareMap.get (DcMotor.class, "lm"); rm = hardwareMap.get (DcMotor.class, "rm"); - rm.setDirection(DcMotorSimple.Direction.REVERSE); + rm.setDirection(DcMotor.Direction.REVERSE); imu = hardwareMap.get(IMU.class, "imu"); imu.initialize( @@ -42,10 +42,10 @@ public class ftc2024_autonome_test extends LinearOpMode { ) ); imu.resetYaw(); - YawPitchRollAngles robotOrientation; + robotOrientation = imu.getRobotYawPitchRollAngles(); double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double yaw_sortie; + double yaw_sortie = 0.0; waitForStart(); @@ -55,8 +55,8 @@ public class ftc2024_autonome_test extends LinearOpMode { double [] x = new double[lm_p.length]; for(int i = 0; i< lm_p.length; i++){ while (opModeIsActive() && Yaw < 90){ - lm.setPower = lm_p[i]; - rm.setPower = rm_p[i]; + lm.setPower(lm_p[i]); + rm.setPower(rm_p[i]); Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); telemetry.addData("Yaw : ", Yaw); telemetry.update(); From 3f523e5e12b438500a09152cd53e707d7b879246 Mon Sep 17 00:00:00 2001 From: Zelina974 Date: Mon, 26 Feb 2024 16:15:47 +0100 Subject: [PATCH 09/20] fix --- ftc2024-autonome.java | 242 +++++++++++++++++------------------------- 1 file changed, 99 insertions(+), 143 deletions(-) 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 Date: Mon, 26 Feb 2024 16:35:33 +0100 Subject: [PATCH 10/20] update --- ftc2024-autonome.java | 84 +++++++++---------------------------------- 1 file changed, 17 insertions(+), 67 deletions(-) diff --git a/ftc2024-autonome.java b/ftc2024-autonome.java index b61d737..bfc06ed 100644 --- a/ftc2024-autonome.java +++ b/ftc2024-autonome.java @@ -53,13 +53,13 @@ public class ftc2024_autonome_test extends LinearOpMode { double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1}; double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1}; double [] x = new double[lm_p.length]; - for(int i = 0; i< lm_p.length; i++){ + for(int i = 0; i< 9; i++){ while (opModeIsActive() && Yaw < 90){ lm.setPower(lm_p[i]); rm.setPower(rm_p[i]); robotOrientation = imu.getRobotYawPitchRollAngles(); Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - telemetry.addData("Yaw : ", Yaw); + telemetry.addData("Yaw ", Yaw); telemetry.update(); yaw_sortie = Yaw; } @@ -67,74 +67,24 @@ public class ftc2024_autonome_test extends LinearOpMode { telemetry.update(); Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); x [i]= Yaw - yaw_sortie; - - - /*if (i = 0) { - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - a = yaw_sortie; - a1 = Yaw; - } - if (i = 1){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - b = yaw_sortie; - b1 = Yaw; - } - if (i = 2){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double c = yaw_sortie; - double c1 = Yaw; - } - if (i = 3){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double d = yaw_sortie; - double d1 = Yaw; - } - if (i = 4){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double e = yaw_sortie; - double e1 = Yaw; - } - if (i = 5){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double f = yaw_sortie; - double f1 = Yaw; - } - if (i = 6){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double g = yaw_sortie; - double g1 = Yaw; - } - if (i = 7){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double h = yaw_sortie; - double h1 = Yaw; - } - if (i = 8){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double j = yaw_sortie; - double j1 = Yaw; - } - if (i = 9){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - k = yaw_sortie; - k1 = Yaw; - } - */ imu.resetYaw(); + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + telemetry.addData("Yaw", Yaw); + telemetry.update(); + } + while (opModeIsActive()){ + telemetry.addData("0.1", x[0]); + telemetry.addData("0.2", x[1]); + telemetry.addData("0.3", x[2]); + telemetry.addData("0.4", x[3]); + telemetry.addData("0.5", x[4]); + telemetry.addData("0.6", x[5]); + telemetry.addData("0.7", x[6]); + telemetry.addData("0.8", x[7]); + telemetry.addData("0.9", x[8]); + telemetry.addData("1", x[9]); } - - - telemetry.addData("0.1", x[0]); - telemetry.addData("0.2", x[1]); - telemetry.addData("0.3", x[2]); - telemetry.addData("0.4", x[3]); - telemetry.addData("0.5", x[4]); - telemetry.addData("0.6", x[5]); - telemetry.addData("0.7", x[6]); - telemetry.addData("0.8", x[7]); - telemetry.addData("0.9", x[8]); - telemetry.addData("1", x[9]); } } } \ No newline at end of file From 7b7944424f7f350064405761d4e8d6a9647894f5 Mon Sep 17 00:00:00 2001 From: Zelina974 Date: Wed, 28 Feb 2024 12:08:05 +0100 Subject: [PATCH 11/20] update --- test.java | 107 ++++++++++++++++++------------------------------------ 1 file changed, 35 insertions(+), 72 deletions(-) diff --git a/test.java b/test.java index 80a4baa..3b9b3f8 100644 --- a/test.java +++ b/test.java @@ -27,8 +27,8 @@ public class ftc2024_autonome_test extends LinearOpMode { @Override public void runOpMode() { - lm = hardwareMap.get (DcMotor.class, "lm"); - rm = hardwareMap.get (DcMotor.class, "rm"); + lm = hardwareMap.get (DcMotor.class, "blm"); + rm = hardwareMap.get (DcMotor.class, "brm"); rm.setDirection(DcMotor.Direction.REVERSE); @@ -52,88 +52,51 @@ public class ftc2024_autonome_test extends LinearOpMode { while (opModeIsActive()){ double [] lm_p = {0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9,1}; double [] rm_p = {-0.1,-0.2,-0.3,-0.4,-0.5,-0.6,-0.7,-0.8,-0.9,-1}; + double [] y = new double[lm_p.length]; double [] x = new double[lm_p.length]; + for(int i = 0; i< lm_p.length; i++){ while (opModeIsActive() && Yaw < 90){ lm.setPower(lm_p[i]); rm.setPower(rm_p[i]); + robotOrientation = imu.getRobotYawPitchRollAngles(); Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - telemetry.addData("Yaw : ", Yaw); + telemetry.addData("Yaw ", Yaw); + telemetry.addData("I", i); telemetry.update(); - yaw_sortie = Yaw; - } - telemetry.addData("Yaw sortie", yaw_sortie); - telemetry.update(); + } + + lm.setPower(0); + rm.setPower(0); + + robotOrientation = imu.getRobotYawPitchRollAngles(); Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - x [i]= Yaw - yaw_sortie; - + x [i]= Yaw - 90.0; - /*if (i = 0) { - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - a = yaw_sortie; - a1 = Yaw; - } - if (i = 1){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - b = yaw_sortie; - b1 = Yaw; - } - if (i = 2){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double c = yaw_sortie; - double c1 = Yaw; - } - if (i = 3){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double d = yaw_sortie; - double d1 = Yaw; - } - if (i = 4){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double e = yaw_sortie; - double e1 = Yaw; - } - if (i = 5){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double f = yaw_sortie; - double f1 = Yaw; - } - if (i = 6){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double g = yaw_sortie; - double g1 = Yaw; - } - if (i = 7){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double h = yaw_sortie; - double h1 = Yaw; - } - if (i = 8){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - double j = yaw_sortie; - double j1 = Yaw; - } - if (i = 9){ - Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - k = yaw_sortie; - k1 = Yaw; - } - */ imu.resetYaw(); + robotOrientation = imu.getRobotYawPitchRollAngles(); + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + telemetry.addData("Yaw", Yaw); + telemetry.update(); + /* + Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + telemetry.addData("Yaw", Yaw); + telemetry.update();*/ + } + while (opModeIsActive()){ + telemetry.addData("0.1", x[0]); + telemetry.addData("0.2", x[1]); + telemetry.addData("0.3", x[2]); + telemetry.addData("0.4", x[3]); + telemetry.addData("0.5", x[4]); + telemetry.addData("0.6", x[5]); + telemetry.addData("0.7", x[6]); + telemetry.addData("0.8", x[7]); + telemetry.addData("0.9", x[8]); + telemetry.addData("1", x[9]); + telemetry.update(); } - - - telemetry.addData("0.1", x[0]); - telemetry.addData("0.2", x[1]); - telemetry.addData("0.3", x[2]); - telemetry.addData("0.4", x[3]); - telemetry.addData("0.5", x[4]); - telemetry.addData("0.6", x[5]); - telemetry.addData("0.7", x[6]); - telemetry.addData("0.8", x[7]); - telemetry.addData("0.9", x[8]); - telemetry.addData("1", x[9]); } } } \ No newline at end of file From 94529dcd758e6f1d6cfc2d3d9717b43e51887839 Mon Sep 17 00:00:00 2001 From: GZod01 Date: Sun, 3 Mar 2024 13:36:23 +0100 Subject: [PATCH 12/20] Update helloworld.py.java --- helloworld.py.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/helloworld.py.java b/helloworld.py.java index ad926e0..f20944e 100644 --- a/helloworld.py.java +++ b/helloworld.py.java @@ -1,4 +1,7 @@ -String pythonCode = """abc = \"\"\" +String pythonCode = "code in comment"; +/* +```py +abc = """ booledan already_//CURR// = false; if(gamepad1.//CURR// && !already_//CURR//){ already_//CURR// =! already_//CURR//; @@ -8,7 +11,7 @@ if (!gamepad1.//CURR// && already_//CURR//){ already_//CURR// =! already_//CURR//; } -\"\"\"; +"""; defg = ""; hijk = "abcdefghijklmnopqrstuvwxyz"; @@ -17,4 +20,5 @@ for i in hijk: defg += abc.replace("//CURR//",i); print(defg); -"""; +``` +*/ From 1983c91c46227dbf440648d932a117bf578d9904 Mon Sep 17 00:00:00 2001 From: GZod01 Date: Thu, 7 Mar 2024 14:54:46 +0100 Subject: [PATCH 13/20] Create autonome_api.java --- autonome_api.java | 1 + 1 file changed, 1 insertion(+) create mode 100644 autonome_api.java diff --git a/autonome_api.java b/autonome_api.java new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/autonome_api.java @@ -0,0 +1 @@ + From da8cd904db146562749fe0193553272670cc592d Mon Sep 17 00:00:00 2001 From: GZod01 Date: Thu, 7 Mar 2024 15:15:33 +0100 Subject: [PATCH 14/20] Update autonome_api.java --- autonome_api.java | 116 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 116 insertions(+) diff --git a/autonome_api.java b/autonome_api.java index 8b13789..5cb33b1 100644 --- a/autonome_api.java +++ b/autonome_api.java @@ -1 +1,117 @@ +package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +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; + +@Autonomous + +public class ftc2024_autonome_api extends LinearOpMode { + private DcMotor rm; + private DcMotor lm; + private IMU imu; + private static double wheel_width = 9.0e-2; // metres + private static double wheel_perimeter = Math.PI * wheel_width; + private static double tour_par_minutes = 300.0; + private static double ground_tiles_width = 61.0e-2; // metres + /* + * return a metre/sec speed + * @param motor_speed = double between 0 and 1 + */ + public double getSpeedFromMotorSpeed(double motor_speed = 1.0){ + double speed_tour_par_minutes = this.tour_par_minutes*motor_speed; + double speed = (speed_tour_par_minutes/60)*this.wheel_perimeter; + return speed; + } + public double time_for_dist(double dist, double motor_speed=1.0){ + double speed = getSpeedFromMotorSpeed(motor_speed); + return (dist/speed); + } + public void forward(double n_tiles, double motor_speed = 1.0){ + double total_time = time_for_dist(n_tiles, motor_speed); + double start_time = runtime.seconds(); + while( opModeIsActive() && ((runtime.seconds()-start_time) Date: Thu, 7 Mar 2024 15:38:51 +0100 Subject: [PATCH 15/20] Update autonome_api.java --- autonome_api.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/autonome_api.java b/autonome_api.java index 5cb33b1..ab6acc3 100644 --- a/autonome_api.java +++ b/autonome_api.java @@ -50,6 +50,20 @@ public class ftc2024_autonome_api extends LinearOpMode { lm.setPower(0); rm.setPower(0); } + public void rotate(double angle, double motor_speed=1.0){ + double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + angle = Math.toDegrees(Math.atan(Math.tan(Math.toRadians(angle)))); + double left_multiplier = -( (double) Math.signum(angle)); + double right_multiplier = ((double) Math.signum(angle)); + double m_power = motor_speed; + while(opModeIsActive() && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle)){ + m_power = //relative + lm.setPower(left_multiplier*m_power); + rm.setPower(right_multiplier*m_power); + } + lm.setPower(0); + rm.setPower(0); + } @Override From fe5e982adf18cfdb4d23f28f5b3ed3b54dbd22fe Mon Sep 17 00:00:00 2001 From: GZod01 Date: Thu, 7 Mar 2024 15:50:00 +0100 Subject: [PATCH 16/20] Update autonome_api.java --- autonome_api.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonome_api.java b/autonome_api.java index ab6acc3..3642b5a 100644 --- a/autonome_api.java +++ b/autonome_api.java @@ -57,7 +57,7 @@ public class ftc2024_autonome_api extends LinearOpMode { double right_multiplier = ((double) Math.signum(angle)); double m_power = motor_speed; while(opModeIsActive() && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle)){ - m_power = //relative + m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative lm.setPower(left_multiplier*m_power); rm.setPower(right_multiplier*m_power); } From ce1f5a5741ec80bfb73fdd5467ba911b822dd52e Mon Sep 17 00:00:00 2001 From: GZod01 Date: Thu, 7 Mar 2024 16:21:56 +0100 Subject: [PATCH 17/20] Update autonome_api.java --- autonome_api.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/autonome_api.java b/autonome_api.java index 3642b5a..ae902e8 100644 --- a/autonome_api.java +++ b/autonome_api.java @@ -52,7 +52,9 @@ public class ftc2024_autonome_api extends LinearOpMode { } public void rotate(double angle, double motor_speed=1.0){ double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - angle = Math.toDegrees(Math.atan(Math.tan(Math.toRadians(angle)))); + angle = 200.0; + double anglerad = Math.toRadians(angle); + angle = Math.toDegrees(Math.atan2(Math.sin(anglerad),Math.cos(anglerad))); double left_multiplier = -( (double) Math.signum(angle)); double right_multiplier = ((double) Math.signum(angle)); double m_power = motor_speed; From e5d1c81fbc07afa360f7f1f357392af6914d3148 Mon Sep 17 00:00:00 2001 From: GZod01 Date: Thu, 7 Mar 2024 16:28:23 +0100 Subject: [PATCH 18/20] Update autonome_api.java --- autonome_api.java | 51 +++++++++-------------------------------------- 1 file changed, 9 insertions(+), 42 deletions(-) diff --git a/autonome_api.java b/autonome_api.java index ae902e8..c27825c 100644 --- a/autonome_api.java +++ b/autonome_api.java @@ -23,10 +23,10 @@ public class ftc2024_autonome_api extends LinearOpMode { private DcMotor rm; private DcMotor lm; private IMU imu; - private static double wheel_width = 9.0e-2; // metres - private static double wheel_perimeter = Math.PI * wheel_width; - private static double tour_par_minutes = 300.0; - private static double ground_tiles_width = 61.0e-2; // metres + private final double wheel_width = 9.0e-2; // metres + private final double wheel_perimeter = Math.PI * wheel_width; + private final double tour_par_minutes = 300.0; + private final double ground_tiles_width = 61.0e-2; // metres /* * return a metre/sec speed * @param motor_speed = double between 0 and 1 @@ -41,7 +41,7 @@ public class ftc2024_autonome_api extends LinearOpMode { return (dist/speed); } public void forward(double n_tiles, double motor_speed = 1.0){ - double total_time = time_for_dist(n_tiles, motor_speed); + double total_time = time_for_dist(n_tiles*ground_tiles_width, motor_speed); double start_time = runtime.seconds(); while( opModeIsActive() && ((runtime.seconds()-start_time) Date: Thu, 7 Mar 2024 16:31:51 +0100 Subject: [PATCH 19/20] Update autonome_api.java --- autonome_api.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonome_api.java b/autonome_api.java index c27825c..bd3b284 100644 --- a/autonome_api.java +++ b/autonome_api.java @@ -93,8 +93,8 @@ public class ftc2024_autonome_api extends LinearOpMode { waitForStart(); if(opModeIsRunning()){ - forward(2); - rotate(90); + forward(0.5); + rotate(-90); forward(2); } } From 7cd7be1f14eec5f1be12b9f60543771de58ca782 Mon Sep 17 00:00:00 2001 From: GZod01 Date: Thu, 7 Mar 2024 16:38:24 +0100 Subject: [PATCH 20/20] Update autonome_api.java --- autonome_api.java | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/autonome_api.java b/autonome_api.java index bd3b284..7437fda 100644 --- a/autonome_api.java +++ b/autonome_api.java @@ -22,6 +22,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; public class ftc2024_autonome_api extends LinearOpMode { private DcMotor rm; private DcMotor lm; + private DcMotor harvestmotor; private IMU imu; private final double wheel_width = 9.0e-2; // metres private final double wheel_perimeter = Math.PI * wheel_width; @@ -50,6 +51,12 @@ public class ftc2024_autonome_api extends LinearOpMode { lm.setPower(0); rm.setPower(0); } + public void backward(double n_tiles, double motor_speed= 1.0){ + forward(n_tiles, -motor_speed); + } + public void harvest(double motor_speed=0.0){ + harvestmotor.setPower(motor_speed); + } public void rotate(double angle, double motor_speed=1.0){ double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES); angle = 200.0; @@ -72,6 +79,7 @@ public class ftc2024_autonome_api extends LinearOpMode { public void runOpMode() { lm = hardwareMap.get (DcMotor.class, "blm"); rm = hardwareMap.get (DcMotor.class, "brm"); + harvestmotor = hardwareMap.get(DcMotor.class, "flm"); rm.setDirection(DcMotor.Direction.REVERSE); @@ -94,7 +102,10 @@ public class ftc2024_autonome_api extends LinearOpMode { if(opModeIsRunning()){ forward(0.5); - rotate(-90); - forward(2); + rotate(-90.0); + forward(1.5); + harvest(-1); + backward(1); + harvest(0); } }