diff --git a/FTC2024WeRobotControl.java b/FTC2024WeRobotControl.java index b486db4..6d8c944 100644 --- a/FTC2024WeRobotControl.java +++ b/FTC2024WeRobotControl.java @@ -38,7 +38,7 @@ public class FTC2024WeRobotControl { * the width size of the tiles on the ground in metres */ private final double ground_tiles_width = 61.0e-2; // metres - // + // private final double defaultspeed = 0.6; private ElapsedTime timer; @@ -54,8 +54,8 @@ public class FTC2024WeRobotControl { */ public FTC2024WeRobotControl(Ftc2024_autonome_api Parent) { - this.Parent = Parent; - this.timer = new ElapsedTime(); + this.Parent = Parent; + this.timer = new ElapsedTime(); } /* @@ -64,42 +64,29 @@ public class FTC2024WeRobotControl { * @param motor_speed = (optional) double between 0 and 1; motor power; default * to 1 */ - - public void boxElv(){ + + public void boxElv() { Parent.lmelevator.setVelocity(600); Parent.rmelevator.setVelocity(600); + Parent.lmelevator.setTargetPosition(90); + Parent.rmelevator.setTargetPosition(90); Parent.rotation.setVelocity(600); - Parent.lmelevator.setTargetPosition(50); - Parent.rmelevator.setTargetPosition(50); - Parent.rotation.setTargetPosition(30); + Parent.rotation.setTargetPosition(20); Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); - } - - public void boxElv() { - Parent.lmelevator.setVelocity(600); - Parent.rmelevator.setVelocity(600); - Parent.lmelevator.setTargetPosition(90); - Parent.rmelevator.setTargetPosition(90); - Parent.rotation.setVelocity(600); - Parent.rotation.setTargetPosition(-50); - Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); } - - public void posBasse(){ - Parent.lmelevator.setVelocity(600); - Parent.rmelevator.setVelocity(600); - Parent.rotation.setVelocity(600); - Parent.lmelevator.setTargetPosition(0); - Parent.rmelevator.setTargetPosition(0); - Parent.rotation.setTargetPosition(800); - Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); - Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + public void posBasse() { + Parent.lmelevator.setVelocity(600); + Parent.rmelevator.setVelocity(600); + Parent.rotation.setVelocity(600); + Parent.lmelevator.setTargetPosition(0); + Parent.rmelevator.setTargetPosition(0); + Parent.rotation.setTargetPosition(800); + Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); } /* @@ -110,12 +97,11 @@ public class FTC2024WeRobotControl { */ public double getSpeedFromMotorSpeed(double motor_speed) { - double speed_tour_par_minutes = this.tour_par_minutes * motor_speed; - double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter; - return speed; + double speed_tour_par_minutes = this.tour_par_minutes * motor_speed; + double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter; + return speed; } - /* * return the needed time for a distance * @@ -125,8 +111,8 @@ public class FTC2024WeRobotControl { * to 1 */ public double time_for_dist(double dist, double motor_speed) { - double speed = getSpeedFromMotorSpeed(motor_speed); - return (dist / speed); + double speed = getSpeedFromMotorSpeed(motor_speed); + return (dist / speed); } /* @@ -139,14 +125,14 @@ public class FTC2024WeRobotControl { * to 1 */ public void forward(double n_tiles, double motor_speed) { - double total_time = time_for_dist(n_tiles * ground_tiles_width, motor_speed); - timer.reset(); - while (Parent.opModeIsActive() && timer.seconds() < total_time) { - Parent.lm.setPower(motor_speed); - Parent.rm.setPower(motor_speed); - } - Parent.lm.setPower(0); - Parent.rm.setPower(0); + double total_time = time_for_dist(n_tiles * ground_tiles_width, Math.abs(motor_speed)); + timer.reset(); + while (Parent.opModeIsActive() && timer.seconds() < total_time) { + Parent.lm.setPower(motor_speed); + Parent.rm.setPower(motor_speed); + } + Parent.lm.setPower(0); + Parent.rm.setPower(0); } /* @@ -156,7 +142,7 @@ public class FTC2024WeRobotControl { * @param n_tiles number of tiles */ public void forward(double n_tiles) { - this.forward(n_tiles, this.defaultspeed); + this.forward(n_tiles, this.defaultspeed); } /* @@ -169,11 +155,11 @@ public class FTC2024WeRobotControl { * to 1 */ public void backward(double n_tiles, double motor_speed) { - forward(n_tiles, -motor_speed); + forward(n_tiles, -motor_speed); } public void backward(double n_tiles) { - this.backward(n_tiles, this.defaultspeed); + this.backward(n_tiles, this.defaultspeed); } /* @@ -183,11 +169,11 @@ public class FTC2024WeRobotControl { * to 1 */ public void harvest(double motor_speed) { - Parent.harvestmotor.setPower(motor_speed); + Parent.harvestmotor.setPower(motor_speed); } public void harvest() { - this.harvest(1.0); + this.harvest(1.0); } /* @@ -199,32 +185,32 @@ public class FTC2024WeRobotControl { * to 1 */ public void rotate(double angle, double motor_speed) { - robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); - double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - 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; - if (Math.abs(angle) == 180) { - angle = (double) (((double) Math.signum(angle)) * 179.9); - } - while (Parent.opModeIsActive() - && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))) { - robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); - double yaw = robotOrientation.getYaw(AngleUnit.DEGREES); - Parent.telemetry.addData("Yaw", yaw); - Parent.telemetry.update(); - m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw));// relative - Parent.lm.setPower(left_multiplier * m_power); - Parent.rm.setPower(right_multiplier * m_power); - } - Parent.lm.setPower(0); - Parent.rm.setPower(0); + robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); + double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + 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; + if (Math.abs(angle) == 180) { + angle = (double) (((double) Math.signum(angle)) * 179.9); + } + while (Parent.opModeIsActive() + && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))) { + robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); + double yaw = robotOrientation.getYaw(AngleUnit.DEGREES); + Parent.telemetry.addData("Yaw", yaw); + Parent.telemetry.update(); + m_power = (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw));// relative + Parent.lm.setPower(left_multiplier * m_power); + Parent.rm.setPower(right_multiplier * m_power); + } + Parent.lm.setPower(0); + Parent.rm.setPower(0); } public void rotate(double angle) { - this.rotate(angle, this.defaultspeed); + this.rotate(angle, this.defaultspeed); } } diff --git a/ftc2024_auto_r2d.java b/ftc2024_auto_r2d.java index aa60134..b549435 100644 --- a/ftc2024_auto_r2d.java +++ b/ftc2024_auto_r2d.java @@ -1,7 +1,18 @@ +package org.firstinspires.ftc.teamcode; +import org.firstinspires.ftc.teamcode.Ftc2024_autonome_api; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - package org.firstinspires.ftc.teamcode; - import org.firstinspires.ftc.teamcode.Ftc2024_autonome_api; - public class ftc2024_auto_r2d extends Ftc2024_autonome_api{ - public AutoMode autonomous_mode = AutoMode.R2D; - } +@Autonomous +public class ftc2024_auto_r2d extends LinearOpMode{ + //hi + public Ftc2024_autonome_api.AutoMode autonomous_mode = Ftc2024_autonome_api.AutoMode.R2D; + + @Override + public void runOpMode() { + Ftc2024_autonome_api a = new Ftc2024_autonome_api(); + a.hardwareMap = this.hardwareMap; + a.runOpMode(); + } +} \ No newline at end of file diff --git a/ftc2024_autonome_api.java b/ftc2024_autonome_api.java index 0aa970d..fc1d255 100644 --- a/ftc2024_autonome_api.java +++ b/ftc2024_autonome_api.java @@ -19,28 +19,29 @@ 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 extends LinearOpMode { - public enum AutoMode{ - B2D,B4D,B2N,B4N,R2D,R4D,R2N,R4N +public class Ftc2024_autonome_api{ + public enum AutoMode { + B2D, B4D, B2N, B4N, R2D, R4D, R2N, R4N } + public HardwareMap hardwareMap; public AutoMode autonomous_mode; - 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; - @Override public void runOpMode() { - - boolean auto = false; - lm = hardwareMap.get(DcMotor.class, "blm"); - rm = hardwareMap.get(DcMotor.class, "brm"); + + 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"); @@ -63,25 +64,24 @@ public class Ftc2024_autonome_api extends LinearOpMode { imu.resetYaw(); YawPitchRollAngles robotOrientation; FTC2024WeRobotControl robot = new FTC2024WeRobotControl(this); - autonomous_mode = AutoMode.B4D; telemetry.addData("wait for start", ""); telemetry.update(); - - - - waitForStart(); - telemetry.addData("started", ""); - telemetry.update(); - robotOrientation = imu.getRobotYawPitchRollAngles(); + 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){ + if (gamepad1.a && !auto) { auto = true; break; } } - if (opModeIsActive()){ + if (opModeIsActive()) { /* * autonomous_mode differents possibles values respect the next scheme: * team_color_shortcode + start_line_index + direct_or_no @@ -94,7 +94,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { * * default is "b4d" */ - + switch (autonomous_mode) { default: robot.boxElv(); @@ -103,6 +103,10 @@ public class Ftc2024_autonome_api extends LinearOpMode { /*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); @@ -110,7 +114,14 @@ public class Ftc2024_autonome_api extends LinearOpMode { 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(1); robot.harvest(0); @@ -127,6 +138,10 @@ public class Ftc2024_autonome_api extends LinearOpMode { 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); @@ -135,16 +150,21 @@ public class Ftc2024_autonome_api extends LinearOpMode { break; case R2D: robot.boxElv(); - robot.harvest(1); - robot.forward(0.5); - robot.harvest(0); + //rebuild + //robot.harvest(1); + robot.forward(1.5); + //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); + //robot.harvest(-1); + //robot.backward(0.5); + //robot.harvest(0); + //robot.forward(0.5); break; case B4N: robot.boxElv(); diff --git a/ftc_new.java b/ftc_new.java index 5948748..a896c2e 100644 --- a/ftc_new.java +++ b/ftc_new.java @@ -181,6 +181,7 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2))); telemetry.addData("Status", "Running"); + telemetry.addData("mode", mode); if (gamepad1.right_bumper && gamepad1.left_bumper){ @@ -556,7 +557,7 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { //telemetry.addData("Position rotation", rotation.getCurrentPosition()); //telemetry.addData("Position box", box.getCurrentPosition()); //telemetry.addData("box velocity", rotation.getVelocity()); - //telemetry.update(); + telemetry.update(); } }