diff --git a/include/autons.hpp b/include/autons.hpp index be80686..c7011f8 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -16,13 +16,22 @@ void default_constants(); void empty(); -void skills(); -void skills_without_odom(); +void norcal_skills(); +void safe_skills(); +void left_4_3_push(); +void top_middle_bottom(); +void left_elims_quick_ml(); void left_elims(); void red_top_elims(); void blue_top_quals(); +void left_7_mid(); +void push_solo(); +void left_elims_7ball(); + +void intake_test(); + void blue_bottom_elims(); void red_bottom_elims(); void blue_bottom_quals(); @@ -32,6 +41,8 @@ void new_elim_auton(); void solo_left(); +void pid_test(); + /* Odom TESTING Functions */ void odom_pure_pursuit_wait_until_example(); @@ -40,6 +51,7 @@ void square_odom_test(); void auton_setup_left(); void auton_setup_right(); void solo_right(); +void elims_mid_control(); /* Wall Tracking TEST */ @@ -48,15 +60,15 @@ void wall_alignment_test(); void pid_tune(); /* safe routes */ -void left_safe(); -void right_safe(); +void left_middle_top(); +void right_4_3_push(); /* old */ void right_safe1(); void right_auton_new(); void left_elims_quick(); -void right_elims_quick(); +void right_elims_7ball(); /* Color Sort tests */ void color_sort_test(); diff --git a/include/main.h b/include/main.h index 964ac36..573cfb7 100644 --- a/include/main.h +++ b/include/main.h @@ -37,6 +37,7 @@ extern std::string color; void autonomous(void); //void color_sort_task(void); void color_sort_S(void); +void color_sort_bottom(void); void initialize(void); void disabled(void); void competition_initialize(void); diff --git a/include/subsystems.hpp b/include/subsystems.hpp index 40bfaaf..da6c525 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -15,37 +15,44 @@ inline pros::Controller master(pros::E_CONTROLLER_MASTER); extern Drive chassis; -inline pros::Motor L1(10); -inline pros::Motor L2(-7); -inline pros::Motor L3(-4); +inline pros::Motor L1(-6); +inline pros::Motor L2(-5); +inline pros::Motor L3(-8); -inline pros::Motor R1(-3); -inline pros::Motor R2(6); -inline pros::Motor R3(1); +inline pros::Motor R1(14); +inline pros::Motor R2(19); +inline pros::Motor R3(18); -inline pros::Imu inertial(2); +inline pros::Imu inertial(15); -inline pros::Distance distance_back_l(13); -inline pros::Distance distance_front_l(19); -inline pros::Distance distance_back_r(17); -inline pros::Distance distance_front_r(18); -inline pros::Distance distance_front(14); +inline pros::Distance distance_back_l(13); // removed sensor +inline pros::Distance distance_front_l(9); +inline pros::Distance distance_back(3); +inline pros::Distance distance_match_loader(21); +inline pros::Distance distance_back_r(99); // removed sensor +inline pros::Distance distance_front_r(99); // removed sensor +inline pros::Distance distance_front(99); // removed sensor -inline pros::Optical color_sort(15); +inline pros::Optical color_sort(16); -inline pros::Motor intake_bottom(20); -inline pros::Motor intake_top(11); +inline pros::Motor intake_bottom(17); +inline pros::Motor intake_top(-2); +inline pros::Motor intake_top_score(-10); +inline pros::Motor vertical_tracker('Z'); -inline ez::Piston trapdoor('G'); -inline ez::Piston middle_stage('C'); -inline ez::Piston Little_Mech_Mac('B'); -inline ez::Piston color_sort_piston('H'); +inline ez::Piston trapdoor('B'); +inline ez::Piston middle_stage('C'); // removed mech +inline ez::Piston Little_Mech_Mac('H'); +inline ez::Piston color_sort_piston('D'); // removed mech +inline ez::Piston intake_piston('C'); -inline ez::Piston right_rush_mech('F'); -inline ez::Piston left_rush_mech('F'); +inline ez::Piston mid_descore('G'); + +inline ez::Piston right_rush_mech('F'); // removed mech +inline ez::Piston left_rush_mech('B'); // removed mech inline ez::Piston discore_mech('A'); inline pros::Distance intake_distance(16); -inline pros::Rotation vertical_tracker(12); +//inline pros::Rotation vertical_tracker(12); diff --git a/include/wall_tracking.hpp b/include/wall_tracking.hpp index d4623c6..3be8581 100644 --- a/include/wall_tracking.hpp +++ b/include/wall_tracking.hpp @@ -19,4 +19,7 @@ void wall_tracking(float target_distance, float DRIVE_SPEED, float drive_distanc void wall_riding(float target_distance, float DRIVE_SPEED, float drive_distance); void wall_tracking_with_alignment(float target_distance, float DRIVE_SPEED, float drive_distance); void wall_tracking_with_alignment_B(float target_distance, float DRIVE_SPEED, float drive_distance); -void drive_wall(float distance); \ No newline at end of file +void drive_wall(float distance, float DRIVE_SPEED); +void chassis_drive_wall(float distance, float DRIVE_SPEED, bool chain, bool back_sensor); +void chassis_brake(); +void drive_wall_task(); \ No newline at end of file diff --git a/project.pros b/project.pros index 039febe..61b9630 100644 --- a/project.pros +++ b/project.pros @@ -1,7 +1,7 @@ { "py/object": "pros.conductor.project.Project", "py/state": { - "project_name": "BeSt PrOG evEr", + "project_name": "2550R TMB", "target": "v5", "templates": { "EZ-Template": { @@ -424,8 +424,8 @@ } }, "upload_options": { - "description": "roboticsisez.com", - "icon": "X", + "description": "2550R", + "icon": "power", "slot": 1 }, "use_early_access": false diff --git a/src/autons.cpp b/src/autons.cpp index 556b1a6..315cce9 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -38,16 +38,14 @@ int screen = 0; void controller_update() { while (true) { if (screen == 0) { - master.print(0, 0, "%d ", (vertical_tracker.get_position()/100 * 2 * 3.14 * 3.25)/*, intake_bottom.get_current_draw(),distance_front.get_distance()*/); + master.print(0, 0, "%f ", (inertial.get_heading())/*, intake_bottom.get_current_draw(),distance_front.get_distance()*/); pros::delay(100); } else { master.print(0, 0, "%f/%f ", p_x, p_y); pros::delay(100); } - pros::delay(100); } - std::cout << "Controller output running\n"; } bool intake_auto_reverse_enabled = true; void intake_counter_spin(){ @@ -64,9 +62,11 @@ void intake_counter_spin(){ } -int top_stage_intake = 0; -int bottom_stage_intake = 0; +int top_speed_intake = 0; +int top_speed_score_intake = 0; +int bottom_speed_intake = 0; bool change = false; + void anti_jam_auton(){ float spin_time = 200; int v_threshold_top; @@ -74,49 +74,84 @@ void anti_jam_auton(){ int current_top; int velocity_bottom; int current_bottom; - int current_threshold = 2300; - bool is_jammed_fwd = false; - bool is_jammed_bcwd = false; + int current_threshold = 1600; + bool is_jammed_top = false; + bool is_jammed_bottom = false; while(true){ - if (change){pros::delay(300); change = false;} - velocity_top = intake_top.get_actual_velocity(); velocity_bottom = intake_bottom.get_actual_velocity(); - current_top = intake_top.get_current_draw(); + velocity_top = intake_top.get_actual_velocity(); + if (change){pros::delay(300); change = false;} current_bottom = intake_bottom.get_current_draw(); - if (top_stage_intake <= 0 || bottom_stage_intake <= 0){ - if ((current_top > current_threshold && velocity_top > -10) || (current_bottom > current_threshold && velocity_top > -10) ){ - is_jammed_fwd = true; - } + current_top = intake_top.get_current_draw(); + + if (current_bottom > current_threshold && abs(velocity_bottom) < 10){ + is_jammed_bottom = true; } - if (top_stage_intake > 0 || bottom_stage_intake > 0){ - if ((current_top > current_threshold && velocity_top < 10) || (current_bottom > current_threshold && velocity_bottom < 10) ){ - is_jammed_bcwd = true; - } + + if (current_top > current_threshold && abs(velocity_top) < 10){ + is_jammed_top = true; } - if (is_jammed_fwd){ + //This one chacks the top motor + if (is_jammed_top){ float start_time = pros::millis(); while (intake_distance.get_distance() > 150 && ((float)pros::millis() - start_time) < spin_time){ - intake_top.move(127); - intake_bottom.move(127); + intake_top.move(-top_speed_intake*127); } - is_jammed_fwd = false; - intake_top.move(top_stage_intake); - intake_bottom.move(bottom_stage_intake); + is_jammed_top = false; + intake_top.move(bottom_speed_intake); pros::delay(300); } - if (is_jammed_bcwd){ + + // This one checks the bottom motor + if (is_jammed_bottom){ float start_time = pros::millis(); - while (intake_distance.get_distance() > 150 && ((float)pros::millis() - start_time) < spin_time){ - intake_top.move(-127); - intake_bottom.move(-127); + while ( ((float)pros::millis() - start_time) < spin_time){ + intake_bottom.move(-bottom_speed_intake*127); } - is_jammed_bcwd = false; - intake_top.move(top_stage_intake); - intake_bottom.move(bottom_stage_intake); + is_jammed_bottom = false; + intake_bottom.move(top_speed_intake); + pros::delay(300); + } + + } +} + + +int auton_count_color = 0; +void color_sort_top_auton() { + color_sort.set_integration_time(10); + while (true) { + int hue_lower; + int hue_higher; + color_sort.set_led_pwm(100); + if (color == "B") { + hue_lower = 210; + hue_higher = 240; + } else if (color == "R") { + hue_lower = 0; + hue_higher = 10; + } else { + continue; + } + + bool in_proximity = color_sort.get_proximity() > 220; + + if (in_proximity && ((hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher) || (color == "R" && color_sort.get_hue() > 340))){ + auton_count_color ++; + } else { + auton_count_color = 0; + } + + if (auton_count_color >= 6) { + pros::delay(50); + intake_top_score.move(-127); + intake_top.move(20); pros::delay(300); + intake_top.move(top_speed_intake); + intake_top_score.move(top_speed_score_intake); } - pros::delay(40); + } } @@ -137,24 +172,31 @@ void timer(int timeout){ void top_intake(int speed_top){ change = true; intake_top.move(speed_top); - top_stage_intake = speed_top; + top_speed_intake = speed_top; } + +void top_intake_score(int speed_top){ + change = true; + intake_top_score.move(speed_top); + top_speed_score_intake = speed_top; +} + void bottom_intake(int speed_bt){ change = true; intake_bottom.move(speed_bt); - bottom_stage_intake = speed_bt; + bottom_speed_intake = speed_bt; } - void default_constants() { - chassis.pid_drive_constants_set(22, 0, 130); + chassis.pid_drive_constants_set(22, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.2, 0, 22, 12.0); // 3.2 0 22 12.0 chassis.pid_heading_constants_set(11.0, 0.0, 20.0); - chassis.pid_turn_constants_set(4, 0.05, 30, 15.0); + // large distance turn pid: chassis.pid_turn_constants_set(4.3, 0, 37, 15.0); chassis.pid_swing_constants_set(6.0, 0.0, 65.0); chassis.pid_odom_angular_constants_set(2.4,0.0,34); chassis.pid_odom_boomerang_constants_set(1.5, 0.0, 35); - chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms,500_ms); + chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); chassis.pid_odom_turn_exit_condition_set(90_ms, 1_deg, 250_ms, 3_deg, 500_ms, 750_ms); @@ -177,1080 +219,1218 @@ void default_constants() { chassis.drive_imu_scaler_set(1); } -void right_elims_quick(){ - chassis.odom_xyt_set(0_in, 0_in, 30_deg); - bottom_intake(127); - trapdoor.set(true); - chassis.pid_drive_set(30_in, 100); - pros::delay(450); - Little_Mech_Mac.set(true); - chassis.pid_wait(); +///////////////////////////////////////////////////////// +// AUTONS // +//////////////////////////////////////////////////////// - chassis.pid_turn_set(135, 60); - chassis.pid_wait(); +// unfinished +void top_middle_bottom(){ + chassis.odom_xyt_set(0_in, 0_in, -90_deg); + trapdoor.set(1); - chassis.pid_drive_set(30, 100); - chassis.pid_wait(); - Little_Mech_Mac.set(true); - chassis.pid_turn_set(180, 60); - chassis.pid_wait(); - top_intake(127); - chassis.pid_drive_set(24, 60); - chassis.pid_wait(); - pros::delay(100); - chassis.pid_drive_set(-32, 100); - chassis.pid_wait(); - trapdoor.set(false); -} -// FINISHED -void left_safe(){ - pros::Task contor1 (controller_update); - chassis.odom_xyt_set(0_in, 0_in, -30_deg); + pros::Task color_sor(color_sort_top_auton); + trapdoor.set(1); + chassis.pid_drive_set(21, 90, true); + chassis.pid_wait_quick_chain(); - bottom_intake(127); - trapdoor.set(false); - chassis.pid_drive_set(30, 100, true); + chassis.pid_turn_set(180, 80, true); pros::delay(500); - Little_Mech_Mac.set(true); - chassis.pid_wait(); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-4, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(12.3, 70, true); + Little_Mech_Mac.set(1); + bottom_intake(127); + intake_top.move(127); + intake_top_score.move(127); + chassis.pid_wait_quick_chain(); - intake_bottom.move(-30); - intake_top.move(-30); + chassis.pid_drive_set(1000, 60); + pros::delay(200); - chassis.pid_turn_set(-135, 60, true); - chassis.pid_wait(); - - intake_bottom.move(0); - intake_top.move(0); - pros::delay(100); - trapdoor.set(true); - chassis.pid_drive_set(-16.5, 80, true); + chassis.pid_drive_set(-28.8, 75, true); chassis.pid_wait(); + trapdoor.set(0); - intake_top.move(127); - intake_bottom.move(127); - pros::delay(900); - trapdoor.set(false); - chassis.pid_drive_set(15, 80, true); - chassis.pid_wait(); - middle_stage.set(0); + pros::delay(1000); + trapdoor.set(1); - chassis.pid_turn_set(179, 80, true); - chassis.pid_wait(); + //Scoring on a high goal + pros::delay(1100); + trapdoor.set(1); + Little_Mech_Mac.set(0); - Little_Mech_Mac.set(false); - //drive_wall(450); - while (distance_front.get_distance() > 705){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + //Getting the 3 balls in the middle + // chassis.pid_drive_set(13.5, 127, true); + // chassis.pid_wait_quick_chain(); + // trapdoor.set(1); - pros::delay(100); + // chassis.pid_turn_set(45, 80, true); - chassis.pid_turn_set(-91, 60, true); - chassis.pid_wait(); - pros::delay(300); - //drive_wall(450); - while (distance_front.get_distance() > 700){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + // chassis.pid_wait_quick_chain(); - pros::delay(100); + + chassis.pid_turn_set(-88, 100, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(16, 100, true); + chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(1); + //Intaking the balls + // chassis.pid_drive_set(35, 100, true); + // chassis.pid_wait(); + // pros::delay(300); - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait(); + chassis.pid_turn_set(-135, 80, true); + chassis.pid_wait_quick_chain(); - intake_bottom.move(127); + chassis.pid_drive_set(-18, 80, true); + chassis.pid_wait_quick(); + trapdoor.set(0); - chassis.pid_drive_set(13.5, 60, true); - chassis.pid_wait(); + //Scoring the balls + intake_top.move(127); + intake_top_score.move(-60); + pros::delay(600); + intake_top_score.move(0); + intake_top.move(-40); - pros::delay(300); + //Grabbing 3 more balls + chassis.pid_drive_set(16, 100, true); + chassis.pid_wait_quick(); + trapdoor.set(0); - chassis.pid_drive_set(-35, 80, true); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(42, 100, true); chassis.pid_wait(); + pros::delay(200); - trapdoor.set(1); + chassis.pid_turn_set(-45, 80, true); + chassis.pid_wait_quick_chain(); - intake_top.move(127); + //Scoring the 3 balls + intake_bottom.move(-70); + chassis.pid_drive_set(15, 80, true); + chassis.pid_wait_quick(); + chassis.pid_drive_set(-1, 30, true); + pros::delay(3000); } -// FINISHED -void left_elims_quick(){ - chassis.odom_xyt_set(0_in, 0_in, -30_deg); +// finished +void left_4_3_push(){ + discore_mech.set(0); + trapdoor.set(1); - bottom_intake(127); - trapdoor.set(false); - chassis.pid_drive_set(30, 100, true); - pros::delay(550); - Little_Mech_Mac.set(true); - chassis.pid_wait(); + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); - chassis.pid_turn_set(-135, 60, true); - chassis.pid_wait(); + chassis.pid_swing_constants_set(9, 0.0, 90.5); + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - - chassis.pid_drive_set(30, 100); - chassis.pid_wait(); - Little_Mech_Mac.set(true); - chassis.pid_turn_set(180, 60); - chassis.pid_wait(); - top_intake(127); - chassis.pid_drive_set(24, 60); - chassis.pid_wait(); - pros::delay(50); - chassis.pid_drive_set(-34, 100); - chassis.pid_wait(); - trapdoor.set(true); - + chassis.pid_swing_set(RIGHT_SWING, -12, 127, 0, false); + chassis.pid_wait_quick_chain(); - - -} + chassis.pid_drive_set(21.67, 127, true); //24.8 with little bill activation + pros::delay(500); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(-138, 100, true); + chassis.pid_wait_quick_chain(); -/* old -void right_safe1(){ - // get 3 middle balls - //pros::Task controller (controller_update); + chassis.pid_drive_set(24, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_odom_set({{0_in, 14_in}, fwd, 100}, true); - chassis.pid_wait(); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_turn_set(45.9, 100, true); - chassis.pid_wait(); + chassis.pid_turn_set(-180, 100, true); + chassis.pid_wait_quick_chain(); - bottom_intake(127); + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_drive_set(20, 30, true); - chassis.pid_wait(); + chassis.pid_drive_set(11.8, 70, true); + chassis.pid_wait_quick_chain(); - //score 3 in middle goal - chassis.pid_drive_set(-5.6, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(680); - // chassis.pid_odom_set({{-9.9_in, 26_in}, fwd, 60}, true); - // pros::delay(100); - // chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(11, 0, 175, 10.0); + chassis.pid_drive_set(-27, 80, true); + pros::delay(450); + Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(22, 0, 150); - chassis.pid_turn_set(-45, 80); - intake_bottom.move(-55); - chassis.pid_wait(); + trapdoor.set(0); + pros::delay(700); + trapdoor.set(1); + intake_top.move(-60); + intake_top_score.move(-127); + intake_bottom.move(-60); - chassis.pid_drive_set(15, 80); - chassis.pid_wait(); + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); - intake_bottom.move(-70); - pros::delay(900); - intake_top.move(0); - intake_bottom.move(0); + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); - // line up for match loader + discore_mech.set(1); - chassis.pid_drive_set(-10, 80, true); - chassis.pid_wait(); + chassis.pid_turn_set(-130, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_swing_set(LEFT_SWING, 180, 90, 22, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-12, 90, true); + chassis.pid_wait_quick(); - middle_stage.set(0); Little_Mech_Mac.set(0); - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait(); + chassis.pid_drive_set(10, 80, true); + chassis.pid_wait_quick_chain(); - while (distance_front.get_distance() > 655){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + discore_mech.set(0); - pros::delay(100); + chassis_drive_wall(810, 80, false, false); - chassis.pid_turn_set(91, 60, true); - chassis.pid_wait(); + chassis.pid_turn_set(-135, 60, true); + chassis.pid_wait_quick_chain(); - while (distance_front.get_distance() > 690){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + chassis.pid_drive_set(-35, 80, true); + chassis.pid_wait_quick(); - pros::delay(50); + chassis.pid_drive_set(2, 1, true); - Little_Mech_Mac.set(1); + intake_bottom.move(-80); + intake_top.move(-80); + intake_top_score.move(-80); - chassis.pid_turn_set(-179, 80, true); - chassis.pid_wait(); + pros::delay(200); intake_bottom.move(127); + intake_top.move(50); + intake_top_score.move(-30); + + int waittime = pros::millis(); + + while (pros::millis() - waittime <= 2800) { + if(color == "R"){ + if (color_sort.get_hue() > 0 && color_sort.get_hue() < 10){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + pros::delay(100); + break; + } + } + if(color == "B"){ + if (color_sort.get_hue() > 210 && color_sort.get_hue() < 240){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + pros::delay(100); + break; + } + } + } - chassis.pid_drive_set(13.5, 60, true); - chassis.pid_wait(); + mid_descore.set(1); - pros::delay(150); + chassis.pid_drive_set(4.3, 80, true); + chassis.pid_wait_quick(); +} - chassis.pid_drive_set(-33, 60, true); - chassis.pid_wait(); - intake_top.move(127); - pros::delay(900); +// finished +void left_7_mid(){ - // chassis.pid_drive_set(8, 80, true); - // chassis.pid_wait_quick(); + discore_mech.set(0); + trapdoor.set(1); - // chassis.pid_drive_set(-15, 127, false); -} -*/ + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); + chassis.pid_swing_constants_set(9, 0.0, 90.5); + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 -void right_safe(){ - pros::Task contor1 (controller_update); - chassis.odom_xyt_set(0_in, 0_in, 30_deg); + chassis.pid_swing_set(RIGHT_SWING, -12, 127, 0, false); + chassis.pid_wait_quick_chain(); - bottom_intake(127); - trapdoor.set(false); - chassis.pid_drive_set(30, 100, true); + chassis.pid_drive_set(21.67, 127, true); //24.8 with little bill activation pros::delay(500); - Little_Mech_Mac.set(true); - chassis.pid_wait(); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-4, 60, true); - chassis.pid_wait(); + chassis.pid_turn_set(-138, 100, true); + chassis.pid_wait_quick_chain(); - intake_bottom.move(-30); - intake_top.move(-30); + chassis.pid_drive_set(23, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-45, 60, true); - chassis.pid_wait(); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 - Little_Mech_Mac.set(false); + chassis.pid_turn_set(-180, 100, true); + chassis.pid_wait_quick_chain(); - intake_bottom.move(0); - intake_top.move(0); - pros::delay(200); - trapdoor.set(true); - chassis.pid_drive_set(15.5, 80, true); - chassis.pid_wait(); + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - intake_top.move(-127); - intake_bottom.move(-80); - pros::delay(1300); - trapdoor.set(false); - chassis.pid_drive_set(-15, 80, true); - chassis.pid_wait(); - middle_stage.set(0); + chassis.pid_drive_set(11.8, 70, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-179, 80, true); - chassis.pid_wait(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(680); + chassis.pid_drive_set(-8, 40, true); + chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(false); - //drive_wall(450); - while (distance_front.get_distance() > 705){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + Little_Mech_Mac.set(0); - pros::delay(100); + chassis.pid_turn_set(-116.5, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(91, 60, true); - chassis.pid_wait(); - pros::delay(300); - //drive_wall(450); - while (distance_front.get_distance() > 730){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + chassis.pid_swing_set(LEFT_SWING, 175, 90, 20, true); + chassis.pid_wait_quick_chain(); - pros::delay(100); - - Little_Mech_Mac.set(1); + chassis.pid_drive_set(-18, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 80, true); + chassis.pid_turn_set(-180, 60, true); chassis.pid_wait(); - intake_bottom.move(127); + pros::delay(1500); - chassis.pid_drive_set(13.5, 60, true); - chassis.pid_wait(); + discore_mech.set(1); - pros::delay(150); + chassis.pid_drive_set(19, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(-135, 60, true); + chassis.pid_wait_quick_chain(); chassis.pid_drive_set(-35, 80, true); - chassis.pid_wait(); + chassis.pid_wait_quick(); - intake_top.move(127); + chassis.pid_drive_set(2, 1, true); -} + intake_bottom.move(-80); + intake_top.move(-80); + intake_top_score.move(-80); + + pros::delay(200); + + intake_bottom.move(127); + intake_top.move(50); + intake_top_score.move(-30); + + int waittime = pros::millis(); + + while (pros::millis() - waittime <= 2800) { + if(color == "R"){ + if (color_sort.get_hue() > 0 && color_sort.get_hue() < 10){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + pros::delay(100); + break; + } + } + if(color == "B"){ + if (color_sort.get_hue() > 210 && color_sort.get_hue() < 240){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + pros::delay(100); + break; + } + } + } + mid_descore.set(1); -// needs updated + chassis.pid_drive_set(4.3, 80, true); + chassis.pid_wait_quick(); -void solo_left() { - chassis.odom_xyt_set(0_in, 0_in, -90_deg); - // trapdoor.set(1); - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 300_ms); - chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 100_ms, 400_ms); - - // while (distance_front.get_distance() > 712){ - // chassis.pid_drive_set(1000000, 50); - // } - // L1.brake(); - // L2.brake(); - // L3.brake(); - // R1.brake(); - // R2.brake(); - // R3.brake(); + +} + +// finished +void left_elims_7ball(){ + discore_mech.set(0); + trapdoor.set(1); + + intake_top.move(127); + intake_top_score.move(127); intake_bottom.move(127); - chassis.pid_drive_set(37.5, 80, true); - chassis.pid_wait(); + + chassis.pid_swing_constants_set(9, 0.0, 90.5); + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 + + chassis.pid_swing_set(RIGHT_SWING, -12, 127, 0, false); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(21.67, 127, true); //24.8 with little bill activation + pros::delay(500); Little_Mech_Mac.set(1); - pros::delay(50); - chassis.pid_turn_set(180, 110, true); - chassis.pid_wait_quick(); + chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(-138, 100, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(24.5, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(13, 90, true); - chassis.pid_wait_quick(); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); - chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms,500_ms); + chassis.pid_turn_set(-180, 100, true); + chassis.pid_wait_quick_chain(); - intake_bottom.move(127); - chassis.pid_drive_set(13, 60, true); - pros::delay(140); + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 + + chassis.pid_drive_set(11.8, 70, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(1000, 40, true); + pros::delay(660); + + chassis.pid_drive_constants_set(11, 0, 175, 10.0); + chassis.pid_drive_set(-27, 80, true); + pros::delay(450); + Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(22, 0, 150); + + trapdoor.set(0); - // chassis.pid_turn_set(180, 80, true); - // chassis.pid_wait_quick(); + int waittime = pros::millis(); - chassis.pid_drive_set(-32, 100, true); - // pros::delay(400); - // intake_top.move(127); - chassis.pid_wait_quick(); - //chassis.drive_brake_set(MOTOR_BRAKE_COAST); - chassis.pid_drive_set(1, 5, true); + while (pros::millis() - waittime <= 2000) { + if(color == "R"){ + if (color_sort.get_hue() > 0 && color_sort.get_hue() < 10){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + pros::delay(100); + break; + } + } + if(color == "B"){ + if (color_sort.get_hue() > 210 && color_sort.get_hue() < 240){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + pros::delay(100); + break; + } + } + } trapdoor.set(1); - intake_top.move(127); - pros::delay(1600); - chassis.drive_brake_set(MOTOR_BRAKE_HOLD); +} - Little_Mech_Mac.set(0); +// unfinished +void left_elims_quick_ml(){ - //intake_top.brake(); + chassis.odom_xyt_set(0_in, 0_in, -90_deg); + pros::Task color_sor(color_sort_top_auton); + trapdoor.set(1); + chassis.pid_drive_set(22.6, 90, true); + chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(180, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(4, 100, true); + chassis.pid_drive_set(12.4, 70, true); + Little_Mech_Mac.set(1); + bottom_intake(127); + intake_top.move(127); + intake_top_score.move(127); chassis.pid_wait(); - chassis.pid_swing_set(RIGHT_SWING, 46.2, 74, -18.5, true); + pros::delay(200); + + chassis.pid_drive_set(-27.3, 75, true); chassis.pid_wait(); trapdoor.set(0); - chassis.pid_drive_set(35, 70, true); - //pros::delay(700); - chassis.pid_wait(); - intake_top.move(127); - chassis.pid_turn_set(-130, 100, true); - chassis.pid_wait_quick(); - - chassis.pid_drive_set(-8, 100, true); - pros::delay(200); + pros::delay(1200); trapdoor.set(1); - chassis.pid_wait_quick(); - + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); - intake_bottom.move(127); - intake_top.move(127); - pros::delay(150); + chassis.pid_turn_set(-130, 80, true); + chassis.pid_wait_quick_chain(); - trapdoor.set(0); - /* - chassis.pid_drive_set(15, 80, true); + chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-20, 80, true); chassis.pid_wait_quick(); - intake_top.move(-127); + chassis.pid_turn_set(-145, 60, true); + chassis.pid_wait(); + - middle_stage.set(0); - Little_Mech_Mac.set(0); + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); +} - chassis.pid_turn_set(95, 100, true); - chassis.pid_wait(); - */ - - chassis.pid_swing_set(RIGHT_SWING, 92, 110, -24, true); - chassis.pid_wait(); - intake_top.brake(); +// finished +void left_elims_quick(){ + discore_mech.set(0); + trapdoor.set(1); - chassis.pid_drive_set(33, 90, true); - chassis.pid_wait_quick(); + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); - chassis.pid_turn_set(-43,80, false); - chassis.pid_wait_quick(); - intake_bottom.move(-127); - chassis.pid_drive_set(8.5, 90, true); - chassis.pid_wait_quick(); + chassis.pid_swing_constants_set(9, 0.0, 90.5); + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 + chassis.pid_swing_exit_condition_set(0_ms, 0.1_deg, 250_ms, 7_deg, 500_ms, 200_ms); + chassis.pid_swing_set(RIGHT_SWING, -12, 127, 0, false); + chassis.pid_wait_quick_chain(); + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); + chassis.pid_drive_set(23.67, 127, false); //24.8 with little bill activation + pros::delay(500); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - // intake_bottom.set_brake_mode(MOTOR_BRAKE_COAST); + chassis.pid_turn_set(59, 127, true); + pros::delay(200); Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); - // chassis.pid_swing_set(LEFT_SWING, -37, 70*1.45, 52*1.15, false); - // chassis.pid_wait_quick(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 50_ms, 50_ms); + chassis.pid_drive_constants_set(25, 0, 150); // 22 0 150 - // chassis.pid_drive_set(-3, 60, true); + chassis.pid_drive_set(-12.5, 127, true); + chassis.pid_wait_quick_chain(); - pros::delay(100); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 -} + chassis.pid_swing_constants_set(12, 0.0, 90); + chassis.pid_swing_set(RIGHT_SWING, 180, -127, 0, true); + pros::delay(500); + trapdoor.set(0); + chassis.pid_wait_quick_chain(); -// needs updated + chassis.pid_drive_set(-2, 127, true); + pros::delay(320); -void solo_right (){ - chassis.odom_xyt_set(0_in, 0_in, 90_deg); - trapdoor.set(1); + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 300_ms); - chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 100_ms, 400_ms); + discore_mech.set(1); - while (distance_front.get_distance() > 645){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + chassis.pid_turn_set(-131, 80, true); + chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(1); + chassis.pid_swing_exit_condition_set(0_ms, 4_deg, 250_ms, 7_deg, 500_ms, 100_ms); + chassis.pid_swing_constants_set(9, 0.0, 90.5); + chassis.pid_swing_set(LEFT_SWING, 180, 127, 22, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 110, true); - chassis.pid_wait(); + chassis.pid_drive_set(-20, 80, true); - chassis.pid_drive_set(11, 80, true); - chassis.pid_wait(); - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); - chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms,500_ms); + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); +/* +working but a little slow + discore_mech.set(0); + trapdoor.set(1); - intake_bottom.move(127); intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); + chassis.pid_swing_constants_set(9, 0.0, 90.5); + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 + chassis.pid_swing_set(RIGHT_SWING, -12, 127, 0, false); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(21.67, 127, true); //24.8 with little bill activation pros::delay(500); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(0); + chassis.pid_turn_set(59, 127, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 60, true); - chassis.pid_wait_quick(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 50_ms, 50_ms); - chassis.pid_drive_set(-30, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(-10.5, 127, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); + chassis.pid_swing_set(RIGHT_SWING, 180, -127, 0, true); + pros::delay(800); trapdoor.set(0); - chassis.pid_drive_set(-1000, 60, true); - - pros::delay(1600); - - intake_top.brake(); - intake_bottom.move(127); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, -55, 90, -17, true); - chassis.pid_wait(); + chassis.pid_drive_set(-2, 127, true); + pros::delay(190); - chassis.pid_drive_set(32, 45, true); - chassis.pid_wait(); + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); - trapdoor.set(1); + discore_mech.set(1); - intake_top.move(-20); - intake_bottom.move(-20); + chassis.pid_turn_set(-130, 80, true); + chassis.pid_wait_quick_chain(); - intake_top.set_brake_mode(MOTOR_BRAKE_HOLD); - intake_bottom.set_brake_mode(MOTOR_BRAKE_HOLD); + chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); + chassis.pid_wait_quick_chain(); - intake_top.move(0); - intake_bottom.brake(); + chassis.pid_drive_set(-20, 90, true); - chassis.pid_drive_set(5.7, 60, true); - chassis.pid_wait(); - - intake_bottom.move(-90); - intake_top.move(-90); - pros::delay(300); - intake_bottom.move(-60); - intake_top.move(-60); - pros::delay(900); - // chassis.pid_drive_set(-12, 50, true); - // chassis.pid_wait(); + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); + */ } -/* old -void left_elims() { - pros::Task anti (anti_jam_auton); +// unfinished +void right_4_3_push(){ + +discore_mech.set(0); trapdoor.set(1); - // intake_top.move(127); - top_intake(127); - // chassis.odom_xyt_set(0_in, 0_in, 152.5_deg); - chassis.pid_drive_constants_set(27, 0, 135); - chassis.pid_drive_exit_condition_set(50_ms, 1_in, 100_ms, 3_in, 200_ms, 200_ms); - chassis.pid_odom_set({{{-10.6_in, 18_in}, rev, 127}, - {{-12_in, 22_in}, rev, 127}, - {{-13_in, 27_in}, rev, 127}, - {{-16.2_in, 42.5_in}, rev, 127}}, - true); - pros::delay(900); - right_rush_mech.set(1); - chassis.pid_wait_quick(); - chassis.pid_drive_constants_set(22, 0, 130); + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); - // intake_bottom.move(127); - bottom_intake(127); + chassis.pid_swing_constants_set(9, 0.0, 90.5); + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_swing_set(RIGHT_SWING, 115, 75, 15); + chassis.pid_swing_set(LEFT_SWING, 13, 127, 0, false); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(25, 60, true); - chassis.pid_wait_quick(); - - //Turn that on when we have the pump - pros::delay(300); - right_rush_mech.set(0); - pros::delay(200); - + chassis.pid_drive_set(21.67, 127, true); //24.8 with little bill activation + pros::delay(500); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-15, -40, true); - chassis.pid_wait(); + chassis.pid_turn_set(138, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(140, 60, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(24, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(16, 60, true); - chassis.pid_wait_quick(); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_turn_set(-98 , 60); - chassis.pid_wait(); + chassis.pid_turn_set(-180, 100, true); + chassis.pid_wait_quick_chain(); - while (distance_front.get_distance() > 655){ - chassis.pid_drive_set(1000000, 50); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 + chassis.pid_drive_set(11.8, 70, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(174, 60, true); - chassis.pid_wait_quick(); - //wall_alignment_R(1000); + chassis.pid_drive_set(1000, 40, true); + pros::delay(680); - // blooper - Little_Mech_Mac.set(1); - pros::delay(100); + chassis.pid_drive_constants_set(11, 0, 175, 10.0); + chassis.pid_drive_set(-27, 80, true); + pros::delay(450); + Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(22, 0, 150); - chassis.pid_drive_set(25, 55, true); - chassis.pid_wait(); - // intake_bottom.move(127); - bottom_intake(127); + Little_Mech_Mac.set(0); + trapdoor.set(0); pros::delay(600); + trapdoor.set(1); - top_intake(127); - Little_Mech_Mac.set(1); - // intake_top.move(127); - - //intake_top.move(127); - - chassis.pid_drive_set(-27.2, 70, true); - chassis.pid_wait(); - - trapdoor.set(0); + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-5, 20, true); -} -*/ -void skills_without_odom(){ - chassis.odom_xyt_set(0_in, 0_in, -90_deg); - trapdoor.set(1); - color = "x"; + chassis.pid_turn_set(-60, 80, true); + chassis.pid_wait_quick_chain(); - bottom_intake(127); - top_intake(127); - //Instead of while 735 - // chassis.pid_turn_set(0,60); - // chassis.pid_wait(); + chassis.pid_drive_set(30, 90, true); + chassis.pid_wait_quick(); - // chassis.pid_turn_set(-90,60); - // chassis.pid_wait(); + intake_piston.set(1); - chassis.pid_drive_set(-10,60); - chassis.pid_wait(); + chassis.pid_drive_set(12, 80, true); + pros::delay(400); + intake_top.move(-60); + intake_top_score.move(-127); + intake_bottom.move(-60); + chassis.pid_wait_quick_chain(); - drive_wall(500); + pros::delay(2000); - /* - Empty first match loader - */ - Little_Mech_Mac.set(1); + /////////////////////////////// - chassis.pid_turn_set(180, 110, true); + chassis.pid_drive_set(-34, 127, true); chassis.pid_wait(); - chassis.pid_drive_set(11.5, 90, true); + chassis.pid_turn_set(180, 80, true); + chassis.pid_wait(); + + chassis.pid_drive_set(22, 127, true); chassis.pid_wait(); + + chassis.pid_turn_set(-135, 80, true); + chassis.pid_wait(); +} - //intake_bottom.move(127); - //intake_top.move(127); +// finished +void right_elims_7ball(){ + discore_mech.set(0); + trapdoor.set(1); - pros::delay(1350); + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); - /* - Back up from match loader - */ + chassis.pid_swing_constants_set(9, 0.0, 90.5); + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - Little_Mech_Mac.set(0); - pros::delay(100); + chassis.pid_swing_set(LEFT_SWING, 13, 127, 0, false); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-9.8, 90, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(21.67, 127, true); //24.8 with little bill activation + pros::delay(500); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - bottom_intake(0); - top_intake(0); + chassis.pid_turn_set(138, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-90, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(24.5, 100, true); + chassis.pid_wait_quick_chain(); - bottom_intake(0); - top_intake(0); - drive_wall(200); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait(); + chassis.pid_turn_set(-180, 100, true); + chassis.pid_wait_quick_chain(); - + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 + chassis.pid_drive_set(11.8, 70, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(1000, 40, true); + pros::delay(660); + + chassis.pid_drive_constants_set(11, 0, 175, 10.0); + chassis.pid_drive_set(-27, 80, true); + pros::delay(450); + Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(22, 0, 150); -} -void skills() { - pros::Task task1(controller_update); - //pros::Task color_sort_task_running(color_sort_S); - drive_wall(600); - discore_mech.set(1); - chassis.odom_xyt_set(0_in, 0_in, -90_deg); trapdoor.set(0); - color = "x"; - /* - Setup for first matchload - */ + int waittime = pros::millis(); - // chassis.pid_odom_set({{-34_in, 0_in}, fwd, 127}, true); - // chassis.pid_wait(); - - bottom_intake(127); - top_intake(127); - // while (distance_front.get_distance() > 787){ - // chassis.pid_drive_set(1000000, 50); - // } - // L1.brake(); - // L2.brake(); - // L3.brake(); - // R1.brake(); - // R2.brake(); - // R3.brake(); - chassis.odom_xy_set(-33, 0); - pros::delay(100); - - /* - Empty first match loader - */ - Little_Mech_Mac.set(1); - - chassis.pid_turn_set(180, 110, true); - chassis.pid_wait(); + while (pros::millis() - waittime <= 2000) { + if(color == "R"){ + if (color_sort.get_hue() > 0 && color_sort.get_hue() < 10){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + pros::delay(100); + break; + } + } + if(color == "B"){ + if (color_sort.get_hue() > 210 && color_sort.get_hue() < 240){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + pros::delay(100); + break; + } + } + } + trapdoor.set(1); +} - chassis.pid_drive_set(11.5, 90, true); - chassis.pid_wait(); +// finished +void push_solo(){ - //intake_bottom.move(127); - //intake_top.move(127); - chassis.pid_drive_set(13, 60, true); - pros::delay(1000); + pros::Task color_sor(color_sort_top_auton); - /* - Back up from match loader - */ + chassis.odom_xyt_set(0_in, 0_in, -90_deg); + discore_mech.set(0); - Little_Mech_Mac.set(0); - pros::delay(100); + top_intake(127); + top_intake_score(127); + bottom_intake(127); - chassis.pid_drive_set(-9.8, 90, true); - chassis.pid_wait_quick(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); + chassis.pid_drive_set(3, 127, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); + + //chassis_drive_wall(460, 100, false, true); + + chassis.pid_drive_set(-37.5, 100, true); + chassis.pid_wait_quick_chain(); + - bottom_intake(0); - top_intake(0); + chassis.pid_turn_set(180, 90, true); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-90, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(10, 100); + trapdoor.set(1); + chassis.pid_wait_quick_chain(); - /* - Relocate to blue side - */ + chassis.pid_drive_set(1000, 65, true); + pros::delay(300); - chassis.pid_odom_set({{-45_in, 0_in}, fwd, 100}, true); + chassis.pid_drive_set(-30, 75, true); + pros::delay(650); + trapdoor.set(0); chassis.pid_wait(); + Little_Mech_Mac.set(0); - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait(); - /* - Reset location to 0, 0 - */ + pros::delay(900); - chassis.pid_odom_set({{-46_in, 90_in}, fwd, 110}, true); - chassis.pid_wait(); - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait(); - pros::delay(100); + chassis.pid_turn_set(-88, 100, true); + chassis.pid_wait_quick_chain(); + trapdoor.set(1); + intake_top.move(127); + intake_top_score.move(127); - // Reset odom position with distance sensors - float position_y = (distance_front.get_distance() - 600) / 25.4; - float position_x = ((distance_front_l.get_distance() + distance_back_l.get_distance()) / 2 - 160) / 25.4; + chassis.pid_drive_set(56, 100, true); + chassis.pid_wait_quick_chain(); - p_x = position_x; - p_y = position_y; + chassis.pid_turn_set(-135, 127, true); + chassis.pid_wait_quick_chain(); - chassis.odom_xy_set(position_x, position_y); - pros::delay(150); + chassis.pid_drive_set(16.5, 127, true); + chassis.pid_wait_quick_chain(); - /* - Setup for match loader - */ - - chassis.pid_turn_set(90, 60, true); - chassis.pid_wait(); + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); - // chassis.pid_odom_set({{6.7_in, 0_in}, fwd, 110}, true); - // chassis.pid_wait(); - // screen = 1; - chassis.pid_drive_set(11_in, 100); + chassis.pid_drive_set(-10, 100, true); + pros::delay(500); + trapdoor.set(0); chassis.pid_wait(); - /* - Score first match loader - */ + trapdoor.set(0); + pros::delay(1000); + trapdoor.set(1); + + //Intaking the 2rd machloader + chassis.pid_drive_set(26.4, 100, true); Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(1000, 50); + trapdoor.set(1); + pros::delay(300); - top_intake(-20); - bottom_intake(-20); - //intake_top.move(127); - //intake_bottom.move(127); + chassis.pid_drive_set(-3, 100, true); + chassis.pid_wait_quick_chain(); + Little_Mech_Mac.set(0); - chassis.pid_drive_set(-16, 90, true); - chassis.pid_wait_quick(); + chassis.pid_turn_set(-135, 100, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(-50, 100, true); + top_intake(-50); + top_intake_score(-50); + bottom_intake(-50); + pros::delay(200); top_intake(127); + top_intake_score(127); bottom_intake(127); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-100, 10, true); - trapdoor.set(1); - pros::delay(100); - trapdoor.set(1); - pros::delay(1900); + top_intake_score(-127); + top_intake(80); + bottom_intake(127); + while (true) { + if(color == "R"){ + if (color_sort.get_hue() > 0 && color_sort.get_hue() < 10){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + } + } + if(color == "B"){ + if (color_sort.get_hue() > 210 && color_sort.get_hue() < 240){ + top_intake_score(127); + top_intake(127); + bottom_intake(127); + } + } + } - /* - empty second match loader - */ - chassis.pid_drive_set(45.5, 100, true); - chassis.pid_wait(); + +} - chassis.pid_drive_set(13, 60, true); - chassis.pid_wait(); - pros::delay(1000); - /* - Score second 6 on long goal - */ - trapdoor.set(0); - pros::delay(100); +// mostly finished +void safe_skills(){ + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_drive_set(-29.5, 60, true); - chassis.pid_wait(); - Little_Mech_Mac.set(0); + //grab ball 2 + discore_mech.set(0); + trapdoor.set(1); - top_intake(-15); - bottom_intake(-15); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); - chassis.pid_drive_set(-5, 20, true); - pros::delay(150); + chassis.pid_drive_set(1, 90, true); + chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(-45, 80, true); + chassis.pid_wait_quick_chain(); - top_intake(127); - bottom_intake(127); + chassis.pid_drive_set(26, 40.67, true); + chassis.pid_wait_quick_chain(); - trapdoor.set(1); + chassis.pid_drive_set(-0.1, 80, true); + chassis.pid_wait_quick_chain(); - pros::delay(1000); + //score 2 into mid + chassis.pid_turn_set(-135, 80, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(-16, 60, true); + chassis.pid_wait_quick_chain(); + intake_top_score.move(-32.67); + intake_top.move(-65); + pros::delay(300); + intake_top.move(60); + chassis.pid_drive_set(1, 60, true); - /* - Set up for first 2 middle balls - */ + while (!(color_sort.get_hue() < 250 && color_sort.get_hue() > 160)){ + pros::delay(10); + } + pros::delay(300); - // Activate color sort - + intake_top_score.move(0); + intake_top.move(0); + intake_bottom.move(0); - chassis.pid_drive_set(10, 110, true); - chassis.pid_wait(); - - chassis.pid_turn_set(88, 80, true); - chassis.pid_wait(); - trapdoor.set(0); + //grab match loader + chassis.pid_drive_set(41, 80, true); + pros::delay(300); + intake_bottom.move(80); + intake_top.move(-80); + intake_top_score.move(80); + chassis.pid_wait_quick_chain(); - // chassis.pid_odom_set({{26.8, -10.5}, fwd, 80}, true); - // chassis.pid_wait_quick(); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); - chassis.pid_drive_set(80, 80, true); - chassis.pid_wait(); + chassis.pid_turn_set(180, 80, true); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(1000, 70, true); + pros::delay(300); + chassis.pid_drive_set(1000, 40, true); + pros::delay(850); + chassis.pid_drive_set(-1, 40, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1300); - chassis.pid_turn_set(-1.5, 60); - chassis.pid_wait(); + //rotate to other side - while (distance_front.get_distance() > 730){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - pros::delay(200); + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); - - //drive_wall(450); + Little_Mech_Mac.set(0); - chassis.pid_turn_set(88.5, 60); - chassis.pid_wait(); + chassis.pid_turn_set(-39, 100, true); + chassis.pid_wait_quick_chain(); - while (distance_front.get_distance() > 756){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - pros::delay(200); + chassis.pid_drive_set(10, 100, true); + chassis.pid_wait_quick_chain(); - //drive_wall(450); + chassis.pid_turn_set(1, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-3, 60); - chassis.pid_wait(); + chassis.pid_drive_constants_set(11, 0, 175, 10.0); + chassis.pid_drive_set(59, 100, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(22, 0, 150); - pros::delay(150); + //score match loader + chassis.pid_turn_set(60, 100, true); + chassis.pid_wait_quick_chain(); - /* - reset position - */ + chassis.pid_drive_set(6.2, 100, true); + chassis.pid_wait_quick_chain(); - float position_y_1 = (distance_front.get_distance() - 525) / 25.4; - float position_x_1 = ((distance_front_r.get_distance() + distance_back_r.get_distance()) / 2 - 435) / 25.4; + chassis.pid_turn_set(0, 100, true); + chassis.pid_wait_quick_chain(); - p_x = position_x_1; - p_y = position_y_1; + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 300_ms); + chassis.pid_drive_set(-10.2, 100, true); + pros::delay(500); + trapdoor.set(0); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); - pros::delay(150); + pros::delay(1700); + trapdoor.set(1); + + // grab second match loader + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); - chassis.odom_xy_set(position_x_1, position_y_1); - chassis.pid_wait(); + chassis.pid_drive_set(6, 80, true); + chassis.pid_wait_quick_chain(); - /* - grab third match load - */ + chassis.pid_turn_set(20, 80, true); + chassis.pid_wait_quick_chain(); + chassis.pid_swing_set(RIGHT_SWING, 0, 127, 30, true); Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(24, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1000); + chassis.pid_drive_set(-1, 40, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1500); + + //score long goal + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 300_ms); + chassis.pid_drive_constants_set(11, 0, 175, 10.0); + chassis.pid_drive_set(-30, 80, true); + pros::delay(1500); + trapdoor.set(0); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(22, 0, 150); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); + + trapdoor.set(0); + Little_Mech_Mac.set(0); + pros::delay(1700); + //line up for clear - top_intake(127); - bottom_intake(127); + discore_mech.set(0); - //intake_top.move(127); - //intake_bottom.move(127); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); - chassis.pid_drive_set(13, 60, true); - pros::delay(1000); + chassis.pid_drive_set(7.3, 127, true); + chassis.pid_wait_quick_chain(); + trapdoor.set(1); + intake_top_score.move(-127); - chassis.pid_drive_set(-20, 60, true); - chassis.pid_wait(); + trapdoor.set(1); - Little_Mech_Mac.set(0); - top_intake(0); - bottom_intake(0); + chassis.pid_swing_set(LEFT_SWING, 87, 84, 31, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-88, 60, true); - chassis.pid_wait(); + // chassis.pid_drive_set(5, 127, true); + // chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-11.9, 60, true); + //clear the 6 ball from the park zone + + chassis.pid_drive_set(75, 75, true); + intake_top_score.move(127); chassis.pid_wait(); - /* - go to blue side - */ + chassis_drive_wall(1000, 80, false, false); - chassis.pid_turn_set(-3, 60, true); - chassis.pid_wait_quick(); + //grab ball number 7 + + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-85, 80, true); - chassis.pid_wait(); + chassis_drive_wall(1000, 80, false, false); - chassis.pid_turn_set(86, 60, true); - chassis.pid_wait(); + //score 8 in mid - chassis.pid_drive_set(-11, 80, true); - chassis.pid_wait(); + chassis.pid_turn_set(45, 80, true); + chassis.pid_wait_quick_chain(); - /* - score third set of match load - */ + chassis.pid_drive_set(-20.5, 65, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(176, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(2, 1, true); - chassis.pid_drive_set(-19.5, 60, true); - chassis.pid_wait(); - top_intake(127); - bottom_intake(127); - trapdoor.set(1); + intake_bottom.move(-80); + intake_top.move(-80); + intake_top_score.move(-80); - pros::delay(2000); + pros::delay(200); - /* - empty second match loader - */ + intake_bottom.move(127); + intake_top.move(50); + intake_top_score.move(-30); - - + pros::delay(2100); + + //grab 3rd match loader + + chassis.pid_drive_set(39, 80, true); + pros::delay(300); + intake_bottom.move(80); + intake_top.move(-80); + intake_top_score.move(80); + chassis.pid_wait_quick_chain(); + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_turn_set(0, 80, true); Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(1000, 70, true); + pros::delay(300); + chassis.pid_drive_set(1000, 40, true); + pros::delay(900); + chassis.pid_drive_set(-1, 40, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1300); - chassis.pid_drive_set(45, 80, true); - chassis.pid_wait(); - trapdoor.set(0); - chassis.pid_drive_set(13, 60, true); - chassis.pid_wait(); - pros::delay(1000); + //rotate to other side - /* - Score second 6 on long goal - */ + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); Little_Mech_Mac.set(0); - pros::delay(100); + chassis.pid_turn_set(141, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(179, 60, true); + chassis.pid_drive_set(10, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-33.5, 60, true); - chassis.pid_wait(); + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); - top_intake(127); - bottom_intake(127); + chassis.pid_drive_constants_set(11, 0, 175, 10.0); + chassis.pid_drive_set(59, 100, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(22, 0, 150); + + //score match loader + chassis.pid_turn_set(-120, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(6.2, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 100_ms, 3_in, 100_ms, 300_ms); + chassis.pid_drive_set(-10.2, 100, true); + pros::delay(500); + trapdoor.set(0); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); + + pros::delay(1950); trapdoor.set(1); + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); - pros::delay(2000); + chassis.odom_xyt_set(0_in, 0_in, 180_deg); - chassis.odom_xyt_set(0, 0, 180); + chassis.pid_drive_set(6, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(5, 127, true); - chassis.pid_wait(); + chassis.pid_turn_set(-165, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-95, 127, true); - chassis.pid_wait(); + chassis.pid_swing_set(RIGHT_SWING, 180, 127, 30, true); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(48, 127, true); - chassis.pid_wait(); + chassis.pid_drive_set(1000, 70, true); + pros::delay(300); + chassis.pid_drive_set(1000, 40, true); + pros::delay(700); + chassis.pid_drive_set(-1, 40, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1300); - chassis.pid_turn_set(178, 127, true); - chassis.pid_wait(); + //score long goal + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 300_ms); + chassis.pid_drive_constants_set(11, 0, 175, 10.0); + chassis.pid_drive_set(-29, 80, true); + pros::delay(1500); + trapdoor.set(0); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_constants_set(22, 0, 150); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); - chassis.pid_drive_set(100, 127, false); - chassis.pid_wait_quick(); -} + trapdoor.set(0); + Little_Mech_Mac.set(0); + pros::delay(1700); + //line up for clear + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); -/* old -void new_elim_auton(){ - // pros::Task anti (anti_jam_auton); + chassis.pid_drive_set(7.3, 127, true); + chassis.pid_wait_quick_chain(); trapdoor.set(1); - chassis.pid_drive_set(14.1, 60, true); - chassis.pid_wait_quick(); + intake_top_score.move(-127); - chassis.pid_turn_set(-45.9, 100, true); - chassis.pid_wait(); + trapdoor.set(1); - bottom_intake(127); - top_intake(127); + chassis.odom_xyt_set(0_in, 0_in, 180_deg); + discore_mech.set(0); - chassis.pid_drive_set(20, 30, true); - chassis.pid_wait(); + chassis.pid_swing_set(LEFT_SWING, -93, 84, 31, true); + chassis.pid_wait_quick_chain(); - //score 3 in middle goal - chassis.pid_drive_set(-4, 60, true); - chassis.pid_wait(); + // chassis.pid_drive_set(5, 127, true); + // chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 80, true); + //clear the 6 ball from the park zone + + intake_top_score.move(127); + chassis.pid_drive_set(40, 65, true); chassis.pid_wait(); - while (distance_front.get_distance() > 655){ - chassis.pid_drive_set(1000000, 40); + while (distance_front.get_distance() < 1660){ + chassis.pid_drive_set(1000000, 60); } L1.brake(); L2.brake(); @@ -1259,287 +1439,337 @@ void new_elim_auton(){ R2.brake(); R3.brake(); - pros::delay(50); +} + +//old but worth keeping +void norcal_skills(){ + + // pros::Task anti_jam_auton1(anti_jam_auton); + chassis.odom_xyt_set(0_in, 0_in, -90_deg); + + discore_mech.set(0); + trapdoor.set(1); + intake_piston.set(0); - chassis.pid_turn_set(-91, 60, true); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + // Taking the working 6 balls + chassis.pid_drive_set(2, 20, true); chassis.pid_wait(); - while (distance_front.get_distance() > 725){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + pros::delay(400); - pros::delay(100); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 1000_ms); + chassis.pid_drive_set(50, 65, true); + pros::delay(200); Little_Mech_Mac.set(1); - - chassis.pid_turn_set(179, 80, true); + pros::delay(300); + Little_Mech_Mac.set(0); chassis.pid_wait(); - intake_bottom.move(127); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); - chassis.pid_drive_set(15.5, 60, true); + chassis.pid_drive_set(-10, 100, true); chassis.pid_wait(); - pros::delay(90); - chassis.pid_drive_set(-31, 80, true); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); + + //line up on the wall + + chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms,500_ms); + + chassis.pid_turn_set(180, 80, true); chassis.pid_wait(); - intake_top.move(127); - intake_bottom.move(127); - trapdoor.set(0); + chassis_drive_wall(600, 127, false, false); - chassis.pid_drive_set(-2, 80, true); - chassis.pid_wait_quick(); -} -*/ + chassis.pid_turn_set(-88, 80, true); + chassis.pid_wait(); + chassis_drive_wall(1450, 127, false, false); -/* old -void blue_top_quals() { - // pros::Task anti (anti_jam_auton); - trapdoor.set(1); - intake_top.move(127); - chassis.odom_xyt_set(0_in, 0_in, 152.5_deg); - chassis.pid_drive_constants_set(27, 0, 135); - chassis.pid_drive_exit_condition_set(50_ms, 1_in, 100_ms, 3_in, 200_ms, 200_ms); - chassis.pid_odom_set({{{-10.6_in, 18_in}, rev, 127}, - {{-12_in, 22_in}, rev, 127}, - {{-11.2_in, 27_in}, rev, 127}, - {{-15_in, 42.3_in}, rev, 127}}, - true); - - pros::delay(950); - right_rush_mech.set(1); - chassis.pid_wait_quick(); + chassis.pid_turn_set(180, 80, true); + chassis.pid_wait(); - chassis.pid_drive_constants_set(22, 0, 130); + chassis.pid_drive_set(-26, 80, true); + chassis.pid_wait(); - intake_bottom.move(127); + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 150_ms, 150_ms); - chassis.pid_swing_set(RIGHT_SWING, 80, 60, 17); - chassis.pid_wait_quick(); - chassis.pid_drive_set(10, 60, true); + chassis.pid_swing_set(RIGHT_SWING, -127, 127, -30, true); + pros::delay(500); + top_intake(-80); + top_intake_score(-70); + bottom_intake(-80); + pros::delay(300); + bottom_intake(60); + top_intake(60); + top_intake_score(-40); chassis.pid_wait_quick(); - pros::delay(350); - //Turn that on when we have the pump - right_rush_mech.set(0); - pros::delay(250); - - chassis.pid_drive_set(-14, -80, true); - chassis.pid_wait_quick(); + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - chassis.pid_turn_set(125, 60, true); - intake_bottom.move(80); - intake_top.move(0); - chassis.pid_wait_quick(); + //7 ball score + chassis.pid_drive_set(-1, 1, false); - chassis.pid_swing_set(RIGHT_SWING, 90, 80, 40, true); - chassis.pid_wait_quick(); + pros::delay(1200); + top_intake(-80); + top_intake_score(-70); + bottom_intake(-80); + //grab 7th ball + bottom_intake(127); + top_intake(127); + top_intake_score(0); + chassis.pid_turn_set(-135, 60, true); + chassis.pid_wait(); - intake_bottom.move(0); + chassis.pid_drive_set(7, 80, true); + chassis.pid_wait(); - chassis.pid_odom_set({{14.6, 36.8}, fwd, 100}, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(-5, 30, true); + chassis.pid_wait_quick_chain(); + top_intake(70); + top_intake_score(-30); + pros::delay(1600); - middle_stage.set(1); - Little_Mech_Mac.set(1); + //line up for first match loader - chassis.pid_turn_set(45, 60, true); + chassis.pid_drive_set(50, 80, true); chassis.pid_wait(); - chassis.pid_drive_set(8.5, 60, true); + chassis.pid_turn_set(180, 80, true); + top_intake(127); + bottom_intake(127); + top_intake_score(127); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(10, 80, true); chassis.pid_wait(); + chassis.pid_drive_set(1000, 60, true); + pros::delay(1150); - intake_top.move(127); - intake_bottom.move(127); + //cross to other side + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); - pros::delay(550); + Little_Mech_Mac.set(0); - chassis.pid_odom_set({{-11_in, 8_in}, rev, 100}, true); - chassis.pid_wait_quick(); - - middle_stage.set(0); + chassis.pid_turn_set(-39, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(173, 60, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(8, 100, true); + chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(1); + chassis.pid_turn_set(1, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(13, 60, true); - chassis.pid_wait(); + top_intake(0); + bottom_intake(0); + top_intake_score(0); - pros::delay(250); + chassis.pid_drive_set(52, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-31, 80, true); - chassis.pid_wait(); + //score first match loader - trapdoor.set(0); -} -*/ + chassis.pid_turn_set(60, 100, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(7.8, 100, true); + chassis.pid_wait_quick_chain(); -/* old -void blue_bottom_elims() { - trapdoor.set(1); - intake_top.move(127); - chassis.odom_xyt_set(0_in, 0_in, -152.5_deg); - chassis.pid_drive_constants_set(30, 0, 129); - chassis.pid_drive_exit_condition_set(50_ms, 1_in, 100_ms, 3_in, 200_ms, 200_ms); - chassis.pid_odom_set({{{9_in, 18_in}, rev, 127}, - {{10_in, 22_in}, rev, 127}, - {{11_in, 28_in}, rev, 127}, - {{12.5_in, 43_in}, rev, 127}}, - true); - chassis.pid_wait_quick(); - // right_rush_mech.set(1); - chassis.pid_drive_constants_set(22, 0, 130); - pros::delay(200); + chassis.pid_turn_set(0, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, -105, 60); + chassis.pid_drive_set(-9.8, 100, true); chassis.pid_wait(); + chassis.pid_drive_set(-1, 10, true); + // intake_top.move(-100); + // intake_top_score.move(127); + // intake_bottom.move(127); + // pros::delay(200); + // intake_top_score.move(-60); + // pros::delay(300); + intake_top.move(127); + intake_top_score.move(127); intake_bottom.move(127); + trapdoor.set(0); - chassis.pid_swing_set(RIGHT_SWING, -150, 60, 30); - chassis.pid_wait(); - - chassis.pid_swing_set(LEFT_SWING, -100, 60, 20); + pros::delay(2000); chassis.pid_wait(); - //Turn that on when we have the pump - // right_rush_mech.set(0); - pros::delay(200); - chassis.pid_drive_set(-15, -40, true); - chassis.pid_wait(); + //grab second match loader - chassis.pid_turn_set(-140, -60, true); - chassis.pid_wait_quick(); + Little_Mech_Mac.set(1); - chassis.pid_swing_set(LEFT_SWING, -90, 60, 30, true); - chassis.pid_wait_quick(); + trapdoor.set(1); - chassis.pid_odom_set({{13.5_in, 14_in}, fwd, 80}, true); + chassis.pid_drive_set(27.5, 80, true); chassis.pid_wait(); + chassis.pid_drive_set(1000, 60, true); + pros::delay(1300); - chassis.pid_turn_set(-172, 60, true); - chassis.pid_wait_quick(); - //wall_alignment_R(1000); + //score second match loader + + chassis.pid_drive_set(-29.5, 70, true); + chassis.pid_wait(); - // blooper - Little_Mech_Mac.set(1); + chassis.pid_drive_set(-1, 10, true); + // intake_top.move(-100); + // intake_top_score.move(127); + // pros::delay(200); + // intake_top_score.move(-60); + // pros::delay(300); + intake_top.move(127); + intake_top_score.move(127); + trapdoor.set(0); - chassis.pid_drive_set(18, 50, true); + pros::delay(1500); chassis.pid_wait(); + Little_Mech_Mac.set(0); - pros::delay(500); + // line up for clear + + top_intake(127); + bottom_intake(127); + top_intake_score(127); + discore_mech.set(0); - intake_top.move(127); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); + trapdoor.set(1); - chassis.pid_drive_set(-32, 70, true); - chassis.pid_wait(); + chassis.pid_drive_set(60, 80, true); + chassis.pid_wait_quick_chain(); - trapdoor.set(0); -} -*/ + chassis.pid_turn_set(35, 60, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(16.5, 127, true); + chassis.pid_wait_quick_chain(); -/* old -void blue_bottom_quals() { - trapdoor.set(1); - chassis.odom_xyt_set(0_in, 0_in, -166_deg); + chassis.pid_turn_set(0, 60, true); + chassis.pid_wait_quick_chain(); - chassis.pid_odom_set({{{14_in, 18_in}, rev, 127}, - {{11_in, 22_in}, rev, 127}, - {{10.9_in, 30_in}, rev, 110}, - {{8.3_in, 46.3_in}, rev, 110}}, - true); - chassis.pid_wait(); + chassis.pid_drive_set(-13, 60, true); + chassis.pid_wait_quick_chain(); + trapdoor.set(0); + //Scroing on the + chassis.pid_drive_set(-1, 10, true); + pros::delay(700); - right_rush_mech.set(1); - pros::delay(300); - chassis.pid_swing_set(RIGHT_SWING, -90, 60); - chassis.pid_wait(); + //Intaking the 3rd machloader + Little_Mech_Mac.set(1); - intake_top.move(127); - intake_bottom.move(127); + + ///new stuff - chassis.pid_swing_set(LEFT_SWING, -160, 60); + Little_Mech_Mac.set(1); + chassis.pid_drive_set(28.3, 60, true); chassis.pid_wait(); + trapdoor.set(1); + chassis.pid_drive_set(1000, 60, true); + pros::delay(1650); - chassis.pid_swing_set(RIGHT_SWING, -100, 60, 20); - chassis.pid_wait(); + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); - right_rush_mech.set(0); - pros::delay(350); + Little_Mech_Mac.set(0); - chassis.pid_drive_set(-20, 40, true); - chassis.pid_wait(); + chassis.pid_turn_set(133, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-150, 60, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(7, 80, true); + chassis.pid_wait_quick_chain(); - intake_top.move(127); - intake_bottom.move(50); + chassis.pid_turn_set(-180, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(RIGHT_SWING, -80, 60, 20, true); - chassis.pid_wait(); + top_intake(0); + bottom_intake(0); + top_intake_score(0); - intake_bottom.move(0); + chassis.pid_drive_set(56, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_odom_set({{-16, 41}, fwd, 60}, true); - chassis.pid_wait(); + //score first match loader - chassis.pid_turn_set(-45, 60, true); - chassis.pid_wait(); + chassis.pid_turn_set(-120, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(8, 100, true); + chassis.pid_wait_quick_chain(); - middle_stage.set(1); + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); - pros::delay(1000); - - chassis.pid_drive_set(8.5, 60, true); + chassis.pid_drive_set(-9, 100, true); chassis.pid_wait(); + chassis.pid_drive_set(1, 60, true); + // intake_top.move(-100); + // intake_top_score.move(127); + // intake_bottom.move(127); + // pros::delay(200); + // intake_top_score.move(-60); + // pros::delay(300); intake_top.move(127); + intake_top_score.move(127); intake_bottom.move(127); + trapdoor.set(0); pros::delay(1500); - - chassis.pid_odom_set({{17.5_in, 13_in}, rev, 80}, true); chassis.pid_wait(); - - middle_stage.set(0); - pros::delay(250); + //grab second match loader + Little_Mech_Mac.set(1); - chassis.pid_turn_set(180, 60, true); + trapdoor.set(1); + + chassis.pid_drive_set(27.4, 80, true); chassis.pid_wait(); + chassis.pid_drive_set(1000, 60, true); + pros::delay(1650); + + //score second match loader + + chassis.pid_drive_set(-32.5, 70, true); + chassis.pid_wait(); + top_intake_score(127); + chassis.pid_drive_set(-1, 60, true); + // intake_top.move(-100); + // intake_top_score.move(127); + // pros::delay(200); + // intake_top_score.move(-60); + // pros::delay(300); + intake_top.move(127); + intake_top_score.move(127); + trapdoor.set(0); - // blooper - chassis.pid_drive_set(12, 60, true); + pros::delay(1300); chassis.pid_wait(); + Little_Mech_Mac.set(0); + + chassis.pid_drive_set(8.3, 127, true); + chassis.pid_wait_quick_chain(); - pros::delay(1500); + chassis.pid_swing_set(LEFT_SWING, -93, 85, 31, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(20, 127, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-28, 60, true); + chassis.pid_drive_set(23, 127, true); chassis.pid_wait(); - - trapdoor.set(0); } -*/ /* Odom TESTING Functions */ @@ -1603,13 +1833,28 @@ void auton_setup_right(){ } /* TESTS */ -void empty(){ - pros::delay(100); - L1.brake(); +void pid_test(){ + chassis.pid_drive_constants_set(23, 0, 150); // 22 0 150 + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 + + chassis.pid_turn_set(90, 100, true); + chassis.pid_wait(); + pros::delay(1000); + + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait(); + pros::delay(1000); + + chassis.pid_turn_set(-90, 100, true); + chassis.pid_wait(); + pros::delay(1000); + + chassis.pid_turn_set(0, 100, true); + chassis.pid_wait(); } void wall_tracking_test() { - drive_wall(450); + drive_wall(450,127); } void wall_alignment_test() { pros::Task task1(controller_update); @@ -1623,17 +1868,51 @@ void wall_alignment_test() { R3.brake(); } void pid_tune(){ - pros::Task controller (controller_update); - chassis.pid_drive_set(odom_scaling * 24, 90, true); + pros::Task Task (controller_update); + chassis.drive_imu_scaler_set(1); + chassis.pid_turn_exit_condition_set(90_ms, 1_deg, 250_ms, 7_deg, 500_ms, 500_ms); + //pros::delay(5000); + chassis.pid_turn_set(90_deg, 40); + chassis.pid_wait(); + pros::delay(100); + chassis.pid_turn_set(180_deg, 40); + chassis.pid_wait(); + pros::delay(100); + chassis.pid_turn_set(270_deg, 40); + chassis.pid_wait(); + chassis.pid_turn_set(0_deg, 40); + chassis.pid_wait(); + pros::delay(1999999999); + + chassis.pid_turn_set(360_deg, 40, ez::raw); + chassis.pid_wait(); + pros::delay(1000); + + chassis.pid_turn_set(360*2_deg, 40, ez::raw); chassis.pid_wait(); + pros::delay(1000); + + chassis.pid_turn_set(360*3_deg, 80, ez::raw); + chassis.pid_wait(); + pros::delay(1000); +} + +void intake_test(){ + pros::Task anti_jam_auton1 (anti_jam_auton); + + bottom_intake(127); + intake_top.move(127); + intake_top_score.move(127); } + void color_sort_test(){ + pros::Task color_sor(color_sort_S); pros::Task antij(anti_jam_auton); top_intake(120); bottom_intake(120); - color = "B"; + trapdoor.set(1); intake_top.move(127); intake_bottom.move(127); diff --git a/src/main.cpp b/src/main.cpp index 2bc99b9..57705d0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -20,19 +20,21 @@ */ ez::Drive chassis( - {10, -7, -4}, //left - {-3, 6, 1}, //right - 2, + {-6, -5, -8}, //left + {14, 19, 18}, //right + 15, 3.25, 450 ); -//ez::tracking_wheel horiz_tracker(9, 2, 0); -ez::tracking_wheel vert_tracker(-12, 2, 0); +// NOTE 1/20 +// DISTANCE ON PORT 9 bool anti_jam_w = false; +int anti_jam_is_working = 0; +int color_count = 0; void anti_jam(){ - while(true){ + while(true && anti_jam_is_working==1){ float currentTime = float(pros::millis()); double current_top = intake_top.get_current_draw(); double velocity_top = intake_top.get_actual_velocity(); @@ -91,10 +93,12 @@ void anti_jam(){ pros::delay(ez::util::DELAY_TIME); } } + std::string color = "x"; // against R or B; press UP+X to change; x for disabled -int color_count = 0; -void color_sort_S() { - bool blue_color_sort = true; +bool control_to_controller = true; +int middgoal_Srore = 0; +int count_color = 0; +void color_sort_top() { color_sort.set_integration_time(3); while (true) { int hue_lower; @@ -102,55 +106,92 @@ void color_sort_S() { color_sort.set_led_pwm(100); if (color == "B") { hue_lower = 210; - hue_higher = 250; + hue_higher = 240; } else if (color == "R") { hue_lower = 0; - hue_higher = 10; + hue_higher = 20; } else { continue; } - bool in_proximity = color_sort.get_proximity() > 50; + if (master.get_digital(DIGITAL_R1)){ + middgoal_Srore = 1; + } else { + middgoal_Srore = 0; + } - if (in_proximity && ((hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)||(color == "R" && color_sort.get_hue() > 300)) && color_count < 17) { - color_sort_piston.set(1); - pros::delay(200); - color_sort_piston.set(0); - color_count += 1; + bool in_proximity = color_sort.get_proximity() > 240; + + if (in_proximity && ((hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher) || (color == "R" && color_sort.get_hue() > 340))){ + count_color ++; + } else { + count_color = 0; } + if (middgoal_Srore == 0 && count_color >= 4 ) { + count_color = 0; + pros::delay(50); + control_to_controller = false; + intake_top_score.move(-127); + intake_top.move(20); + pros::delay(350); + control_to_controller = true; + + } else if (middgoal_Srore == 1 && count_color >= 4){ + count_color = 0; + control_to_controller = false; + trapdoor.set(0); + intake_top_score.move(127); + intake_top.move(10); + pros::delay(600); + control_to_controller = true; + trapdoor.set(1); + } + //pros::delay(ez::util::DELAY_TIME); } } + void initialize() { + + // Set the color of the balls you want to throw out here + + color = "x"; + // discore_mech.set(1); + // trapdoor.set(1); + //intake_piston.set(1); + + + + // discore_mech.set(1); + // intake_piston.set(1); ez::ez_template_print(); color_sort.set_led_pwm(100); pros::delay(500); // Stop the user from doing anything while legacy ports configure //chassis.odom_tracker_back_set(&horiz_tracker); - chassis.odom_tracker_right_set(&vert_tracker); + // chassis.odom_tracker_right_set(&vert_tracker); chassis.opcontrol_curve_buttons_toggle(false); chassis.opcontrol_drive_activebrake_set(0.0); chassis.opcontrol_curve_default_set(0.0, 1); default_constants(); - pros::Task task1(anti_jam); + //pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"Skills", skills}, - {"Left Safe", solo_left}, - {"Left Side Solo", left_elims_quick}, - {"Right Safe", right_safe}, - {"Left Elims Quick", left_elims_quick}, - {"Right Elims Quick", right_elims_quick}, - {"Testing PID VS Odom", wall_alignment_test}, - {"Skills", skills}, - {"Pure Pursuit Wait Until\n\nGo to (24, 24) but start running an intake once the robot passes (12, 24)", odom_pure_pursuit_wait_until_example}, - {"Injected Boomerang Example", odom_boomerang_injected_pure_pursuit_example} + {"right safe", safe_skills}, + {"right solo", pid_tune}, + {"elims auton 3 goals", left_elims_quick_ml}, + {"elims left", left_elims_7ball}, }); chassis.initialize(); + bool broken = false; + double initialise = chassis.drive_imu_get(); + pros::delay(500); + if(initialise > chassis.drive_imu_get())(broken = true); ez::as::initialize(); master.rumble(chassis.drive_imu_calibrated() ? "." : "-"); + master.rumble(broken ? "." : "-"); } void controller_update_main() { @@ -179,7 +220,11 @@ void odom_reset(){ void disabled() { } -void competition_initialize() { } +void competition_initialize() { + + // discore_mech.set(1); + // intake_piston.set(1); + } void autonomous() { chassis.pid_targets_reset(); @@ -256,61 +301,109 @@ double avg_motor_temps() { double mean = sum / 6; return mean; -} - +} +bool r2_active = false; +uint32_t r2_time = 0; +bool r1_active = false; +uint32_t r1_time = 0; int Digital_X; void opcontrol() { chassis.drive_brake_set(MOTOR_BRAKE_COAST); int count = 0; - bool intake_auto_reverse_enabled = true; - pros::Task anti_jam_T(anti_jam); - pros::Task color_sort_task_running (color_sort_S); + bool intake_auto_reverse_enabled = false; + // pros::Task anti_jam_T(anti_jam); + pros::Task color_sort_task_running(color_sort_top); while (true) { chassis.opcontrol_arcade_standard(ez::SPLIT); - + + if (master.get_digital_new_press(DIGITAL_R2)) { + r2_active = true; + r2_time = pros::millis(); + intake_piston.set(1); + } + else if (master.get_digital_new_press(DIGITAL_R1)) { + r1_active = true; + r1_time = pros::millis(); + } - if (master.get_digital(DIGITAL_L1)) { + else if (master.get_digital(DIGITAL_L1)) { intake_bottom.move(-127); intake_top.move(-127); + // intake_top_score.move(-127); + } else if (master.get_digital(DIGITAL_L2)) { + //intake_piston.set(0); intake_bottom.move(127); - intake_top.move(127); - } - else if (master.get_digital(DIGITAL_R1)) { - intake_bottom.move(-60); - intake_top.move(-127); - } - else if (master.get_digital(DIGITAL_R2)) { - intake_bottom.move(127); - intake_top.move(0); + if (control_to_controller)(intake_top.move(127)); + if (control_to_controller)(intake_top_score.move(127)); } - else { - intake_bottom.move(0); - intake_top.move(0); - } - // } + + else if (master.get_digital(DIGITAL_A)) { + intake_bottom.move(127); + intake_top.move(55); + intake_top_score.move(-30); + } + else if (control_to_controller){ + intake_bottom.move(0); + intake_top.move(0); + intake_top_score.move(0); + } + + // } // else { // intake_bottom.move(-40); // intake_top.move(-60); // } // } + if (r2_active) { + if (pros::millis() - r2_time >= 1000) { + if (!master.get_digital(DIGITAL_L1) && !master.get_digital(DIGITAL_L2)){ + intake_bottom.move(-44); + intake_top.move(-127); + intake_top_score.move(-10); + } + } + + if (!master.get_digital(DIGITAL_R2)) { + r2_active = false; + intake_piston.set(0); + } + } + if (r1_active) { + if (pros::millis() - r1_time >= 150) { + intake_bottom.move(127); + intake_top.move(65); + intake_top_score.move(-40); + } + else { + intake_bottom.move(-75); + intake_top.move(-75); + intake_top_score.move(-70); + } + + if (!master.get_digital(DIGITAL_R1)) { + r1_active = false; + intake_piston.set(0); + } + } + if (master.get_digital(DIGITAL_RIGHT)) { - trapdoor.set(1); + trapdoor.set(0); } else { - trapdoor.set(0); + if(control_to_controller)(trapdoor.set(1)); } if (master.get_digital(DIGITAL_Y)) { - middle_stage.set(1); + mid_descore.set(1); } else { - middle_stage.set(0); + mid_descore.set(0); } if (master.get_digital(DIGITAL_B)) { @@ -319,11 +412,12 @@ void opcontrol() { else { Little_Mech_Mac.set(0); } + if (master.get_digital(DIGITAL_DOWN)) { - discore_mech.set(0); + discore_mech.set(1); } else { - discore_mech.set(1); + discore_mech.set(0); } if (master.get_digital_new_press(DIGITAL_X)) { @@ -333,6 +427,10 @@ void opcontrol() { if (Digital_X == 2){color = "x";} if (Digital_X == 3){color = "R";} } + if (master.get_digital_new_press(DIGITAL_UP)){ + if (anti_jam_is_working == 1){anti_jam_is_working = 0;} + else {anti_jam_is_working = 1;} + } if ( master.get_digital_new_press(DIGITAL_LEFT) @@ -348,19 +446,36 @@ void opcontrol() { // left_rush_mech.button_toggle(master.get_digital_new_press(DIGITAL_RIGHT)); - if (count == 80) { + if (count == 10) { // only update controller screen every 80 cycles count = 0; int dt_temps = (int) to_fahrenheit(avg_motor_temps()); int top_temp = (int) to_fahrenheit(intake_top.get_temperature()); + int top_score_temp = (int) to_fahrenheit(intake_top_score.get_temperature()); int bottom_temp = (int) to_fahrenheit(intake_bottom.get_temperature()); std::string intake_back = ""; + + // double left1temp = L1.get_temperature(); + // double left2temp = L2.get_temperature(); + // double left3temp = L3.get_temperature(); + // double right1temp = R1.get_temperature(); + // double right2temp = R2.get_temperature(); + // double right3temp = R3.get_temperature(); + + // int farenheitl1 = (int) to_fahrenheit(left1temp); + + + if (intake_auto_reverse_enabled){ intake_back = "N"; } - master.print(0, 0, "%f/%d/%d/%s/%s ", L1.get_temperature()/*color_sort.get_hue()dt_temps*//*(int)vertical_tracker.get_position()/100*/ , color_sort.get_proximity()/*top_temp*/, bottom_temp, color, intake_back); + + + + + master.print(0, 0, "%d/%d/%d/%s/%d ", /*L1.get_temperature*/(int)inertial.get_heading()/*color_sort.get_hue()*//*anti_jam_is_working(int)vertical_tracker.get_position()/100 dt_temps */, color_sort.get_proximity()/*top_temp*/, top_temp, color, middgoal_Srore); } count++; @@ -374,3 +489,5 @@ void opcontrol() { pros::delay(ez::util::DELAY_TIME); } } + + diff --git a/src/wall_tracking.cpp b/src/wall_tracking.cpp index 43c0f23..8116806 100644 --- a/src/wall_tracking.cpp +++ b/src/wall_tracking.cpp @@ -19,27 +19,97 @@ float wa_kP = .4; float wa_kI = 0; float wa_kD = 0.5; -float d_KP = 0.2; -float d_KI = 0.00002; -float d_KD = 0; -void drive_wall(float distance) { + +bool stop_task = false; +float targer_distance = 0; + +void chassis_drive_wall(float distance, float DRIVE_SPEED, bool chain, bool back_sensor) { + + if (back_sensor){ + float distance_for_chassis_ml = -(distance_back.get_distance() - distance)/24.4; + master.print(0, 0, "%d", distance_back.get_distance() ); + master.print(0, 0, "%.1f", distance_for_chassis_ml); + chassis.pid_drive_set(distance_for_chassis_ml, DRIVE_SPEED, true); + if (chain){ + chassis.pid_wait_quick_chain(); + } else { + chassis.pid_wait(); + } + + } + + if (!back_sensor){ + float distance_for_chassis = (distance_front_l.get_distance() - distance)/24.4; + master.print(0, 0, "%d", distance_front_l.get_distance() ); + master.print(0, 0, "%.1f", distance_for_chassis); + chassis.pid_drive_set(distance_for_chassis, DRIVE_SPEED, true); + if (chain){ + chassis.pid_wait_quick_chain(); + } else { + chassis.pid_wait(); + } +} +} + + +// void drive_wall(float distance){ +// targer_distance = distance; +// pros::Task drive_wall_task_running (drive_wall_task); +// while (stop_task){ +// pros::delay(10); +// } +// drive_wall_task_running.remove(); +// } + +float d_KP = 0.3; +float d_KI = 0; +float d_KD = 0.0021; + +void drive_wall(float distance, float DRIVE_SPEED) { + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); + stop_task = true; float error; float new_error; float prev_error; float prev_output; float integral; float derivative; - float slue_value = 10; - pros::delay(100); - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - while (distance_front.get_distance() > distance) { - error = distance_front.get_distance() - distance; + float arrival_time_B = 0; + float arrival_time_S = 0; + float arrival_distance_S = 5; + float arrival_distance_B = 15; + float time_out_S = 10; + float time_out_B = 100; + //float slue_value = 10; + + chassis_brake(); + while (true) { + + //Big error timeout + if (distance_front_l.get_distance() < distance + arrival_distance_B && distance_front_l.get_distance() > distance - arrival_distance_B){ + if (arrival_time_B == 0){ + arrival_time_B = pros::millis(); + } + }else { + arrival_time_B = 0; + } + if (arrival_time_B != 0 && (pros::millis() - arrival_time_B) > time_out_B){ + break; + } + //Small error timeout + if (distance_front_l.get_distance() < distance + arrival_distance_S && distance_front_l.get_distance() > distance - arrival_distance_S ){ + if (arrival_time_S == 0){ + arrival_time_S = pros::millis(); + } + }else { + arrival_time_S = 0; + } + if (arrival_time_S != 0 && (pros::millis() - arrival_time_S) > time_out_S){ + break; + } + + error = distance_front_l.get_distance() - distance; derivative = error - prev_error; // if (error == 0){ // error = 300; @@ -47,31 +117,21 @@ void drive_wall(float distance) { //imu_error = imu_sensor_value - inertial.get_heading(); //float turn_output = imu_error*0.1; float output = (error * d_KP + error * derivative * d_KD + error * integral * d_KI); - if ((output - prev_output) > slue_value){ - output = prev_output + slue_value; - } - L1.move_velocity(output); - L2.move_velocity(output); - L3.move_velocity(output); - R1.move_velocity(output); - R2.move_velocity(output); - R3.move_velocity(output); + // if ((output - prev_output) > slue_value){ + // output = prev_output + slue_value; + // } + chassis.drive_set(output,output); prev_error = error; - if (error < 600){ + //if (error < 600){ integral += error; - } - prev_output = output; + //} pros::delay(50); } - intake_top.move_velocity(127); - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + chassis.drive_set(0,0); + // stop_task = false; + chassis_brake(); } @@ -260,3 +320,12 @@ void wall_riding(float target_distance, float DRIVE_SPEED, float drive_distance) integral += error; } } + +void chassis_brake() { + L1.brake(); + L2.brake(); + L3.brake(); + R1.brake(); + R2.brake(); + R3.brake(); +}