diff --git a/src/main.cpp b/src/main.cpp index e103124..dab2d07 100644 --- a/src/main.cpp +++ b/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) { - - if (is_held(good, dist.get() < 70)) { + while (counter<44) { + + + + 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,19 +317,56 @@ 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(-60_deg); - + drive->turnAngle(30_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(8); + pros::ADIDigitalOut funnypiston(3); + 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); @@ -418,6 +489,7 @@ void opcontrol() { ControllerButton leftarrow(ControllerDigital::left); ControllerButton Abutton(ControllerDigital::A); + ControllerButton uparrow(ControllerDigital::up); ControllerButton Xbutton(ControllerDigital::X); @@ -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())) { - // lmode = !lmode; - lmode = false; - } + // if (is_tapped(loopmodearr, downarrow.isPressed())) { + // lmode = !lmode; + // //lmode = false; + // } - // printf("%i",dist.get()); - if (is_held(distarr, dist.get() < 70) && lmode) { - reqtoloop = true; - // printf("raaaaaaaaaaaaaa"); - } + // // printf("%i",dist.get()); + // if (is_held(distarr, dististriball.get() < 70) && lmode) { + // reqtoloop = true; + // // 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()));