From 0acbd85c9aab2cd43c7ce8c546975a9f7ae5672e Mon Sep 17 00:00:00 2001 From: G_Zod01 Date: Sat, 6 Apr 2024 10:45:57 +0200 Subject: [PATCH] h --- .FTC2024WeRobotControl.java.un~ | Bin 0 -> 14940 bytes FTC2024WeRobotControl.java | 356 ++++++++++++++++---------------- FTC2024WeRobotControl.java~ | 211 +++++++++++++++++++ 3 files changed, 385 insertions(+), 182 deletions(-) create mode 100644 .FTC2024WeRobotControl.java.un~ create mode 100644 FTC2024WeRobotControl.java~ diff --git a/.FTC2024WeRobotControl.java.un~ b/.FTC2024WeRobotControl.java.un~ new file mode 100644 index 0000000000000000000000000000000000000000..3f6661ee6c87133e714b2ebf85be539e0393034d GIT binary patch literal 14940 zcmeGjU2hyo(fK%ZLx2Mv=pr2ljl+dm$DUm~IJv~x5Mw*R;+!>(9fXir_0IOLM>jJ) zHy^w4Mfbo9{{ac(C*nox1w}qikB^6rgz;}eM zQCrul^sQOG>~;IhWu0KErH;W|Ph#$Kk0-)Wn#c3aEWT;)@R$oFo`90Xj%#hXG>BM# z-|@0N@zo&dxsFY2mqrn}yT7s0TKW2Sneey`?#Cf_$z#yOIitA%m$pF`$o?TCyRcDw z*p342@tugVDabbWqc}`#5c6Fia2^OW;rhA}gwBY@fEL@xvrDB0SeFKlB9Dqxj9){> zF)$te1DlfcIV@12btPJx+^||~X=RNxn>hi+h!hzG zECg*~n3Bv`sPVde9IqU+O?GH}Xzgyp5`_3t4sCUpQ>J0y5gHSZJ|Y1h16Mu}tM<^$ z`ZAlH|flS6= zA~s+tj3JV;{dZ6`nZpV#RPPAk_KN~Q?G+++3{hLg1AAt@{=ANZ1Dq>9aA>>}EF5 ztY-}j3;1~@m1M-7KI!pCTkgn|OkOt(N#Alk2(b`1cxXi|e#l&IJMocut<@6La=`F- zz=bIk_h~p}@h*=X%;V+_Nj8TrlC6fml+g~kOyQNXHY&kNB$rmD1nmsCr~#~ROqB+# zy~M}g*51MW?tZ7c+j+3PzumbnG?No3E@5T@YncT4s%A!)c0OZ(q0bQb=T!n9<$A*4 z0gt{3JBFE!$MEq+)6SaKNFqGm=p^&C@{ZblLD*p_sZ^%)-aH#F)n(AnJ z^<}XJA_%J_UQd;rq7XE}EV6p;F`Hru;y}euebQr4l&!)MpDbIeK-oZ&4377WoXztx z$j#IsB8jNgriM`Kn{JCvW{Vi&tqbY^j|6QLteR>G^eJj$y~178_nffAXQAo=^w5HH z35#WsvtbzHL(nNPRXXFMF|i%3LziO&DUV#fEX!D^6rhCbM{w&FM4VJ686Tjq$m<}i zDR%LUV3z$v0}H+BVCM``dVyDEIW`BDc`SdX6OzyJ!fmR@&7)sH9P!~y0ktRh8I-ES z#KXcCIzDg|y$KG30hPd2K&uU(s@yNSq=Lq?!{r(5F12<=m7%F`WKJQ%oPOA--bpW^ ze6W*x(AoM}Z>6<@O4H@eyqBQdpHT5q**9~q`PwYpqy-n11v8PJ)Pxt%gwQn^LB|n% zM`f7j6`~djD^+5oYSk4a4pgvMwI3CvfXH{Ij(}Mq*XYGcK2n9BS1L@o!>PR5+dqk_ z61AI_i?8BMwKXX=$yCOW-=Q#)2*iXYNuLcv#-jg1D!5))zZ5A|vygOp9)QzPxiGx(?l{r!4xfFY&2S%Tle<1wjKyuz$sJZ zTOn;ovHKxfM={@{eTVvyDV_-E9m5bma+po2c~hWJD5qBB*U0fD9#Cv7N{TjDwD4X- z5WJbOciABBdeDM#0@s0xpiP!crO1XvkDMWdW+hDtJ9LJJCG-=qznt;*y0Y=r))OoG zIet(%uDr&9w>JT z^}G}siT@|_(oaF9f*)wEHtWG@Jvc3%`|H6ec1KD>vX9x|G%YfVJ)YvsG#`E!N0bv2 z%^a{e>ze?CPO>@yiqi+a*;X}mZ)9La#wC-;j#Ca?WPTFx$@9>B#9b6)=ILWjpeWkR z1Wq|4Mdi{gqPd&3B&N(DaTEnd+mnZzB}&j5dD^k& zOW9m48))WDRa|pD#nhW|vMo_hF`wnj1oafNo?`aXS$#UQP*X0s-|8tQ{4W6>>>>>> 1a4f84df926fc16dbce849ff988c4823ffffb3e1 - 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; + + /* + * return the needed time for a distance + * + * @param dist = distance in metre + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * to 1 + */ + public double time_for_dist(double dist, double motor_speed) { + double speed = getSpeedFromMotorSpeed(motor_speed); + return (dist / speed); + } + + /* + * go forward + * + * @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 + * etc.) + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * 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); + } - /* - * return the needed time for a distance - * - * @param dist = distance in metre - * - * @param motor_speed = (optional) double between 0 and 1; motor power; default - * to 1 - */ - public double time_for_dist(double dist, double motor_speed) { - double speed = getSpeedFromMotorSpeed(motor_speed); - return (dist / speed); + /* + * go forward + * when only one argument passed: + * + * @param n_tiles number of tiles + */ + public void forward(double n_tiles) { + this.forward(n_tiles, this.defaultspeed); + } + + /* + * go backward + * + * @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 + * etc.) + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * to 1 + */ + public void backward(double n_tiles, double motor_speed) { + forward(n_tiles, -motor_speed); + } + + public void backward(double n_tiles) { + this.backward(n_tiles, this.defaultspeed); + } + + /* + * harvest + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * to 1 + */ + public void harvest(double motor_speed) { + Parent.harvestmotor.setPower(motor_speed); + } + + public void harvest() { + this.harvest(1.0); + } + + /* + * rotate + * + * @param angle = the angle to rotate (in degrees) + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * 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); } - - /* - * go forward - * - * @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 - * etc.) - * - * @param motor_speed = (optional) double between 0 and 1; motor power; default - * 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); + 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); - } + Parent.lm.setPower(0); + Parent.rm.setPower(0); + } - /* - * go forward - * when only one argument passed: - * - * @param n_tiles number of tiles - */ - public void forward(double n_tiles) { - this.forward(n_tiles, this.defaultspeed); - } - - /* - * go backward - * - * @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 - * etc.) - * - * @param motor_speed = (optional) double between 0 and 1; motor power; default - * to 1 - */ - public void backward(double n_tiles, double motor_speed) { - forward(n_tiles, -motor_speed); - } - - public void backward(double n_tiles) { - this.backward(n_tiles, this.defaultspeed); - } - - /* - * harvest - * - * @param motor_speed = (optional) double between 0 and 1; motor power; default - * to 1 - */ - public void harvest(double motor_speed) { - Parent.harvestmotor.setPower(motor_speed); - } - - public void harvest() { - this.harvest(1.0); - } - - /* - * rotate - * - * @param angle = the angle to rotate (in degrees) - * - * @param motor_speed = (optional) double between 0 and 1; motor power; default - * 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); - } - - public void rotate(double angle) { - this.rotate(angle, this.defaultspeed); - } + public void rotate(double angle) { + this.rotate(angle, this.defaultspeed); + } } diff --git a/FTC2024WeRobotControl.java~ b/FTC2024WeRobotControl.java~ new file mode 100644 index 0000000..ac32b97 --- /dev/null +++ b/FTC2024WeRobotControl.java~ @@ -0,0 +1,211 @@ +package org.firstinspires.ftc.teamcode;//a tester car pas sur que ça fonctionne + +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; + +public class FTC2024WeRobotControl { + /* + * The Parent class {@see FTC2024WeRobotControl constructor} + */ + private Ftc2024_autonome_api Parent; + /* + * the wheel width in metres + */ + private final double wheel_width = 9.0e-2; // metres + /* + * the wheel perimeter in meter + */ + private final double wheel_perimeter = Math.PI * wheel_width; + /* + * the rpm at max power of the motors + */ + private final double tour_par_minutes = 300.0; + /* + * 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; + + private YawPitchRollAngles robotOrientation; + + /* + * construct the FTC2024WeRobotControl class, the WeRobot Robot Controller Class + * for the FTC2024 + * + * @param Parent = the parent class, use the "this" keyword if you are + * constructing the class directly in + */ + + public FTC2024WeRobotControl(Ftc2024_autonome_api Parent) { + this.Parent = Parent; + this.timer = new ElapsedTime(); + } + + 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); + + } + /* + * return a metre/sec speed + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * to 1 + */ + + 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; + } + + + /* + * return the needed time for a distance + * + * @param dist = distance in metre + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * to 1 + */ + public double time_for_dist(double dist, double motor_speed) { + double speed = getSpeedFromMotorSpeed(motor_speed); + return (dist / speed); + } + + /* + * go forward + * + * @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 + * etc.) + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * 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); + } + + /* + * go forward + * when only one argument passed: + * + * @param n_tiles number of tiles + */ + public void forward(double n_tiles) { + this.forward(n_tiles, this.defaultspeed); + } + + /* + * go backward + * + * @param n_tiles = the number of tiles (a double because it can be 0.5 or 1.5 + * etc.) + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * to 1 + */ + public void backward(double n_tiles, double motor_speed) { + forward(n_tiles, -motor_speed); + } + + public void backward(double n_tiles) { + this.backward(n_tiles, this.defaultspeed); + } + + /* + * harvest + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * to 1 + */ + public void harvest(double motor_speed) { + Parent.harvestmotor.setPower(motor_speed); + } + + public void harvest() { + this.harvest(1.0); + } + + /* + * rotate + * + * @param angle = the angle to rotate (in degrees) + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * 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); + } + + public void rotate(double angle) { + this.rotate(angle, this.defaultspeed); + } + +}