werobot/ftc2024-robotcode
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

fix errors

Browse source
This commit is contained in:
Zelina974 2024-04-01 17:24:07 +02:00
parent bb66abbe84
commit fe0282842b
1 changed files with 112 additions and 157 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

205
ftc_new.java
View file

@ -25,7 +25,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
@TeleOp(name = "WeRobot: FTC2024 NEW!!! Carlike", group = "WeRobot") @TeleOp(name = "WeRobot: FTC2024 NEW!!! Carlike", group = "WeRobot")
public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { public class WEROBOT_FTC2024_New_carlike extends LinearOpMode {
public enum RobotMode{ public enum RobotMode {
ESSAIFRANCK, ELINA, NORMAL, TANK; ESSAIFRANCK, ELINA, NORMAL, TANK;
} }
@ -38,18 +38,42 @@ public enum RobotMode{
private DcMotorEx rotation; private DcMotorEx rotation;
private ElapsedTime runtime = new ElapsedTime(); private ElapsedTime runtime = new ElapsedTime();
private RobotMode mode = RobotMode.ESSAIFRANCK; private RobotMode mode = RobotMode.ESSAIFRANCK;
/*La fonction pour faire des exponentielles spécifiques
/*
* La fonction pour faire des exponentielles spécifiques
*
* @param double t => le nombre dont on veut faire l'exponentielle * @param double t => le nombre dont on veut faire l'exponentielle
* @return double une_exponentielle_très_spéciale_de_t*/ *
* @return double une_exponentielle_très_spéciale_de_t
*/
private double helloexp(double t) { private double helloexp(double t) {
return (Math.exp(5 * t) - 1) / (Math.exp(5) - 1); return (Math.exp(5 * t) - 1) / (Math.exp(5) - 1);
} }
//La fonction du thread principal public void nextMode() {
RobotMode toNextMode;
switch (this.mode) {
case ESSAIFRANCK:
toNextMode = RobotMode.ELINA;
break;
case ELINA:
toNextMode = RobotMode.NORMAL;
break;
case NORMAL:
toNextMode = RobotMode.TANK;
break;
default:
toNextMode = RobotMode.ESSAIFRANCK;
break;
}
this.mode = toNextMode;
}
// La fonction du thread principal
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
double boxRot; double boxRot = 0;
int signeBR; int signeBR;
float x; float x;
@ -122,26 +146,30 @@ public enum RobotMode{
x = gamepad1.left_stick_x; x = gamepad1.left_stick_x;
y = gamepad1.left_stick_y; y = gamepad1.left_stick_y;
/* définition de {@link t} sur la valeur du trigger droit du gamepad 1*/ /* définition de {@link t} sur la valeur du trigger droit du gamepad 1 */
t = gamepad1.right_trigger; t = gamepad1.right_trigger;
/* définition de {@link t2} par utilisation de la fonction {@link helloexp} sur /*
* {@link t}*/ * définition de {@link t2} par utilisation de la fonction {@link helloexp} sur
* {@link t}
*/
t2 = helloexp(t); t2 = helloexp(t);
/* définition de {@link t3} par utilisation de la fonction {@link helloexp} sur /*
* définition de {@link t3} par utilisation de la fonction {@link helloexp} sur
* la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link * la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link
* x} au carré plus {@link y} au carré*/ * x} au carré plus {@link y} au carré
*/
t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2))); t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)));
telemetry.addData("Status", "Running"); telemetry.addData("Status", "Running");
if (0.1<gamepad1.right_stick_x){ if (0.1 < gamepad1.right_stick_x) {
boxRot = 0.2; boxRot = 0.2;
} else if (-0.1>gamepad1.right_stick_x){ } else if (-0.1 > gamepad1.right_stick_x) {
boxRot = -0.2; boxRot = -0.2;
} }
if (gamepad1.right_trigger>0){ if (gamepad1.right_trigger > 0) {
box.setPower(boxRot); box.setPower(boxRot);
} }
// if (boxRot <= 0){ // if (boxRot <= 0){
@ -151,7 +179,6 @@ public enum RobotMode{
// signeBR=1; // signeBR=1;
// } // }
// if (Math.abs(boxRot) < 0.1){ // if (Math.abs(boxRot) < 0.1){
// boxRot = 0.4*signeBR; // boxRot = 0.4*signeBR;
// } // }
@ -162,7 +189,7 @@ public enum RobotMode{
// } // }
// } // }
if(gamepad1.a && !already_a){ if (gamepad1.a && !already_a) {
nextMode(); nextMode();
already_a = true; already_a = true;
} }
@ -174,8 +201,8 @@ public enum RobotMode{
switch (mode) { switch (mode) {
case NORMAL: case NORMAL:
double ysign = Double.signum(y); double ysign = Math.signum(y);
double xsign = Double.signum(x); double xsign = Math.signum(x);
lpower = -ysign * t + (xsign - 2 * x) * t; lpower = -ysign * t + (xsign - 2 * x) * t;
rpower = ysign * t + (xsign - 2 * x) * t; rpower = ysign * t + (xsign - 2 * x) * t;
break; break;
@ -185,7 +212,7 @@ public enum RobotMode{
rpower = gamepad1.right_stick_y; rpower = gamepad1.right_stick_y;
break; break;
case ESSAIFRANCK : case ESSAIFRANCK:
double a = (-y + x) / Math.pow(2, 1 / 2); double a = (-y + x) / Math.pow(2, 1 / 2);
double b = (-y - x) / Math.pow(2, 1 / 2); double b = (-y - x) / Math.pow(2, 1 / 2);
double vmean = (Math.abs(a) + Math.abs(b)) / 2; double vmean = (Math.abs(a) + Math.abs(b)) / 2;
@ -193,16 +220,16 @@ public enum RobotMode{
rpower = (b / vmean) * t2; rpower = (b / vmean) * t2;
break; break;
case ELINA : case ELINA:
double a = (-y + x) / Math.pow(2, 1 / 2); a = (-y + x) / Math.pow(2, 1 / 2);
double b = (-y - x) / Math.pow(2, 1 / 2); b = (-y - x) / Math.pow(2, 1 / 2);
double vmean = (Math.abs(a) + Math.abs(b)) / 4; vmean = (Math.abs(a) + Math.abs(b)) / 4;
lpower = (a / vmean) * t3; lpower = (a / vmean) * t3;
rpower = (b / vmean) * t3; rpower = (b / vmean) * t3;
break; break;
} }
if (gamepad1.left_trigger>0.1) { if (gamepad1.left_trigger > 0.1) {
lpower /= 3; lpower /= 3;
rpower /= 3; rpower /= 3;
} }
@ -213,7 +240,7 @@ public enum RobotMode{
// activation moissonneuse // activation moissonneuse
if (gamepad1.b && !already_b) { if (gamepad1.b && !already_b) {
double moissoneuseSpeed = 1.0; double moissoneuseSpeed = 1.0;
if (gamepad1.right_bumper){ if (gamepad1.right_bumper) {
moissoneuseSpeed = -1.0; moissoneuseSpeed = -1.0;
} }
already_b = !already_b; already_b = !already_b;
@ -227,8 +254,9 @@ public enum RobotMode{
already_b = false; already_b = false;
} }
//activation elevateur // activation elevateur
if (sinking && Math.abs(lmelevator.getCurrentPosition()-90)<=5 && Math.abs(rmelevator.getCurrentPosition()-90)<=5){ if (sinking && Math.abs(lmelevator.getCurrentPosition() - 90) <= 5
&& Math.abs(rmelevator.getCurrentPosition() - 90) <= 5) {
lmelevator.setVelocity(100); lmelevator.setVelocity(100);
rmelevator.setVelocity(100); rmelevator.setVelocity(100);
lmelevator.setTargetPosition(0); lmelevator.setTargetPosition(0);
@ -236,16 +264,16 @@ public enum RobotMode{
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
} }
if ((gamepad1.dpad_up && !already_up)^(gamepad1.dpad_down && !already_down)){ if ((gamepad1.dpad_up && !already_up) ^ (gamepad1.dpad_down && !already_down)) {
lmelevator.setVelocity(600); lmelevator.setVelocity(600);
rmelevator.setVelocity(600); rmelevator.setVelocity(600);
Long targetPosLong = (Long) Math.round(288*3.4); Long targetPosLong = (Long) Math.round(288 * 3.4);
int targetPos = targetPosLong.intValue(); int targetPos = targetPosLong.intValue();
if (gamepad1.dpad_down){ if (gamepad1.dpad_down) {
targetPos = 90; targetPos = 90;
already_down = true; already_down = true;
sinking = true; sinking = true;
}else{ } else {
already_up = true; already_up = true;
sinking = false; sinking = false;
} }
@ -254,89 +282,34 @@ public enum RobotMode{
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}else if (!gamepad1.dpad_up && already_up){ } else if (!gamepad1.dpad_up && already_up) {
already_up = false; already_up = false;
}else if (!gamepad1.dpad_down && already_down){ } else if (!gamepad1.dpad_down && already_down) {
already_down = false; already_down = false;
} }
if (gamepad1.ps && !already_ps){ if (gamepad1.ps && !already_ps) {
manualMode = !manualMode; manualMode = !manualMode;
already_ps = true; already_ps = true;
} else if (!gamepad1.ps && already_ps){ } else if (!gamepad1.ps && already_ps) {
already_ps = false; already_ps = false;
} }
// commentaires supprimés dans le robot
// if (gamepad1.x && !already_x){
// int targetPos = 0;
// if(gamepad1.right_bumper){
// targetPos = -97;
// }
// // while (Math.abs(box.getCurrentPosition()-targetPos) > 30)
// // {
// // box.setVelocity(100);
// // box.setTargetPosition(targetPos);
// // box.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// // }
// box.setVelocity(50);
// box.setTargetPosition(targetPos);
// box.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// already_x = !already_x;
// } else if(!gamepad1.x && already_x){
// already_x = false;
// }
// if (Math.abs(box.getCurrentPosition() - box.getTargetPosition()) < 20) {
// box.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODERS);
// box.setPower(-0.2);
// telemetry.addData("ModeChanged","without encoders");
// }
// if (gamepad1.y && already_y){
// rotation.setVelocity(200);
// // int targetPos = 0;
// // if (gamepad1.right_bumper){
// // targetPos = 0;
// // }else if (gamepad1.left_bumper){
// // targetPos = 0;
// // }
// // rotation.setTargetPosition(targetPos);
// // rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// int pos = rotation.getCurrentPosition();
// if (gamepad1.right_bumper){
// rotation.setTargetPosition(pos + 100);
// }else if (gamepad1.left_bumper){
// rotation.setTargetPosition(pos - 100);
// }
// rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// }
// activation rotation // activation rotation
if (manualMode){ if (manualMode) {
gamepad1.setLedColor(255,0,0,10); gamepad1.setLedColor(255, 0, 0, 10);
lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
if (gamepad1.dpad_up){ if (gamepad1.dpad_up) {
rmelevator.setPower(0.3); rmelevator.setPower(0.3);
lmelevator.setPower(0.3); lmelevator.setPower(0.3);
} else if (gamepad1.dpad_down){ } else if (gamepad1.dpad_down) {
lmelevator.setPower(-0.3); lmelevator.setPower(-0.3);
rmelevator.setPower(-0.3); rmelevator.setPower(-0.3);
} else if (gamepad1.y){ } else if (gamepad1.y) {
double power = -0.3; double power = -0.3;
if (gamepad1.right_bumper){ if (gamepad1.right_bumper) {
power = -power; power = -power;
} }
rotation.setPower(power); rotation.setPower(power);
@ -348,31 +321,30 @@ public enum RobotMode{
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
} }
} } else {
else{ gamepad1.setLedColor(0, 0, 255, 10);
gamepad1.setLedColor(0,0,255,10);
} }
if (!gamepad1.y && already_y && !manualMode) { if (!gamepad1.y && already_y && !manualMode) {
already_y = false; already_y = false;
} }
if (gamepad1.y && !already_y && !manualMode){ if (gamepad1.y && !already_y && !manualMode) {
already_y = true; already_y = true;
int pos = rotation.getCurrentPosition(); int pos = rotation.getCurrentPosition();
rotation.setVelocity(200); rotation.setVelocity(200);
if (gamepad1.right_bumper){ if (gamepad1.right_bumper) {
// rotation.setTargetPosition(pos - 25); // rotation.setTargetPosition(pos - 25);
rotation.setTargetPosition(-100); // vertical si pos origine = 0 rotation.setTargetPosition(-100); // vertical si pos origine = 0
} else if (gamepad1.left_bumper){ } else if (gamepad1.left_bumper) {
// rotation.setTargetPosition(pos + 25); // rotation.setTargetPosition(pos + 25);
rotation.setTargetPosition(1000); //position basse rotation.setTargetPosition(1000); // position basse
} else { } else {
rotation.setTargetPosition(0); rotation.setTargetPosition(0);
} }
rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
} }
if (gamepad1.right_bumper && gamepad1.left_bumper){ if (gamepad1.right_bumper && gamepad1.left_bumper) {
// launch the plane // launch the plane
} }
@ -383,32 +355,15 @@ public enum RobotMode{
telemetry.addData("rpow", rpower); telemetry.addData("rpow", rpower);
telemetry.addData("ltrigg", t); telemetry.addData("ltrigg", t);
telemetry.addData("t2", t2); telemetry.addData("t2", t2);
telemetry.addData("rotation power",boxRot); telemetry.addData("rotation power", boxRot);
telemetry.addData("mode manuel", manualMode); telemetry.addData("mode manuel", manualMode);
telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition()); telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition());
telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition()); telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition());
telemetry.addData("Position rotation",rotation.getCurrentPosition()); telemetry.addData("Position rotation", rotation.getCurrentPosition());
telemetry.addData("Position box",box.getCurrentPosition()); telemetry.addData("Position box", box.getCurrentPosition());
telemetry.addData("box velocity",rotation.getVelocity()); telemetry.addData("box velocity", rotation.getVelocity());
telemetry.update(); telemetry.update();
} }
} }
public void nextMode(){
RobotMode toNextMode;
switch(this.mode){
case (ESSAIFRANCK):
toNextMode = this.RobotMode.ELINA;
break;
case (ELINA):
toNextMode = this.RobotMode.NORMAL;
break;
case (NORMAL):
toNextMode = this.RobotMode.TANK;
break;
default:
toNextMode = this.RobotMode.ESSAIFRANCK;
break;
}
this.mode = toNextMode;
}
} }