diff --git a/src/main.cpp b/src/main.cpp index 88003df..e103124 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -212,7 +212,7 @@ void autonomous() { bool pullback = false; bool ready = false; - Motor pull(10); + Motor pull(12); printf("%d\n", autonpick); // pull.setReversed(true); @@ -224,7 +224,7 @@ void autonomous() { drive->moveDistance(1_ft); double yaw = IMU.get_heading(); std::printf("%f", yaw); - double turn = -95 - yaw; + double turn = -92 - yaw; drive->setMaxVelocity(300); drive->turnAngle(turn * 1_deg); drive->moveDistanceAsync(-4.1_ft); @@ -252,15 +252,19 @@ void autonomous() { } else if (autonpick == 3) { bool reqtopull = false; bool good[10]; - pros::Distance dist(11); // fixed + pros::Distance dist(13); // fixed pull.tarePosition(); - + printf("hereasdsadfasdfsdf\n"); while (true) { - if (is_held(good, dist.get() < 70)) { - reqtopull = true; - } + if (is_held(good, dist.get() < 70)) { + reqtopull = true; + } + + // if (pros::millis() % 2000 <= 10){ + // reqtopull = true; + // } if (reqtopull) { // pull.moveRelative((1800 - std::fmod(pull.getPosition(), // 1800)), @@ -280,8 +284,30 @@ void autonomous() { } pros::delay(10); - } + } + + + } + + if (autonpick == 3 || autonpick == 8) { + drive->setMaxVelocity(300); + drive->moveDistance(-7_ft); + 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); + pros::delay(300); + drive->moveDistance(-3_ft); + + } + + if (autonpick == -1) { drive->turnAngle(90_deg); } @@ -357,7 +383,7 @@ void opcontrol() { Motor flywheel(15); - Motor pull(11); + Motor pull(12); //pull.setReversed(true); Motor lift(16); @@ -373,7 +399,7 @@ void opcontrol() { // MotorGroup pull = MotorGroup({pull1, pull2}); flywheel.setBrakeMode(okapi::AbstractMotor::brakeMode::coast); flywheel.setGearing(okapi::AbstractMotor::gearset::blue); //should be red for cata - pull.setBrakeMode(okapi::AbstractMotor::brakeMode::hold); + pull.setBrakeMode(okapi::AbstractMotor::brakeMode::coast); lift.setBrakeMode(okapi::AbstractMotor::brakeMode::hold); @@ -633,11 +659,13 @@ void opcontrol() { if (is_tapped(r1arr, R1.isPressed()) || reqtoloop == true) { float temp = std::fmod(pull.getPosition(), 900); + pros::lcd::set_text(6,std::to_string(temp)); if ((900 - temp) < 150) { temp = -(900 - temp); + pros::lcd::set_text(7,std::to_string(temp)); } pull.moveRelative( - (900 - + (900 - temp), // pull.moveRelative((1800 - // std::fmod(pull.getPosition(), 1800)), @@ -670,7 +698,7 @@ void opcontrol() { pull.moveVoltage(0); } - pros::lcd::set_text(5, std::to_string(IMU.get())); + //pros::lcd::set_text(5, std::to_string(IMU.get())); // if (L2.isPressed()) { // intake.moveVoltage(-12000); @@ -683,7 +711,7 @@ void opcontrol() { // testMotor.moveVoltage(0); // } - pros::lcd::set_text(5,std::to_string(pull.getTargetPosition())); + pros::lcd::set_text(5,std::to_string(pull.getPosition())); controller.setText(