diff --git a/FTC2024WeRobotControl.java b/FTC2024WeRobotControl.java index bb75dfe..fe78d18 100644 --- a/FTC2024WeRobotControl.java +++ b/FTC2024WeRobotControl.java @@ -1,4 +1,4 @@ -package fr.werobot.ftc2024.robotcontrol;//a tester car pas sur que ça fonctionne +package org.firstinspires.ftc.teamcode;//a tester car pas sur que ça fonctionne import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -21,7 +21,7 @@ public class FTC2024WeRobotControl { /* * The Parent class {@see FTC2024WeRobotControl constructor} */ - private LinearOpMode Parent; + private Ftc2024_autonome_api Parent; /* * the wheel width in metres */ @@ -38,6 +38,11 @@ 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 ElapsedTime timer; + + private YawPitchRollAngles robotOrientation; + /* * construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class * for the FTC2024 @@ -46,17 +51,11 @@ public class FTC2024WeRobotControl { * constructing the class directly in */ - public FTC2024WeRobotControl(LinearOpMode Parent) { + public FTC2024WeRobotControl(Ftc2024_autonome_api Parent) { this.Parent = Parent; + this.timer = new ElapsedTime(); } - /* - * test if motorSpeed is >= 0 - */ - public void testMotorSpeed(double motor_speed){ - if(!((double)motor_speed>=0.0)){ - throw new Exception("Motor Speed MUST be >= 0"); - } - } + /* * return a metre/sec speed * @@ -64,14 +63,10 @@ public class FTC2024WeRobotControl { * to 1 */ public double getSpeedFromMotorSpeed(double motor_speed) { - testMotorSpeed(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; } - public double getSpeedFromMotorSpeed(){ - this.getSpeedFromMotorSpeed(1); - } /* * return the needed time for a distance @@ -82,13 +77,9 @@ public class FTC2024WeRobotControl { * to 1 */ public double time_for_dist(double dist, double motor_speed) { - testMotorSpeed(motor_speed); double speed = getSpeedFromMotorSpeed(motor_speed); return (dist / speed); } - public double time_for_dist(double dist){ - this.time_for_dist(dist,1); - } /* * go forward @@ -100,10 +91,9 @@ public class FTC2024WeRobotControl { * to 1 */ public void forward(double n_tiles, double motor_speed) { - testMotorSpeed(motor_speed); double total_time = time_for_dist(n_tiles * ground_tiles_width, motor_speed); - double start_time = Parent.runtime.seconds(); - while (Parent.opModeIsActive() && ((Parent.runtime.seconds() - start_time) < total_time)) { + timer.reset(); + while (Parent.opModeIsActive() && timer.seconds() < total_time) { Parent.lm.setPower(motor_speed); Parent.rm.setPower(motor_speed); } @@ -125,11 +115,11 @@ public class FTC2024WeRobotControl { * to 1 */ public void backward(double n_tiles, double motor_speed) { - testMotorSpeed(motor_speed); forward(n_tiles, -motor_speed); } + public void backward(double n_tiles){ - this.forward(n_tiles,1); + this.backward(n_tiles,1); } /* @@ -141,9 +131,7 @@ public class FTC2024WeRobotControl { public void harvest(double motor_speed) { Parent.harvestmotor.setPower(motor_speed); } - public void harvest(){ - this.harvest(1); - } + /* * harvest * @@ -153,24 +141,24 @@ public class FTC2024WeRobotControl { * to 1 */ public void rotate(double angle, double motor_speed){ - Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); - double start_yaw = Parent.robotOrientation.getYaw(AngleUnit.DEGREES); + robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); + double start_yaw = robotOrientation.getYaw(AngleUnit.DEGREES); 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; - while(Parent.opModeIsActive() && (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))){ - Parent.robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); - m_power = (Math.abs(Parent.robotOrientation.getYaw(AngleUnit.DEGREES)-start_yaw));//relative + while(Parent.opModeIsActive() && (Math.abs(robotOrientation.getYaw(AngleUnit.DEGREES) - start_yaw) < Math.abs(angle))){ + robotOrientation = Parent.imu.getRobotYawPitchRollAngles(); + 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(douvle angle){ - this.rotate(tour_par_minutes, 1); + public void rotate(double angle){ + this.rotate(angle,1.0); } } diff --git a/ftc2024_autonome_api.java b/ftc2024_autonome_api.java index 45321dd..7126dec 100644 --- a/ftc2024_autonome_api.java +++ b/ftc2024_autonome_api.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode; -import fr.werobot.ftc2024.robotcontrol.FTC2024WeRobotControl; //a tester car pas sur que ça fonctionne +//import FTC2024WeRobotControl; //a tester car pas sur que ça fonctionne import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -21,147 +21,151 @@ import com.qualcomm.robotcore.util.ElapsedTime; @Autonomous -public class ftc2024_autonome_api extends LinearOpMode { - public String autonomous_mode; - public DcMotor lm; - public DcMotor rm; - public DcMotor harvestmotor; - public IMU imu; - public YawPitchRollAngle robotOrientation; - - @Override - public void runOpMode() { - lm = hardwareMap.get(DcMotor.class, "blm"); - rm = hardwareMap.get(DcMotor.class, "brm"); - harvestmotor = hardwareMap.get(DcMotor.class, "flm"); +public class Ftc2024_autonome_api extends LinearOpMode { + public String autonomous_mode; + public DcMotor lm; + public DcMotor rm; + public DcMotor harvestmotor; + public IMU imu; - rm.setDirection(DcMotor.Direction.REVERSE); + @Override + public void runOpMode() { + lm = hardwareMap.get(DcMotor.class, "blm"); + rm = hardwareMap.get(DcMotor.class, "brm"); + harvestmotor = hardwareMap.get(DcMotor.class, "flm"); - imu = hardwareMap.get(IMU.class, "imu"); - imu.initialize( - new IMU.Parameters( - new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.UP, - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD - ) - ) - ); - imu.resetYaw(); - // YawPitchRollAngle robotOrientation; // FTC2024WeRobotControl à besoin de robotOrientation comme variable de Parent (donc de cette classe actuelle), n'est il pas posible de définir robotOrientation dans la classe? - FTC2024WeRobotControl robot = FTC2024WeRobotControl(this); + rm.setDirection(DcMotor.Direction.REVERSE); - waitForStart(); - robotOrientation = imu.getYawPitchRollAngles(); + 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 = "b4d"; - if(opModeIsRunning()){ - /* - * 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.forward(0.5); - robot.rotate((-90)); - robot.forward(1.5); - robot.harvest(-1); - robot.backward(1); - robot.harvest(0); - break; - case ("b2d"): - robot.forward(0.5); - robot.rotate((-90)); - robot.forward(2.5); - robot.harvest(-1); - robot.backward(1); - robot.harvest(0); - break; - case ("r4d"): - robot.forward(0.5); - robot.rotate(90); - robot.forward(1.5); - robot.harvest(-1); - robot.backward(1); - robot.harvest(0); - break; - case ("r2d"): - robot.forward(0.5); - robot.rotate(90); - robot.forward(2.5); - robot.harvest(-1); - robot.backward(1); - robot.harvest(0); - break; - case ("b4n"): - 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); - break; - case ("b2n"): - robot.forward(1.5); - robot.rotate(90); - robot.harvest(); - robot.forward(1); - 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); - break; - case ("r4n"): - 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); - break; - case ("r2n"): - robot.forward(1.5); - robot.rotate(-90); - robot.harvest(); - robot.forward(1); - 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); - break; - } + telemetry.addData("wait for start", ""); + telemetry.update(); + + waitForStart(); + telemetry.addData("started", ""); + telemetry.update(); + robotOrientation = imu.getRobotYawPitchRollAngles(); + + if (opModeIsRunning()) { + /* + * 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.forward(0.5); + robot.rotate((-90)); + robot.forward(1.5); + robot.harvest(-1); + robot.backward(1); + robot.harvest(0); + break; + case ("b2d"): + robot.forward(0.5); + robot.rotate((-90)); + robot.forward(2.5); + robot.harvest(-1); + robot.backward(1); + robot.harvest(0); + break; + case ("r4d"): + robot.forward(0.5); + robot.rotate(90); + robot.forward(1.5); + robot.harvest(-1); + robot.backward(1); + robot.harvest(0); + break; + case ("r2d"): + robot.forward(0.5); + robot.rotate(90); + robot.forward(2.5); + robot.harvest(-1); + robot.backward(1); + robot.harvest(0); + break; + case ("b4n"): + 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); + break; + case ("b2n"): + robot.forward(1.5); + robot.rotate(90); + robot.harvest(); + robot.forward(1); + 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); + break; + case ("r4n"): + 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); + break; + case ("r2n"): + robot.forward(1.5); + robot.rotate(-90); + robot.harvest(); + robot.forward(1); + 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); + break; + } + } } - } }