diff --git a/AutonomeTest.java b/AutonomeTest.java index 581cd96..49687ba 100644 --- a/AutonomeTest.java +++ b/AutonomeTest.java @@ -17,6 +17,7 @@ public class AutonomeTest extends LinearOpMode { private DcMotorEx boiteG; private DcMotorEx boiteD; private ElapsedTime runtime = new ElapsedTime(); + private DcMotor moissonneuse; @Override public void runOpMode(){ @@ -34,7 +35,7 @@ public class AutonomeTest extends LinearOpMode { droit.setDirection(DcMotorSimple.Direction.REVERSE); droit.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); droit.setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.FLOAT); - moissonneuse = hardwareMap.get(DcMotor.class,"msn"); + moissonneuse = hardwareMap.get(DcMotor.class,"moissonneuse"); waitForStart(); @@ -64,12 +65,14 @@ public class AutonomeTest extends LinearOpMode { gauche.setTargetPosition(165); droit.setTargetPosition(1235); - while (opModeIsActive() && gauche.getCurrentPosition()>gauche.getTargetPosition()){ + while (opModeIsActive() && droit.getCurrentPosition()>droit.getTargetPosition()){ gauche.setVelocity(250); gauche.setMode(DcMotor.RunMode.RUN_TO_POSITION); droit.setVelocity(250); droit.setMode(DcMotor.RunMode.RUN_TO_POSITION); } + gauche.setVelocity(0); + droit.setVelocity(0); gauche.setTargetPosition(700*3); droit.setTargetPosition(700*3); diff --git a/fgc2024Pilotage.java b/fgc2024Pilotage.java index d36100a..f8b1d47 100644 --- a/fgc2024Pilotage.java +++ b/fgc2024Pilotage.java @@ -22,42 +22,47 @@ import org.firstinspires.ftc.robotcore.external.navigation.Orientation; import org.firstinspires.ftc.robotcore.external.navigation.Position; import org.firstinspires.ftc.robotcore.external.navigation.Velocity; -@TeleOp(name = "WeRobot: FTC2024 NEW! Carlike", group = "WeRobot") -public class WEROBOT_FTC2024_New_carlike extends LinearOpMode { - +@TeleOp(name = "FGC2024", group = "WeRobot") +public class FGC_2024 extends LinearOpMode { + private DcMotorEx rm; private DcMotorEx lm; @Override public void runOpMode() throws InterruptedException { - - float x = gamepad1.left_stick_x; // abscisse joystick gauche - double y = gamepad1.left_stick_y; // ordonnées joystick gauche - double lpower = 0.0; //puissance moteur gauche - double rpower = 0.0; //puissance moteur droit - telemetry.addData("Status"," Initialized"); - lm = hardwareMap.get(DcMotorEx.class, "lm"); - rm = hardwareMap.get(DcMotorEx.class, "rm"); - lm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - rm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + telemetry.addData("Status"," Initialized"); + telemetry.update(); - lpower = ((1+x)*Math.signum(y))/2; - rpower = ((1-x)*Math.signum(y))/2; + lm = hardwareMap.get(DcMotorEx.class, "lm"); + lm.setDirection(DcMotor.Direction.REVERSE); + rm = hardwareMap.get(DcMotorEx.class, "rm"); + //lm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //rm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - if ( x=1 || x=-1 ){ - lpower = 1*Math.signum(x); - rpower = - lpower; - } + waitForStart(); + while(opModeIsActive()){ + float x = gamepad1.left_stick_x; // abscisse joystick gauche + double y = gamepad1.left_stick_y; // ordonnées joystick gauche + double lpower = 0.0; //puissance moteur gauche + double rpower = 0.0; //puissance moteur droit - lpower = lpower*gamepad1.left_trigger; - rpower = lpower*gamepad1.left_trigger; + lpower = ((1+x)*Math.signum(y))/2; + rpower = ((1-x)*Math.signum(y))/2; - rm.setPower(rpower); - lm.setPower(lpower); + if (Math.abs(x)>0.8){ + lpower = 1*Math.signum(x); + rpower = -lpower; + } + + lpower = lpower*gamepad1.left_trigger; + rpower = lpower*gamepad1.left_trigger; + + rm.setPower(rpower); + lm.setPower(lpower); + telemetry.update(); + } } -} \ No newline at end of file +}