Update ftc2024_autonome_api.java

This commit is contained in:
GZod01 2024-04-05 16:46:09 +02:00
parent 445983495f
commit cfcbbd5fe2
2 changed files with 14 additions and 9 deletions

View file

@ -22,7 +22,10 @@ import com.qualcomm.robotcore.util.ElapsedTime;
@Autonomous @Autonomous
public class Ftc2024_autonome_api extends LinearOpMode { public class Ftc2024_autonome_api extends LinearOpMode {
public String autonomous_mode; public enum AutoMode{
B2D,B4D,B2N,B4N,R2D,R4D,R2N,R4N
}
public AutoMode autonomous_mode;
public DcMotor lm; public DcMotor lm;
public DcMotor rm; public DcMotor rm;
public DcMotor harvestmotor; public DcMotor harvestmotor;
@ -77,7 +80,7 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.backward(1); robot.backward(1);
robot.harvest(0); robot.harvest(0);
break; break;
case ("b2d"): case B2D:
robot.forward(0.5); robot.forward(0.5);
robot.rotate((-90)); robot.rotate((-90));
robot.forward(2.5); robot.forward(2.5);
@ -85,7 +88,7 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.backward(1); robot.backward(1);
robot.harvest(0); robot.harvest(0);
break; break;
case ("r4d"): case R4D:
robot.forward(0.5); robot.forward(0.5);
robot.rotate(90); robot.rotate(90);
robot.forward(1.5); robot.forward(1.5);
@ -93,7 +96,7 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.backward(1); robot.backward(1);
robot.harvest(0); robot.harvest(0);
break; break;
case ("r2d"): case R2D:
robot.forward(0.5); robot.forward(0.5);
robot.rotate(90); robot.rotate(90);
robot.forward(2.5); robot.forward(2.5);
@ -101,7 +104,7 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.backward(1); robot.backward(1);
robot.harvest(0); robot.harvest(0);
break; break;
case ("b4n"): case B4N:
robot.forward(1.5); robot.forward(1.5);
robot.rotate(90); robot.rotate(90);
robot.harvest(); robot.harvest();
@ -117,7 +120,7 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.backward(1); robot.backward(1);
robot.harvest(0); robot.harvest(0);
break; break;
case ("b2n"): case B2N:
robot.forward(1.5); robot.forward(1.5);
robot.rotate(90); robot.rotate(90);
robot.harvest(); robot.harvest();
@ -133,7 +136,7 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.backward(1); robot.backward(1);
robot.harvest(0); robot.harvest(0);
break; break;
case ("r4n"): case R4N:
robot.forward(1.5); robot.forward(1.5);
robot.rotate(-90); robot.rotate(-90);
robot.harvest(); robot.harvest();
@ -149,7 +152,7 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.backward(1); robot.backward(1);
robot.harvest(0); robot.harvest(0);
break; break;
case ("r2n"): case R2N:
robot.forward(1.5); robot.forward(1.5);
robot.rotate(-90); robot.rotate(-90);
robot.harvest(); robot.harvest();

View file

@ -370,6 +370,7 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode {
rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rotation.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
box.setMode(DcMotor.RunMode.RUN_USING_ENCODER); box.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
telemetry.addData("mode","MANUEL");
// Elevator manual mode // Elevator manual mode
if (gamepad1.dpad_up) { if (gamepad1.dpad_up) {
// rmelevator.setPower(0.3); // rmelevator.setPower(0.3);
@ -406,6 +407,7 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode {
// lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); // rmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
} }
// Box manual mode // Box manual mode
if (gamepad1.dpad_left) { if (gamepad1.dpad_left) {
@ -434,6 +436,7 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode {
} else { } else {
gamepad1.setLedColor(0.0, 0.0, 0.0,10); gamepad1.setLedColor(0.0, 0.0, 0.0,10);
telemetry.addData("mode","AUTOMATIQUE");
if (!gamepad1.dpad_right && already_padright) { if (!gamepad1.dpad_right && already_padright) {
already_padright = false; already_padright = false;
@ -539,7 +542,6 @@ public class WEROBOT_FTC2024_New_carlike extends LinearOpMode {
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("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());