From caf88ca7a51d7a9187a5433aa8832f5bc8cabbe9 Mon Sep 17 00:00:00 2001
From: Zelina974 <elina.claerebout@gmail.com>
Date: Sat, 6 Apr 2024 11:56:53 +0200
Subject: [PATCH] update

---
 FTC2024WeRobotControl.java |  7 +++++--
 ftc2024_autonome_api.java  | 35 +++++++++++++++++++++++++++++------
 2 files changed, 34 insertions(+), 8 deletions(-)

diff --git a/FTC2024WeRobotControl.java b/FTC2024WeRobotControl.java
index c4fbf18..b2ed22c 100644
--- a/FTC2024WeRobotControl.java
+++ b/FTC2024WeRobotControl.java
@@ -68,10 +68,13 @@ public class FTC2024WeRobotControl {
     public void boxElv(){
         Parent.lmelevator.setVelocity(600);
         Parent.rmelevator.setVelocity(600);
-        Parent.lmelevator.setTargetPosition(90);
-        Parent.rmelevator.setTargetPosition(90);
+        Parent.rotation.setVelocity(600);
+        Parent.lmelevator.setTargetPosition(50);
+        Parent.rmelevator.setTargetPosition(50);
+        Parent.rotation.setTargetPosition(30);
         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(){
diff --git a/ftc2024_autonome_api.java b/ftc2024_autonome_api.java
index 7638547..7355321 100644
--- a/ftc2024_autonome_api.java
+++ b/ftc2024_autonome_api.java
@@ -37,12 +37,19 @@ public class Ftc2024_autonome_api extends LinearOpMode {
 
     @Override
     public void runOpMode() {
+        
+        boolean auto = false; 
         lm = hardwareMap.get(DcMotor.class, "blm");
         rm = hardwareMap.get(DcMotor.class, "brm");
         harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse");
         rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
         lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
+        lmelevator.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        lmelevator.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         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.setDirection(DcMotor.Direction.REVERSE);
         rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -68,7 +75,13 @@ public class Ftc2024_autonome_api extends LinearOpMode {
         telemetry.update();
         robotOrientation = imu.getRobotYawPitchRollAngles();
 
-        if (opModeIsActive()) {
+        while (opModeIsActive()) {
+            if (gamepad1.a && !auto){
+                auto = true;
+                break;
+            }
+        }
+        if (opModeIsActive()){
             /*
              * autonomous_mode differents possibles values respect the next scheme:
              * team_color_shortcode + start_line_index + direct_or_no
@@ -81,11 +94,15 @@ public class Ftc2024_autonome_api extends LinearOpMode {
              *
              * default is "b4d"
              */
+            
             switch (autonomous_mode) {
                 default:
                     robot.boxElv();
+                    robot.harvest(1);
                     robot.forward(2);
+                    robot.harvest(0);
                     robot.rotate((-90));
+                    robot.posBasse();
                     robot.forward(3.5);
                     robot.harvest(-1);
                     robot.backward(0.5);
@@ -93,18 +110,21 @@ public class Ftc2024_autonome_api extends LinearOpMode {
                     robot.forward(0.5);
                     break;
                 case B2D:
-                    robot.boxElv();
+                    robot.posBasse();
+                    robot.harvest(1);
                     robot.forward(0.5);
-                    robot.rotate((-90));
-                    robot.forward(1.5);
-                    robot.harvest(-1);
-                    robot.backward(1);
                     robot.harvest(0);
                     robot.forward(1);
+                    robot.harvest(-1);
+                    robot.backward(0.5);
+                    robot.harvest(0);
+                    robot.forward(0.5);
                     break;
                 case R4D:
                     robot.boxElv();
+                    robot.harvest(1);
                     robot.forward(2);
+                    robot.harvest(0);
                     robot.rotate(90);
                     robot.posBasse();
                     robot.forward(3.5);
@@ -115,8 +135,11 @@ public class Ftc2024_autonome_api extends LinearOpMode {
                     break;
                 case R2D:
                     robot.boxElv();
+                    robot.harvest(1);
                     robot.forward(0.5);
+                    robot.harvest(0);
                     robot.rotate(90);
+                    robot.posBasse();
                     robot.forward(1.5);
                     robot.harvest(-1);
                     robot.backward(0.5);