Update
This commit is contained in:
Zelina974
parent
60d01e81f5
commit
07423e3692
1 changed files with 321 additions and 318 deletions
639
ftc_new.java
639
ftc_new.java
|
|
@ -25,8 +25,8 @@ 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, TANCK;
|
ESSAIFRANCK, ELINA, NORMAL, TANK;
|
||||||
}
|
}
|
||||||
|
|
||||||
private DcMotor rm;
|
private DcMotor rm;
|
||||||
|
|
@ -37,360 +37,363 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode {
|
||||||
private DcMotorEx box;
|
private DcMotorEx box;
|
||||||
private DcMotorEx rotation;
|
private DcMotorEx rotation;
|
||||||
private ElapsedTime runtime = new ElapsedTime();
|
private ElapsedTime runtime = new ElapsedTime();
|
||||||
|
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
|
//La fonction du thread principal
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() throws InterruptedException {
|
public void runOpMode() throws InterruptedException {
|
||||||
|
|
||||||
|
double boxRot;
|
||||||
|
int signeBR;
|
||||||
|
|
||||||
|
float x;
|
||||||
|
double y;
|
||||||
|
|
||||||
double boxRot;
|
double t;
|
||||||
int signeBR;
|
double t2;
|
||||||
|
double t3;
|
||||||
|
|
||||||
float x;
|
boolean already_b = false;
|
||||||
double y;
|
boolean already_a = false;
|
||||||
|
boolean already_x = false;
|
||||||
|
boolean already_y = false;
|
||||||
|
boolean already_up = false;
|
||||||
|
boolean already_down = false;
|
||||||
|
boolean already_ps = false;
|
||||||
|
|
||||||
|
boolean sinking = false;
|
||||||
|
boolean manualMode = false;
|
||||||
|
boolean firstLaunch = true;
|
||||||
|
|
||||||
double t;
|
telemetry.addData("Status", "Initialized");
|
||||||
double t2;
|
|
||||||
double t3;
|
|
||||||
|
|
||||||
String mode = "elina";
|
lm = hardwareMap.get(DcMotor.class, "blm");
|
||||||
|
|
||||||
boolean already_b = false;
|
rm = hardwareMap.get(DcMotor.class, "brm");
|
||||||
boolean already_a = false;
|
rm.setDirection(DcMotor.Direction.REVERSE);
|
||||||
boolean already_x = false;
|
|
||||||
boolean already_y = false;
|
|
||||||
boolean already_up = false;
|
|
||||||
boolean already_down = false;
|
|
||||||
boolean already_ps = false;
|
|
||||||
|
|
||||||
boolean sinking = false;
|
moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse");
|
||||||
boolean manualMode = false;
|
moissoneuse.setDirection(DcMotor.Direction.REVERSE);
|
||||||
boolean firstLaunch = true;
|
|
||||||
|
|
||||||
telemetry.addData("Status", "Initialized");
|
lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
|
||||||
|
lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
lm = hardwareMap.get(DcMotor.class, "blm");
|
rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv");
|
||||||
|
rmelevator.setDirection(DcMotor.Direction.REVERSE);
|
||||||
|
rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
rm = hardwareMap.get(DcMotor.class, "brm");
|
rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
|
||||||
rm.setDirection(DcMotor.Direction.REVERSE);
|
rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
|
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
|
||||||
moissoneuse = hardwareMap.get(DcMotor.class, "moissonneuse");
|
box = hardwareMap.get(DcMotorEx.class, "boxRot");
|
||||||
|
box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
// box.setPositionPIDFCoefficients(5.0);
|
||||||
|
|
||||||
lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
|
// rotation positions: 20° pos initiale par rapport au sol
|
||||||
lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
// while (runtime.seconds()<0.5){
|
||||||
lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
// rotation.setPower(0.5);
|
||||||
|
// }
|
||||||
|
|
||||||
rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv");
|
telemetry.addData("Mode", "waiting for start");
|
||||||
rmelevator.setDirection(DcMotor.Direction.REVERSE);
|
|
||||||
rmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
|
||||||
rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
||||||
|
|
||||||
rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
|
// rotation.setVelocity(1700);
|
||||||
rotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
// rotation.setTargetPosition(-1000);
|
||||||
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
// rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
|
||||||
box = hardwareMap.get(DcMotorEx.class, "boxRot");
|
telemetry.addData("rotation target Pos", rotation.getTargetPosition());
|
||||||
box.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
telemetry.addData("rotation Pos", rotation.getCurrentPosition());
|
||||||
// box.setPositionPIDFCoefficients(5.0);
|
telemetry.update();
|
||||||
|
|
||||||
// rotation positions: 20° pos initiale par rapport au sol
|
waitForStart();
|
||||||
// while (runtime.seconds()<0.5){
|
rotation.setVelocity(200);
|
||||||
// rotation.setPower(0.5);
|
rotation.setTargetPosition(0);
|
||||||
// }
|
rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
|
x = gamepad1.left_stick_x;
|
||||||
|
y = gamepad1.left_stick_y;
|
||||||
|
|
||||||
|
/* définition de {@link t} sur la valeur du trigger droit du gamepad 1*/
|
||||||
|
t = gamepad1.right_trigger;
|
||||||
|
|
||||||
|
/* définition de {@link t2} par utilisation de la fonction {@link helloexp} sur
|
||||||
|
* {@link t}*/
|
||||||
|
t2 = helloexp(t);
|
||||||
|
|
||||||
telemetry.addData("Mode", "waiting for start");
|
/* 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
|
||||||
|
* x} au carré plus {@link y} au carré*/
|
||||||
|
t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)));
|
||||||
|
|
||||||
|
telemetry.addData("Status", "Running");
|
||||||
|
|
||||||
// rotation.setVelocity(1700);
|
if (0.1<gamepad1.right_stick_x){
|
||||||
// rotation.setTargetPosition(-1000);
|
boxRot = 0.2;
|
||||||
// rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
} else if (-0.1>gamepad1.right_stick_x){
|
||||||
|
boxRot = -0.2;
|
||||||
|
}
|
||||||
|
if (gamepad1.right_trigger>0){
|
||||||
|
box.setPower(boxRot);
|
||||||
|
}
|
||||||
|
// if (boxRot <= 0){
|
||||||
|
// signeBR = -1;
|
||||||
|
// }
|
||||||
|
// else{
|
||||||
|
// signeBR=1;
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
// if (Math.abs(boxRot) < 0.1){
|
||||||
|
// boxRot = 0.4*signeBR;
|
||||||
|
// }
|
||||||
|
// else {
|
||||||
|
// boxRot = 0.4*signeBR + boxRot/1.67;
|
||||||
|
// if (boxRot*signeBR > 0.9){
|
||||||
|
// boxRot = signeBR;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
||||||
|
if(gamepad1.a && !already_a){
|
||||||
|
nextMode();
|
||||||
|
already_a = true;
|
||||||
|
}
|
||||||
|
if (!gamepad1.a && already_a) {
|
||||||
|
already_a = false;
|
||||||
|
}
|
||||||
|
double lpower = 0.0;
|
||||||
|
double rpower = 0.0;
|
||||||
|
|
||||||
telemetry.addData("rotation target Pos", rotation.getTargetPosition());
|
switch (mode) {
|
||||||
telemetry.addData("rotation Pos", rotation.getCurrentPosition());
|
case NORMAL:
|
||||||
telemetry.update();
|
double ysign = Double.signum(y);
|
||||||
|
double xsign = Double.signum(x);
|
||||||
|
lpower = -ysign * t + (xsign - 2 * x) * t;
|
||||||
|
rpower = ysign * t + (xsign - 2 * x) * t;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case TANK:
|
||||||
|
lpower = -y;
|
||||||
|
rpower = gamepad1.right_stick_y;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ESSAIFRANCK :
|
||||||
|
double a = (-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;
|
||||||
|
lpower = (a / vmean) * t2;
|
||||||
|
rpower = (b / vmean) * t2;
|
||||||
|
break;
|
||||||
|
|
||||||
waitForStart();
|
case ELINA :
|
||||||
rotation.setVelocity(200);
|
double a = (-y + x) / Math.pow(2, 1 / 2);
|
||||||
rotation.setTargetPosition(0);
|
double b = (-y - x) / Math.pow(2, 1 / 2);
|
||||||
rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
double vmean = (Math.abs(a) + Math.abs(b)) / 4;
|
||||||
while (opModeIsActive()) {
|
lpower = (a / vmean) * t3;
|
||||||
|
rpower = (b / vmean) * t3;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
x = gamepad1.left_stick_x;
|
if (gamepad1.left_trigger>0.1) {
|
||||||
y = gamepad1.left_stick_y;
|
lpower /= 3;
|
||||||
|
rpower /= 3;
|
||||||
|
}
|
||||||
|
|
||||||
/* définition de {@link t} sur la valeur du trigger droit du gamepad 1*/
|
lm.setPower(lpower);
|
||||||
t = gamepad1.right_trigger;
|
rm.setPower(rpower);
|
||||||
|
|
||||||
/* définition de {@link t2} par utilisation de la fonction {@link helloexp} sur
|
// activation moissonneuse
|
||||||
* {@link t}*/
|
if (gamepad1.b && !already_b) {
|
||||||
t2 = helloexp(t);
|
double moissoneuseSpeed = 1.0;
|
||||||
|
if (gamepad1.right_bumper){
|
||||||
|
moissoneuseSpeed = -1.0;
|
||||||
|
}
|
||||||
|
already_b = !already_b;
|
||||||
|
if (moissoneuse.getPower() == moissoneuseSpeed) {
|
||||||
|
moissoneuse.setPower(0);
|
||||||
|
} else {
|
||||||
|
moissoneuse.setPower(moissoneuseSpeed);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (!gamepad1.b && already_b) {
|
||||||
|
already_b = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
//activation elevateur
|
||||||
|
if (sinking && Math.abs(lmelevator.getCurrentPosition()-90)<=5 && Math.abs(rmelevator.getCurrentPosition()-90)<=5){
|
||||||
|
lmelevator.setVelocity(100);
|
||||||
|
rmelevator.setVelocity(100);
|
||||||
|
lmelevator.setTargetPosition(0);
|
||||||
|
rmelevator.setTargetPosition(0);
|
||||||
|
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
if ((gamepad1.dpad_up && !already_up)^(gamepad1.dpad_down && !already_down)){
|
||||||
|
lmelevator.setVelocity(600);
|
||||||
|
rmelevator.setVelocity(600);
|
||||||
|
Long targetPosLong = (Long) Math.round(288*3.4);
|
||||||
|
int targetPos = targetPosLong.intValue();
|
||||||
|
if (gamepad1.dpad_down){
|
||||||
|
targetPos = 90;
|
||||||
|
already_down = true;
|
||||||
|
sinking = true;
|
||||||
|
}else{
|
||||||
|
already_up = true;
|
||||||
|
sinking = false;
|
||||||
|
}
|
||||||
|
lmelevator.setTargetPosition(targetPos);
|
||||||
|
rmelevator.setTargetPosition(targetPos);
|
||||||
|
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
|
||||||
|
}else if (!gamepad1.dpad_up && already_up){
|
||||||
|
already_up = false;
|
||||||
|
}else if (!gamepad1.dpad_down && already_down){
|
||||||
|
already_down = false;
|
||||||
|
}
|
||||||
|
|
||||||
/* définition de {@link t3} par utilisation de la fonction {@link helloexp} sur
|
if (gamepad1.ps && !already_ps){
|
||||||
* la norme du vecteur du joystick gauche du gamepad 1 (racine carrée de {@link
|
manualMode = !manualMode;
|
||||||
* x} au carré plus {@link y} au carré*/
|
already_ps = true;
|
||||||
t3 = helloexp(Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)));
|
} else if (!gamepad1.ps && already_ps){
|
||||||
|
already_ps = false;
|
||||||
|
}
|
||||||
|
|
||||||
telemetry.addData("Status", "Running");
|
// commentaires supprimés dans le robot
|
||||||
// si le bouton a du gamepad 1 est appuyé et {already_a} est faux
|
// if (gamepad1.x && !already_x){
|
||||||
if (gamepad1.a && !already_a) {
|
|
||||||
if (mode == "normal") {
|
// int targetPos = 0;
|
||||||
mode = "tank";
|
// if(gamepad1.right_bumper){
|
||||||
} else if (mode == "tank") {
|
// targetPos = -97;
|
||||||
mode = "essai franck";
|
// }
|
||||||
} else if (mode == "essai franck") {
|
|
||||||
mode = "elina";
|
// // while (Math.abs(box.getCurrentPosition()-targetPos) > 30)
|
||||||
} else {
|
// // {
|
||||||
mode = "elina";
|
// // box.setVelocity(100);
|
||||||
}
|
// // box.setTargetPosition(targetPos);
|
||||||
already_a = true;
|
// // 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);
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
boxRot = gamepad1.right_stick_x;
|
// activation rotation
|
||||||
// if (boxRot <= 0){
|
if (manualMode){
|
||||||
// signeBR = -1;
|
gamepad1.setLedColor(255,0,0,10);
|
||||||
// }
|
lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
// else{
|
rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
// signeBR=1;
|
rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||||
// }
|
if (gamepad1.dpad_up){
|
||||||
|
rmelevator.setPower(0.3);
|
||||||
|
lmelevator.setPower(0.3);
|
||||||
|
} else if (gamepad1.dpad_down){
|
||||||
|
lmelevator.setPower(-0.3);
|
||||||
|
rmelevator.setPower(-0.3);
|
||||||
|
} else if (gamepad1.y){
|
||||||
|
double power = -0.3;
|
||||||
|
if (gamepad1.right_bumper){
|
||||||
|
power = -power;
|
||||||
|
}
|
||||||
|
rotation.setPower(power);
|
||||||
|
} else {
|
||||||
|
lmelevator.setPower(0);
|
||||||
|
rmelevator.setPower(0);
|
||||||
|
lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
else{
|
||||||
|
gamepad1.setLedColor(0,0,255,10);
|
||||||
|
}
|
||||||
|
|
||||||
// if (Math.abs(boxRot) < 0.1){
|
if (!gamepad1.y && already_y && !manualMode) {
|
||||||
// boxRot = 0.4*signeBR;
|
already_y = false;
|
||||||
// }
|
}
|
||||||
// else {
|
if (gamepad1.y && !already_y && !manualMode){
|
||||||
// boxRot = 0.4*signeBR + boxRot/1.67;
|
already_y = true;
|
||||||
// if (boxRot*signeBR > 0.9){
|
int pos = rotation.getCurrentPosition();
|
||||||
// boxRot = signeBR;
|
rotation.setVelocity(200);
|
||||||
// }
|
if (gamepad1.right_bumper){
|
||||||
// }
|
// rotation.setTargetPosition(pos - 25);
|
||||||
|
rotation.setTargetPosition(-100); // vertical si pos origine = 0
|
||||||
|
} else if (gamepad1.left_bumper){
|
||||||
|
// rotation.setTargetPosition(pos + 25);
|
||||||
|
rotation.setTargetPosition(1000); //position basse
|
||||||
|
} else {
|
||||||
|
rotation.setTargetPosition(0);
|
||||||
|
}
|
||||||
|
rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (gamepad1.right_bumper && gamepad1.left_bumper){
|
||||||
|
// launch the plane
|
||||||
|
}
|
||||||
|
|
||||||
box.setPower(boxRot);
|
telemetry.addData("x", x);
|
||||||
|
telemetry.addData("y", y);
|
||||||
|
telemetry.addData("mode", mode);
|
||||||
|
telemetry.addData("lpow", lpower);
|
||||||
if (!gamepad1.a && already_a) {
|
telemetry.addData("rpow", rpower);
|
||||||
already_a = false;
|
telemetry.addData("ltrigg", t);
|
||||||
}
|
telemetry.addData("t2", t2);
|
||||||
double lpower = 0.0;
|
telemetry.addData("rotation power",boxRot);
|
||||||
double rpower = 0.0;
|
telemetry.addData("mode manuel", manualMode);
|
||||||
if (mode == "normal") {
|
telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition());
|
||||||
double ysign = Double.signum(y);
|
telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition());
|
||||||
double xsign = Double.signum(x);
|
telemetry.addData("Position rotation",rotation.getCurrentPosition());
|
||||||
lpower = -ysign * t + (xsign - 2 * x) * t;
|
telemetry.addData("Position box",box.getCurrentPosition());
|
||||||
rpower = ysign * t + (xsign - 2 * x) * t;
|
telemetry.addData("box velocity",rotation.getVelocity());
|
||||||
} else if (mode == "tank") {
|
telemetry.update();
|
||||||
lpower = -y;
|
}
|
||||||
rpower = gamepad1.right_stick_y;
|
|
||||||
}
|
|
||||||
if (mode == "essaifranck") {
|
|
||||||
double a = (-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;
|
|
||||||
lpower = (a / vmean) * t2;
|
|
||||||
rpower = (b / vmean) * t2;
|
|
||||||
} else if (mode == "elina") {
|
|
||||||
double a = (-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;
|
|
||||||
lpower = (a / vmean) * t3;
|
|
||||||
rpower = (b / vmean) * t3;
|
|
||||||
}
|
|
||||||
if (gamepad1.left_trigger>0.1) {
|
|
||||||
lpower /= 3;
|
|
||||||
rpower /= 3;
|
|
||||||
}
|
|
||||||
lm.setPower(lpower);
|
|
||||||
rm.setPower(rpower);
|
|
||||||
|
|
||||||
// activation moissonneuse
|
|
||||||
if (gamepad1.b && !already_b) {
|
|
||||||
double moissoneuseSpeed = 1.0;
|
|
||||||
if (gamepad1.right_bumper){
|
|
||||||
moissoneuseSpeed = -1.0;
|
|
||||||
}
|
|
||||||
already_b = !already_b;
|
|
||||||
if (moissoneuse.getPower() == moissoneuseSpeed) {
|
|
||||||
moissoneuse.setPower(0);
|
|
||||||
} else {
|
|
||||||
moissoneuse.setPower(moissoneuseSpeed);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (!gamepad1.b && already_b) {
|
|
||||||
already_b = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
//activation elevateur
|
|
||||||
if (sinking && Math.abs(lmelevator.getCurrentPosition()-90)<=5 && Math.abs(rmelevator.getCurrentPosition()-90)<=5){
|
|
||||||
lmelevator.setVelocity(100);
|
|
||||||
rmelevator.setVelocity(100);
|
|
||||||
lmelevator.setTargetPosition(0);
|
|
||||||
rmelevator.setTargetPosition(0);
|
|
||||||
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
||||||
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
||||||
}
|
|
||||||
if ((gamepad1.dpad_up && !already_up)^(gamepad1.dpad_down && !already_down)){
|
|
||||||
lmelevator.setVelocity(600);
|
|
||||||
rmelevator.setVelocity(600);
|
|
||||||
Long targetPosLong = (Long) Math.round(288*3.4);
|
|
||||||
int targetPos = targetPosLong.intValue();
|
|
||||||
if (gamepad1.dpad_down){
|
|
||||||
targetPos = 90;
|
|
||||||
already_down = true;
|
|
||||||
sinking = true;
|
|
||||||
}else{
|
|
||||||
already_up = true;
|
|
||||||
sinking = false;
|
|
||||||
}
|
|
||||||
lmelevator.setTargetPosition(targetPos);
|
|
||||||
rmelevator.setTargetPosition(targetPos);
|
|
||||||
lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
||||||
rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
||||||
|
|
||||||
}else if (!gamepad1.dpad_up && already_up){
|
|
||||||
already_up = false;
|
|
||||||
}else if (!gamepad1.dpad_down && already_down){
|
|
||||||
already_down = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (gamepad1.ps && !already_ps){
|
|
||||||
manualMode = !manualMode;
|
|
||||||
already_ps = true;
|
|
||||||
} else if (!gamepad1.ps && already_ps){
|
|
||||||
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
|
|
||||||
if (manualMode){
|
|
||||||
gamepad1.setLedColor(255,0,0,10);
|
|
||||||
lmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
||||||
rmelevator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
||||||
rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
||||||
if (gamepad1.dpad_up){
|
|
||||||
rmelevator.setPower(0.3);
|
|
||||||
lmelevator.setPower(0.3);
|
|
||||||
} else if (gamepad1.dpad_down){
|
|
||||||
lmelevator.setPower(-0.3);
|
|
||||||
rmelevator.setPower(-0.3);
|
|
||||||
} else if (gamepad1.y){
|
|
||||||
double power = -0.3;
|
|
||||||
if (gamepad1.right_bumper){
|
|
||||||
power = -power;
|
|
||||||
}
|
|
||||||
rotation.setPower(power);
|
|
||||||
} else {
|
|
||||||
lmelevator.setPower(0);
|
|
||||||
rmelevator.setPower(0);
|
|
||||||
lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
||||||
rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
||||||
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
gamepad1.setLedColor(0,0,255,10);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!gamepad1.y && already_y && !manualMode) {
|
|
||||||
already_y = false;
|
|
||||||
}
|
|
||||||
if (gamepad1.y && !already_y && !manualMode){
|
|
||||||
already_y = true;
|
|
||||||
int pos = rotation.getCurrentPosition();
|
|
||||||
rotation.setVelocity(200);
|
|
||||||
if (gamepad1.right_bumper){
|
|
||||||
// rotation.setTargetPosition(pos - 25);
|
|
||||||
rotation.setTargetPosition(-100); // vertical si pos origine = 0
|
|
||||||
} else if (gamepad1.left_bumper){
|
|
||||||
// rotation.setTargetPosition(pos + 25);
|
|
||||||
rotation.setTargetPosition(1000); //position basse
|
|
||||||
} else {
|
|
||||||
rotation.setTargetPosition(0);
|
|
||||||
}
|
|
||||||
rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (gamepad1.right_bumper && gamepad1.left_bumper){
|
|
||||||
// launch the plane
|
|
||||||
}
|
|
||||||
|
|
||||||
telemetry.addData("x", x);
|
|
||||||
telemetry.addData("y", y);
|
|
||||||
telemetry.addData("lpow", lpower);
|
|
||||||
telemetry.addData("rpow", rpower);
|
|
||||||
telemetry.addData("ltrigg", t);
|
|
||||||
telemetry.addData("t2", t2);
|
|
||||||
telemetry.addData("manual mode", manualMode);
|
|
||||||
telemetry.addData("rotation power",boxRot);
|
|
||||||
telemetry.addData("mode manuel", manualMode);
|
|
||||||
telemetry.addData("Position elevateur l", lmelevator.getCurrentPosition());
|
|
||||||
telemetry.addData("Position elevateur r", rmelevator.getCurrentPosition());
|
|
||||||
telemetry.addData("Position rotation",rotation.getCurrentPosition());
|
|
||||||
telemetry.addData("Position box",box.getCurrentPosition());
|
|
||||||
telemetry.addData("box velocity",rotation.getVelocity());
|
|
||||||
telemetry.update();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
public void nextMode(){
|
public void nextMode(){
|
||||||
this.RobotMode toNextMode;
|
this.RobotMode toNextMode;
|
||||||
switch(this.mode){
|
switch(this.mode){
|
||||||
case (this.RobotMode.ESSAIFRANCK):
|
case (this.RobotMode.ESSAIFRANCK):
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue