diff --git a/src/main.cpp b/src/main.cpp index f41ea2e..7a03c04 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -300,19 +300,43 @@ void autonomous() { if (autonpick == 0) { // Auton #1, used for adding tribal to goal float x = -2.7; drive->moveDistanceAsync(x * 1_ft); - pros::delay(2500); - drive->moveDistance(0.5_ft); - double yaw = IMU.get_heading(); - std::printf("yaw: %f\n", yaw); - // double turn = -92 - yaw; - double turn = 45 - yaw; - std: - printf("turn: %f\n", turn); - drive->setMaxVelocity(300); - drive->turnAngle(turn * 1_deg); - drive->moveDistanceAsync(4.1_ft); - pros::delay(3000); + pros::delay(1500); + drive->moveDistance(0.7_ft); + pros::delay(100); + + pros::Distance distforward(17); + + // double yaw = IMU.get_heading(); + // std::printf("yaw: %f\n", yaw); + // double turn = -92 - yaw; + // double turn = 53 - yaw; + // std:: printf("turn: %f\n", turn); + // drive->setMaxVelocity(300); + // drive->turnAngle(turn * 1_deg); + bool imuholdarray[10]; + + while (!is_held(imuholdarray, (abs(65.0 - IMU.get_rotation()) < 1))) { + drive->getModel()->rotate((65.0 - IMU.get_rotation()) * 0.008); + pros::delay(10); + } + drive->stop(); + + drive->getModel()->stop(); + while (distforward.get() > 150) { + drive->getModel()->curvature(12000, + (IMU.get_rotation() - 65.0) * -0.04); + + // printf("%i\n", distforward.get()); + pros::delay(10); + } + + drive->getModel()->stop(); + + pros::delay(700); piston.set_value(true); + drive->setMaxVelocity(150); + drive->getModel()->setBrakeMode(okapi::AbstractMotor::brakeMode::hold); + drive->moveDistance(5_in); } else if (autonpick == 1 || autonpick == 2) { // Auton #2, Used for removing triball from match loader @@ -323,13 +347,34 @@ void autonomous() { pros::delay(1000); piston.set_value(false); // drive->moveDistance(-1.7_ft); - drive->turnAngle(115_deg); - drive->moveDistance(3.5_ft); - drive->turnAngle(95_deg); + //drive->turnAngle(115_deg); + bool imuholdarray[10]; + while (!is_held(imuholdarray, + (abs(115.0 - IMU.get_rotation()) < 3))) { + drive->getModel()->rotate((115.0 - IMU.get_rotation()) * + 0.004); + pros::delay(10); + } + + drive->getModel()->stop(); + + drive->moveDistance(3.69_ft); + + //drive->turnAngle(95_deg); + + while (!is_held(imuholdarray, + (abs(225.0 - IMU.get_rotation()) < 3))) { + drive->getModel()->rotate((225.0 - IMU.get_rotation()) * + 0.004); + pros::delay(10); + } + + drive->getModel()->stop(); + drive->setMaxVelocity(50); - drive->moveDistanceAsync(2_ft); - pros::delay(2500); piston.set_value(true); + drive->moveDistanceAsync(1.7_ft); + pros::delay(2500); drive->moveDistance(4_in); } @@ -602,12 +647,13 @@ void autonomous() { drive->setMaxVelocity(600); - // while (!is_held(imuholdarray, (abs(100.0 - IMU.get_rotation()) < 1))) { + // while (!is_held(imuholdarray, (abs(100.0 - IMU.get_rotation()) < 1))) + // { // drive->getModel()->rotate((100.0 - IMU.get_rotation()) * 0.008); // pros::delay(10); // } - drive->turnAngle(100_deg); + drive->turnAngle(100_deg); drive->getModel()->stop(); @@ -615,18 +661,19 @@ void autonomous() { drive->moveDistanceAsync(2_ft); - pros::delay(1000); + pros::delay(700); drive->moveDistance(-1_ft); while (!is_held(imuholdarray, (abs(-110.0 - IMU.get_rotation()) < 3))) { - drive->getModel()->rotate((-110.0 - IMU.get_rotation()) * 0.008); + drive->getModel()->rotate((-110.0 - IMU.get_rotation()) * 0.004); pros::delay(10); } - drive->getModel()->stop(); - while (distforward.get() > 300) { - drive->getModel()->curvature(12000, (IMU.get_rotation()+110) * -0.04); + drive->getModel()->stop(); + while (distforward.get() > 300) { + drive->getModel()->curvature(12000, + (IMU.get_rotation() + 110) * -0.04); // printf("%i\n", distforward.get()); pros::delay(10); @@ -635,44 +682,133 @@ void autonomous() { funnypiston.set_value(true); pros::delay(500); - drive->moveDistance(-1_ft); + drive->moveDistance(-1_ft); - while (!is_held(imuholdarray, (abs(75.0 - IMU.get_rotation()) < 3))) { - drive->getModel()->rotate((75.0 - IMU.get_rotation()) * 0.008); + while (!is_held(imuholdarray, (abs(85.0 - IMU.get_rotation()) < 3))) { + drive->getModel()->rotate((85.0 - IMU.get_rotation()) * 0.004); // 98 pros::delay(10); } - drive->getModel()->stop(); + drive->getModel()->stop(); - funnypiston.set_value(false); - - drive->moveDistanceAsync(3_ft); + funnypiston.set_value(false); - pros::delay(1000); + drive->moveDistanceAsync(3_ft); - drive->moveDistance(-1_ft); + pros::delay(700); - while (!is_held(imuholdarray, (abs(-70.0 - IMU.get_rotation()) < 3))) { - drive->getModel()->rotate((-70.0 - IMU.get_rotation()) * 0.008); + drive->moveDistance(-1_ft); + + while (!is_held(imuholdarray, (abs(-70.0 - IMU.get_rotation()) < 3))) { + drive->getModel()->rotate((-70.0 - IMU.get_rotation()) * 0.004); pros::delay(10); } - drive->getModel()->stop(); + drive->getModel()->stop(); - drive->moveDistance(1_ft); - - funnypiston.set_value(true); - pros::delay(300); + while (distforward.get() > 300) { + drive->getModel()->curvature(12000, + (IMU.get_rotation() + 70.0) * -0.04); - drive->turnAngle(180_deg); - funnypiston.set_value(false); + // printf("%i\n", distforward.get()); + pros::delay(10); + } + drive->getModel()->stop(); - drive->moveDistanceAsync(3_ft); - pros::delay(1000); - drive->moveDistanceAsync(-1_ft); + funnypiston.set_value(true); + pros::delay(300); + + drive->turnAngle(180_deg); + funnypiston.set_value(false); + + drive->moveDistanceAsync(3_ft); + pros::delay(700); + drive->moveDistanceAsync(-1_ft); // piston.set_value(false); } + if (autonpick == 10) { + + piston.set_value(true); + pros::delay(300); + piston.set_value(false); + 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() > 300) { + drive->getModel()->curvature(12000, IMU.get_rotation() * -0.04); + + // printf("%i\n", distforward.get()); + pros::delay(10); + } + drive->stop(); + funnypiston.set_value(true); + pros::delay(500); + + // drive->moveDistance(-1_ft); + bool imuholdarray[10]; + while (!is_held(imuholdarray, (abs(130.0 - IMU.get_rotation()) < 3))) { + drive->getModel()->rotate((130.0 - IMU.get_rotation()) * 0.004); + pros::delay(10); + } + + drive->getModel()->stop(); + pros::delay(100); + + drive->moveDistance(-0.5_ft); + + funnypiston.set_value(false); + piston.set_value(true); + + drive->moveDistanceAsync(2.5_ft); + pros::delay(1000); + piston.set_value(false); + drive->moveDistance(-1_ft); + pros::delay(200); + + while (!is_held(imuholdarray, (abs(-82.0 - IMU.get_rotation()) < 3))) { + drive->getModel()->rotate((-82.0 - IMU.get_rotation()) * 0.004); + pros::delay(10); + } + + drive->getModel()->stop(); + + while (distforward.get() > 300) { + drive->getModel()->curvature(12000, + (IMU.get_rotation() + 90.0) * -0.04); + + // printf("%i\n", distforward.get()); + pros::delay(10); + } + funnypiston.set_value(true); + drive->getModel()->stop(); + pros::delay(700); + + while (!is_held(imuholdarray, (abs(165.0 - IMU.get_rotation()) < 3))) { + drive->getModel()->rotate((165.0 - IMU.get_rotation()) * 0.004); + pros::delay(10); + } + + drive->getModel()->stop(); + + pros::delay(200); + + drive->moveDistance(3_ft); + + drive->turnAngle(-110_deg); + pros::delay(200); + + funnypiston.set_value(false); + piston.set_value(true); + + drive->moveDistanceAsync(3_ft); + pros::delay(1200); + drive->moveDistance(-1_ft); + piston.set_value(false); + } } /** @@ -1084,3 +1220,7 @@ void opcontrol() { pros::delay(10); } } +// code sukz lol +// moaning == goofy +// housecats mid +// t team better