diff --git a/FTC2024WeRobotControl.java b/FTC2024WeRobotControl.java index bb75dfe..48b3c9c 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,28 @@ 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); + } + + public void test_forward_10_and_rotate_20deg(){ + } }