here

src/main.cpp

@@ -16,6 +16,7 @@
 #include "okapi/impl/device/controllerUtil.hpp"
 #include "pros/adi.h"
 #include "pros/adi.hpp"
+#include "pros/distance.hpp"
 #include "pros/llemu.hpp"
 #include "pros/motors.h"
 #include "pros/rtos.hpp"
@@ -92,6 +93,16 @@ bool is_tapped(bool *log, bool ap) {
     }
 };
 
+bool is_tapped_short(bool *log, bool ap) {
+    shiftarray(log, ap);
+    if (log[0] == true && log[1] == false) {
+        printf("a: %d\n", log[0]);
+        return true;
+    } else {
+        return false;
+    }
+};
+
 bool is_held(bool *log, bool ap) {
     shiftarray(log, ap);
     if (log[0] == true && log[1] == true && log[2] == true && log[3] == true &&
@@ -253,15 +264,29 @@ void autonomous() {
         bool reqtopull = false;
         bool good[10];
         pros::Distance dist(13); // fixed
+	int counter = 0;
         pull.tarePosition();
         printf("hereasdsadfasdfsdf\n");
+	bool reqtopullarr[10];
+	for (int i = 0; i<10; i++){
+	    reqtopullarr[i] = false;
+	}
         
-          while (true) {
+	while (counter<44) {
 
-	      if (is_held(good, dist.get() < 70)) {
+
+	      
+	      if (is_held(good, dist.get() < 115)) {
 		  reqtopull = true;
 	      }
 
+	      if(is_tapped_short(reqtopullarr,reqtopull)) {
+		  counter = counter + 1;
+	      }
+	      pros::lcd::set_text(7,std::to_string(counter));
+
+	      
+
 	      // if (pros::millis() % 2000 <= 10){
 	      // 	  reqtopull = true;
 	      // }
@@ -281,6 +306,7 @@ void autonomous() {
 
                     10000000);
                 reqtopull = false;
+
             }
 
             pros::delay(10);
@@ -291,20 +317,57 @@ void autonomous() {
     }
 
     if (autonpick == 3 || autonpick == 8) {
+	pros::delay(500);
+	pros::Distance distcal(20);
+	bool good = false;
+	while (!good) {
+	    if (!(distcal.get() < 35)) {
+                // pull.moveVoltage(-12000);
+                pull.moveVoltage(12000);
+
+            } else {
+                pullback = false;
+                ready = true;
+ 		good = true;
+                pull.moveVoltage(0);
+                pull.tarePosition();
+            }
+
+	}
 	drive->setMaxVelocity(300);
-	drive->moveDistance(-7_ft);
+	drive->turnAngle(30_deg);
-	drive->turnAngle(-60_deg);
-
 	drive->setMaxVelocity(600);
-	//piston.set_value(true);
-	drive->moveDistanceAsync(-5_ft);
-	pros::delay(1200);
-	//piston.set_value(false);
 
-	drive->moveDistance(0.7_ft);
+	drive->moveDistance(-7.1_ft);
+
+
+	pros::delay(500);
+
+	double yaw = IMU.get_heading();
+        std::printf("%f\n", yaw);
+        double turn = -60 - yaw;
+	
+        drive->setMaxVelocity(300);
+        drive->turnAngle(turn * 1_deg);
+	
 	pros::delay(300);
+	drive->moveDistanceAsync(-5_ft);
+	pros::delay(2000);
+
+	piston.set_value(true);
+
+
+	//drive->turnAngle(90_deg);
+	yaw = IMU.get_heading();
+	turn = 30 - yaw;
+	drive->turnAngle(turn*1_deg);
+	
 	drive->moveDistance(-3_ft);
 
+	drive->turnAngle(90_deg);
+	drive->moveDistance(-4_ft);
+	
+	
     }
 
     
@@ -321,6 +384,11 @@ void autonomous() {
     if (autonpick == 5) {
         pull.moveRelative(900, 1000000);
     }
+
+    if (autonpick==6) {
+	drive->moveDistance(-1.95_ft);
+	drive->moveDistance(1_ft);
+    }
 }
 
 /**
@@ -359,8 +427,8 @@ void opcontrol() {
 
     pros::ADIDigitalIn limit(1);
     pros::ADIDigitalOut piston(2);
-    pros::ADIDigitalOut upflap(3);
+    pros::ADIDigitalOut funnypiston(3);
-    pros::ADIDigitalOut funnypiston(8);
+    pros::ADIDigitalOut upflap(8);
 
     Motor leftMotor1(-5);
     Motor leftMotor2(-6);
@@ -370,6 +438,9 @@ void opcontrol() {
     Motor rightMotor2(2);
     Motor rightMotor3(3);
 
+    //lift on Ltrig
+
+    // intake on A
 
     leftMotor1.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
     leftMotor2.setBrakeMode(okapi::AbstractMotor::brakeMode::brake);
@@ -386,7 +457,7 @@ void opcontrol() {
     Motor pull(12);
     //pull.setReversed(true);
 
-    Motor lift(16);
+    Motor lift(18);
 
     // Motor pull2(11);
 
@@ -419,6 +490,7 @@ void opcontrol() {
 
     ControllerButton Abutton(ControllerDigital::A);
     
+
     ControllerButton uparrow(ControllerDigital::up);
     ControllerButton Xbutton(ControllerDigital::X);
     ControllerButton downarrow(ControllerDigital::down);
@@ -475,6 +547,12 @@ void opcontrol() {
         modeswap[i] = false;
     }
 
+    bool barr[10];
+    for (int i = 0; i < 10; i++){
+	barr[i] = false;
+    }
+
+    
     bool funnypistonstatus = false;
     auto IMU = okapi::IMU(19, IMUAxes::z);
     bool reqtoloop = false;
@@ -488,6 +566,9 @@ void opcontrol() {
 
     pros::Distance dististriball(11); // fixed
 
+    bool intakebool;
+    intakebool = false;
+
     while (true) {
         // Tank drive with left and right sticks.
         drive->getModel()->tank(controller.getAnalog(ControllerAnalog::leftY),
@@ -545,11 +626,11 @@ void opcontrol() {
             tcount++;
         }
 
-        if (is_tapped(funnyarr, leftarrow.isPressed())) {
+        if (is_tapped(funnyarr, downarrow.isPressed())) {
             funnypistonstatus = !funnypistonstatus;
         }
 
-        if (is_tapped(y1arr, Abutton.isPressed())) {
+        if (is_tapped(y1arr, Bbutton.isPressed())) {
             upflapstatus = !upflapstatus;
         }
 
@@ -646,16 +727,16 @@ void opcontrol() {
         //     pros::lcd::set_text(5, "hit while not");
         // }
 
-        if (is_tapped(loopmodearr, downarrow.isPressed())) {
+        // if (is_tapped(loopmodearr, downarrow.isPressed())) {
         //     lmode = !lmode;
-            lmode = false;
+        //     //lmode = false;
-        }
+        // }
 
-        // printf("%i",dist.get());
+        // // printf("%i",dist.get());
-        if (is_held(distarr, dist.get() < 70) && lmode) {
+        // if (is_held(distarr, dististriball.get() < 70) && lmode) {
-            reqtoloop = true;
+        //     reqtoloop = true;
-            // printf("raaaaaaaaaaaaaa");
+        //     // printf("raaaaaaaaaaaaaa");
-        }
+        // }
 
         if (is_tapped(r1arr, R1.isPressed()) || reqtoloop == true) {
             float temp = std::fmod(pull.getPosition(), 900);
@@ -691,25 +772,12 @@ void opcontrol() {
         
         pros::lcd::set_text(4, std::to_string(pull.getTargetPosition()));
 
-        if (Bbutton.isPressed()) {
+       
-            pull.moveVoltage(12000);
+	
-        }
-        if (uparrow.isPressed()) {
-            pull.moveVoltage(0);
-        }
 
         //pros::lcd::set_text(5, std::to_string(IMU.get()));
 
-        // if (L2.isPressed()) {
+      
-        //     intake.moveVoltage(-12000);
-	//     testMotor.moveVoltage(-12000);
-        // } else if (L1.isPressed()) {
-        //     intake.moveVoltage(12000);
-	//     testMotor.moveVoltage(-12000);
-        // } else {
-        //     intake.moveVoltage(0);
-	//     testMotor.moveVoltage(0);
-        // }
 
 	pros::lcd::set_text(5,std::to_string(pull.getPosition()));