From a7404de94c69b82e190e985e9afebe9587d7ce51 Mon Sep 17 00:00:00 2001
From: InventorXtreme <inventorxtreme@gmail.com>
Date: Sat, 10 Feb 2024 10:47:00 -0500
Subject: [PATCH] rrr

---
 src/main.cpp | 89 +++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 85 insertions(+), 4 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 43bcb1d..e0360b0 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -148,6 +148,10 @@ void disabled() {}
  * starts.
  */
 void competition_initialize() {
+	pros::Imu IMU(19);
+    IMU.reset(true);
+    IMU.tare();
+    pros::delay(200);
 
     lv_obj_t *guibtn1 = lv_btn_create(lv_scr_act(), NULL);
     lv_btn_set_action(guibtn1, LV_BTN_ACTION_CLICK, sa0);
@@ -222,7 +226,7 @@ void autonomous() {
     upflap.set_value(false);
 
     pros::Imu IMU(19);
-    IMU.reset(true);
+    //IMU.reset(true);
     IMU.tare();
     pros::delay(200);
 
@@ -243,8 +247,7 @@ void autonomous() {
         std::printf("yaw: %f\n", yaw);
         // double turn = -92 - yaw;
         double turn = 45 - yaw;
-    std:
-        printf("turn: %f\n", turn);
+    std:printf("turn: %f\n", turn);
         drive->setMaxVelocity(300);
         drive->turnAngle(turn * 1_deg);
         drive->moveDistanceAsync(4.1_ft);
@@ -435,6 +438,84 @@ void autonomous() {
         pros::delay(500);
         drive->moveDistance(0.5_ft);
     }
+
+	if (autonpick == 7) {
+		drive->moveDistance(-1_ft);
+		piston.set_value(true);
+		pros::delay(44500);
+		drive->moveDistance(1_ft);
+		piston.set_value(true);
+		drive->turnAngle(-45_deg);
+		funnypiston.set_value(true);
+		pros::delay(500);
+		drive->moveDistance(6.5_ft);
+		drive->turnAngle(-45_deg);
+		drive->moveDistance(1_ft);
+		drive->moveDistance(-1_ft);
+		drive->moveDistance(1_ft);
+		drive->moveDistance(-1_ft);
+	}
+	if (autonpick == 8) {
+        pros::Distance distforward(17);
+        // drive->moveDistanceAsync(500_ft);
+        drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::hold);
+        // piston.set_value(true);
+        // NEED TO WRTIE IMU CENTER CODE
+        // update IMU each cycle to get true rotation
+        while (distforward.get() > 400) {
+            drive->getModel()->curvature(12000, IMU.get_rotation() * -0.04);
+
+            // printf("%i\n", distforward.get());
+            pros::delay(10);
+        }
+        // piston.set_value(false);
+
+        drive->stop();
+        funnypiston.set_value(true);
+        pros::delay(500);
+
+		auto starttime = Timer().millis();
+        while (Timer().millis() < starttime + 1.5_s  ) {
+            drive->getModel()->curvature(-6000 , IMU.get_rotation() * -0.04);
+
+            // printf("%i\n", distforward.get());
+            pros::delay(10);
+        }
+        // piston.set_value(false);
+        drive->stop();
+        funnypiston.set_value(true);
+        pros::delay(500);
+
+		drive->moveDistance(1_in);
+        // drive->turnAngle(15_deg);
+        //drive->moveDistance(-3.8_ft);
+        //drive->turnAngle(90_deg);
+		bool imuholdarray[10];
+		
+		while (!is_held(imuholdarray, (abs(70.0 - IMU.get_rotation()) < 1)) ) {
+			drive->getModel()->rotate((70.0 - IMU.get_rotation()) * 0.008 );
+			pros::delay(10);
+		}
+
+		drive->getModel()->stop();
+
+		funnypiston.set_value(false);
+        drive->moveDistance(2_ft);
+        drive->moveDistance(-2_ft);
+		funnypiston.set_value(true);
+		drive->moveDistance(2_ft);
+		drive->moveDistance(-2_ft);
+		funnypiston.set_value(false);
+
+		// //drive->moveDistance(0_ft);
+		// pros::delay(300);
+		// drive->turnAngle(50_deg);
+		// pros::delay(300);
+		// drive->moveDistance(-1.5_ft);
+		}
+
+	
+		
 }
 
 /**
@@ -782,7 +863,7 @@ void opcontrol() {
             pull.moveRelative(
                 (900 -
                  temp), //			  pull.moveRelative((1800
-                        //- 			  std::fmod(pull.getPosition(),
+                        //- 		  std::fmod(pull.getPosition(),
                         // 1800)),
 
                 10000000);