From cd9de6f31fd0fda1ff91b2fdefe307e5d83c2e70 Mon Sep 17 00:00:00 2001 From: BestVex Date: Sun, 2 Nov 2025 08:55:48 -0800 Subject: [PATCH 01/15] solo --- src/autons.cpp | 119 ++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 118 insertions(+), 1 deletion(-) diff --git a/src/autons.cpp b/src/autons.cpp index 556b1a6..03d56e4 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -519,7 +519,6 @@ void right_safe(){ // needs updated - void solo_left() { chassis.odom_xyt_set(0_in, 0_in, -90_deg); // trapdoor.set(1); @@ -628,6 +627,124 @@ void solo_left() { + // intake_bottom.set_brake_mode(MOTOR_BRAKE_COAST); + + // chassis.pid_swing_set(LEFT_SWING, -37, 70*1.45, 52*1.15, false); + // chassis.pid_wait_quick(); + + // chassis.pid_drive_set(-3, 60, true); + + pros::delay(100); + +} +void solo_left1() { + 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(); + intake_bottom.move(127); + chassis.pid_drive_set(37.5, 80, true); + chassis.pid_wait(); + Little_Mech_Mac.set(1); + pros::delay(50); + chassis.pid_turn_set(180, 110, true); + chassis.pid_wait_quick(); + + + + chassis.pid_drive_set(13, 90, true); + chassis.pid_wait_quick(); + + 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); + + intake_bottom.move(127); + chassis.pid_drive_set(13, 60, true); + pros::delay(140); + + // chassis.pid_turn_set(180, 80, true); + // chassis.pid_wait_quick(); + + 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); + trapdoor.set(1); + intake_top.move(127); + pros::delay(1600); + chassis.drive_brake_set(MOTOR_BRAKE_HOLD); + + Little_Mech_Mac.set(0); + + //intake_top.brake(); + + + chassis.pid_drive_set(4, 100, true); + chassis.pid_wait(); + + chassis.pid_swing_set(RIGHT_SWING, 46.2, 74, -18.5, 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); + trapdoor.set(1); + chassis.pid_wait_quick(); + + + + intake_bottom.move(127); + intake_top.move(127); + pros::delay(150); + + trapdoor.set(0); + /* + chassis.pid_drive_set(15, 80, true); + chassis.pid_wait_quick(); + + intake_top.move(-127); + + middle_stage.set(0); + Little_Mech_Mac.set(0); + + 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(); + + chassis.pid_drive_set(33, 90, true); + chassis.pid_wait_quick(); + + 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(); + + + // intake_bottom.set_brake_mode(MOTOR_BRAKE_COAST); // chassis.pid_swing_set(LEFT_SWING, -37, 70*1.45, 52*1.15, false); From 8f8b69713b79c46590b59a4086403d519f82f9cd Mon Sep 17 00:00:00 2001 From: BestVex Date: Fri, 14 Nov 2025 10:59:47 -0800 Subject: [PATCH 02/15] 5 autons including solo --- include/autons.hpp | 1 + include/subsystems.hpp | 28 +- project.pros | 4 +- src/autons.cpp | 909 +++++++++++++++++++++++++++-------------- src/main.cpp | 48 ++- 5 files changed, 646 insertions(+), 344 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index be80686..4d22024 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -17,6 +17,7 @@ void default_constants(); void empty(); void skills(); +void skills_before_changing_the_wall(); void skills_without_odom(); void left_elims(); diff --git a/include/subsystems.hpp b/include/subsystems.hpp index 40bfaaf..9b57c16 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -15,15 +15,15 @@ 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(-11); +inline pros::Motor L2(-15); +inline pros::Motor L3(-16); -inline pros::Motor R1(-3); -inline pros::Motor R2(6); -inline pros::Motor R3(1); +inline pros::Motor R1(5); +inline pros::Motor R2(8); +inline pros::Motor R3(21); -inline pros::Imu inertial(2); +inline pros::Imu inertial(1); inline pros::Distance distance_back_l(13); inline pros::Distance distance_front_l(19); @@ -33,16 +33,18 @@ inline pros::Distance distance_front(14); inline pros::Optical color_sort(15); -inline pros::Motor intake_bottom(20); -inline pros::Motor intake_top(11); +inline pros::Motor intake_bottom(7); +inline pros::Motor intake_top(-17); +inline pros::Motor intake_top_score(-20); -inline ez::Piston trapdoor('G'); +inline ez::Piston trapdoor('A'); inline ez::Piston middle_stage('C'); -inline ez::Piston Little_Mech_Mac('B'); -inline ez::Piston color_sort_piston('H'); +inline ez::Piston Little_Mech_Mac('C'); +inline ez::Piston color_sort_piston('D'); +inline ez::Piston intake_piston('H'); inline ez::Piston right_rush_mech('F'); -inline ez::Piston left_rush_mech('F'); +inline ez::Piston left_rush_mech('B'); inline ez::Piston discore_mech('A'); diff --git a/project.pros b/project.pros index 039febe..cc1c53f 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": "Best Pog", "target": "v5", "templates": { "EZ-Template": { @@ -425,7 +425,7 @@ }, "upload_options": { "description": "roboticsisez.com", - "icon": "X", + "icon": "alien", "slot": 1 }, "use_early_access": false diff --git a/src/autons.cpp b/src/autons.cpp index 03d56e4..5b85b2e 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -206,124 +206,124 @@ void right_elims_quick(){ // FINISHED void left_safe(){ - pros::Task contor1 (controller_update); - chassis.odom_xyt_set(0_in, 0_in, -30_deg); + chassis.odom_xyt_set(0_in, 0_in, -30_deg); - bottom_intake(127); - trapdoor.set(false); - chassis.pid_drive_set(30, 100, true); - pros::delay(500); - Little_Mech_Mac.set(true); - chassis.pid_wait(); + intake_bottom.move(127); + intake_top.move(127); - chassis.pid_drive_set(-4, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(24.8, 100, true); + pros::delay(500); + // Little_Mech_Mac.set(true); + chassis.pid_wait_quick(); + //Little_Mech_Mac.set(0); - intake_bottom.move(-30); - intake_top.move(-30); + chassis.pid_turn_set(-135, 80, true); + chassis.pid_wait_quick_chain(); - 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(-13, 80, true); chassis.pid_wait(); - 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); + intake_top_score.move(-127); - chassis.pid_turn_set(179, 80, true); - chassis.pid_wait(); + pros::delay(900); - 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(); + chassis.pid_drive_set(37, 80, true); + chassis.pid_wait_quick_chain(); - pros::delay(100); + chassis.pid_turn_set(180, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-91, 60, true); + chassis.pid_drive_set(14, 70, true); + Little_Mech_Mac.set(1); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); 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(); - pros::delay(100); - - Little_Mech_Mac.set(1); + pros::delay(370); - chassis.pid_turn_set(180, 80, true); + chassis.pid_drive_set(-26, 75, true); + pros::delay(600); + trapdoor.set(1); chassis.pid_wait(); - intake_bottom.move(127); + pros::delay(1600); - chassis.pid_drive_set(13.5, 60, true); - chassis.pid_wait(); + Little_Mech_Mac.set(0); - pros::delay(300); + chassis.pid_drive_set(5, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-35, 80, true); - chassis.pid_wait(); + chassis.pid_turn_set(90, 90, true); + chassis.pid_wait_quick_chain(); - trapdoor.set(1); + chassis.pid_drive_set(6.6, 90, true); + chassis.pid_wait_quick_chain(); - intake_top.move(127); + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(27, 60, true); + chassis.pid_wait_quick_chain(); } // FINISHED void left_elims_quick(){ chassis.odom_xyt_set(0_in, 0_in, -30_deg); - bottom_intake(127); - trapdoor.set(false); - chassis.pid_drive_set(30, 100, true); - pros::delay(550); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_drive_set(25, 100, true); + pros::delay(500); Little_Mech_Mac.set(true); - chassis.pid_wait(); + chassis.pid_wait_quick_chain(); + Little_Mech_Mac.set(0); - chassis.pid_turn_set(-135, 60, true); - chassis.pid_wait(); + chassis.pid_turn_set(-145, 80, true); + chassis.pid_wait_quick_chain(); - - 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_drive_set(29, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(180, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(14, 70, true); + Little_Mech_Mac.set(1); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); chassis.pid_wait(); - pros::delay(50); - chassis.pid_drive_set(-34, 100); + + pros::delay(370); + + chassis.pid_drive_set(-26, 75, true); + pros::delay(600); + trapdoor.set(1); chassis.pid_wait(); - trapdoor.set(true); - - + pros::delay(1600); + + Little_Mech_Mac.set(0); + + chassis.pid_drive_set(5, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(90, 90, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(6.6, 90, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(27, 60, true); + chassis.pid_wait_quick_chain(); + + } @@ -429,91 +429,68 @@ void right_safe1(){ void right_safe(){ - pros::Task contor1 (controller_update); - chassis.odom_xyt_set(0_in, 0_in, 30_deg); + 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); - pros::delay(500); - Little_Mech_Mac.set(true); - chassis.pid_wait(); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-4, 60, true); + chassis.pid_drive_set(10.1, 70, true); + Little_Mech_Mac.set(1); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); chassis.pid_wait(); - intake_bottom.move(-30); - intake_top.move(-30); + pros::delay(350); - chassis.pid_turn_set(-45, 60, true); + chassis.pid_drive_set(-26, 75, true); + pros::delay(200); + trapdoor.set(1); chassis.pid_wait(); - Little_Mech_Mac.set(false); - - 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(); + pros::delay(1000); - 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_swing_set(LEFT_SWING, -135, 127, -20, true); + // chassis.pid_wait_quick(); - chassis.pid_turn_set(-179, 80, true); - chassis.pid_wait(); + chassis.pid_drive_set(5, 80, true); + chassis.pid_wait_quick_chain(); + trapdoor.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(); + chassis.pid_turn_set(-142, 80, true); + Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); - pros::delay(100); + chassis.pid_drive_set(24, 80, true); + pros::delay(650); + Little_Mech_Mac.set(1); + chassis.pid_wait(); - chassis.pid_turn_set(91, 60, true); + chassis.pid_drive_set(23, 60, true); + pros::delay(100); + Little_Mech_Mac.set(0); 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(); - pros::delay(100); + intake_bottom.move(-127); + intake_top.move(-127); + intake_top_score.move(-127); - Little_Mech_Mac.set(1); + intake_piston.set(1); - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait(); + pros::delay(900); - intake_bottom.move(127); + chassis.pid_drive_set(-25, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(13.5, 60, true); - chassis.pid_wait(); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-24, 60, true); + chassis.pid_wait_quick_chain(); - pros::delay(150); - chassis.pid_drive_set(-35, 80, true); - chassis.pid_wait(); - intake_top.move(127); } @@ -535,7 +512,7 @@ void solo_left() { // R2.brake(); // R3.brake(); intake_bottom.move(127); - chassis.pid_drive_set(37.5, 80, true); + chassis.pid_drive_set(36.5, 80, true); chassis.pid_wait(); Little_Mech_Mac.set(1); pros::delay(50); @@ -557,7 +534,7 @@ void solo_left() { // chassis.pid_turn_set(180, 80, true); // chassis.pid_wait_quick(); - chassis.pid_drive_set(-32, 100, true); + chassis.pid_drive_set(-32, 70, true); // pros::delay(400); // intake_top.move(127); chassis.pid_wait_quick(); @@ -576,7 +553,7 @@ void solo_left() { chassis.pid_drive_set(4, 100, true); chassis.pid_wait(); - chassis.pid_swing_set(RIGHT_SWING, 46.2, 74, -18.5, true); + chassis.pid_swing_set(RIGHT_SWING, 46.2, 74, -23.5, true); chassis.pid_wait(); trapdoor.set(0); @@ -588,7 +565,7 @@ void solo_left() { chassis.pid_wait_quick(); chassis.pid_drive_set(-8, 100, true); - pros::delay(200); + pros::delay(300); trapdoor.set(1); chassis.pid_wait_quick(); @@ -616,13 +593,13 @@ void solo_left() { chassis.pid_wait(); intake_top.brake(); - chassis.pid_drive_set(33, 90, true); + chassis.pid_drive_set(39, 90, true); chassis.pid_wait_quick(); - chassis.pid_turn_set(-43,80, false); + chassis.pid_turn_set(-41,80, false); chassis.pid_wait_quick(); intake_bottom.move(-127); - chassis.pid_drive_set(8.5, 90, true); + chassis.pid_drive_set(11.5, 90, true); chassis.pid_wait_quick(); @@ -756,175 +733,94 @@ void solo_left1() { } -// needs updated - +//done void solo_right (){ - 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() > 645){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - - Little_Mech_Mac.set(1); - - chassis.pid_turn_set(180, 110, true); - chassis.pid_wait(); - - 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); - - intake_bottom.move(127); - intake_top.move(127); - - - pros::delay(500); - - Little_Mech_Mac.set(0); + chassis.pid_drive_set(21, 90, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 60, true); - chassis.pid_wait_quick(); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-30, 60, true); + chassis.pid_drive_set(10.5, 70, true); + Little_Mech_Mac.set(1); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); chassis.pid_wait(); - trapdoor.set(0); - chassis.pid_drive_set(-1000, 60, true); - - pros::delay(1600); - - intake_top.brake(); - intake_bottom.move(127); - - chassis.pid_swing_set(LEFT_SWING, -55, 90, -17, true); - chassis.pid_wait(); + pros::delay(350); - chassis.pid_drive_set(32, 45, true); + chassis.pid_drive_set(-26, 75, true); + pros::delay(500); + trapdoor.set(1); chassis.pid_wait(); - trapdoor.set(1); - - intake_top.move(-20); - intake_bottom.move(-20); - - intake_top.set_brake_mode(MOTOR_BRAKE_HOLD); - intake_bottom.set_brake_mode(MOTOR_BRAKE_HOLD); - - intake_top.move(0); - intake_bottom.brake(); + pros::delay(1000); - 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_swing_set(LEFT_SWING, -135, 127, -20, true); + // chassis.pid_wait_quick(); - // chassis.pid_drive_set(-12, 50, true); - // chassis.pid_wait(); -} + chassis.pid_drive_set(5, 80, true); + chassis.pid_wait_quick_chain(); + trapdoor.set(0); -/* old -void left_elims() { - pros::Task anti (anti_jam_auton); - 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_turn_set(-142, 80, true); + Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_constants_set(22, 0, 130); + chassis.pid_drive_set(24, 80, true); + pros::delay(650); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - // intake_bottom.move(127); - bottom_intake(127); + chassis.pid_turn_set(179, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(RIGHT_SWING, 115, 75, 15); + chassis.pid_drive_set(48, 65, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(25, 60, true); + chassis.pid_drive_set(-5.6, 80, 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_turn_set(132, 80, true); + chassis.pid_wait_quick_chain(); + // Little_Mech_Mac.set(1); - chassis.pid_drive_set(-15, -40, true); + chassis.pid_drive_set(-10, 60, true); chassis.pid_wait(); - chassis.pid_turn_set(140, 60, true); - chassis.pid_wait_quick(); + intake_top.move(127); + intake_top_score.move(-127); - chassis.pid_drive_set(16, 60, true); - chassis.pid_wait_quick(); + pros::delay(750); - chassis.pid_turn_set(-98 , 60); - chassis.pid_wait(); + chassis.pid_drive_set(39, 90, 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_set(90, 80, true); + chassis.pid_wait_quick_chain(); + Little_Mech_Mac.set(1); - chassis.pid_turn_set(174, 60, true); - chassis.pid_wait_quick(); - //wall_alignment_R(1000); + chassis.pid_drive_set(11, 60, true); + chassis.pid_wait(); - // blooper - Little_Mech_Mac.set(1); - pros::delay(100); + pros::delay(350); - chassis.pid_drive_set(25, 55, true); + chassis.pid_drive_set(-25, 75, true); + pros::delay(500); + trapdoor.set(1); chassis.pid_wait(); - // intake_bottom.move(127); - bottom_intake(127); - pros::delay(600); - top_intake(127); - Little_Mech_Mac.set(1); - // intake_top.move(127); - - //intake_top.move(127); + pros::delay(1100); - chassis.pid_drive_set(-27.2, 70, true); + chassis.pid_drive_set(12, 60, true); chassis.pid_wait(); - trapdoor.set(0); - chassis.pid_drive_set(-5, 20, true); } -*/ + void skills_without_odom(){ chassis.odom_xyt_set(0_in, 0_in, -90_deg); trapdoor.set(1); @@ -987,10 +883,10 @@ void skills_without_odom(){ } -void skills() { +void skills_before_changing_the_wall() { pros::Task task1(controller_update); //pros::Task color_sort_task_running(color_sort_S); - drive_wall(600); + drive_wall(510); discore_mech.set(1); chassis.odom_xyt_set(0_in, 0_in, -90_deg); trapdoor.set(0); @@ -1030,8 +926,8 @@ void skills() { //intake_bottom.move(127); //intake_top.move(127); - chassis.pid_drive_set(13, 60, true); - pros::delay(1000); + + pros::delay(1350); /* Back up from match loader @@ -1089,7 +985,7 @@ void skills() { // 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(12_in, 100); chassis.pid_wait(); /* Score first match loader @@ -1115,29 +1011,28 @@ void skills() { trapdoor.set(1); pros::delay(100); trapdoor.set(1); - pros::delay(1900); + pros::delay(1700); /* empty second match loader */ - chassis.pid_drive_set(45.5, 100, true); - chassis.pid_wait(); + chassis.pid_drive_set(31.5, 100, true); + + pros::delay(1800); - chassis.pid_drive_set(13, 60, true); - chassis.pid_wait(); - pros::delay(1000); /* Score second 6 on long goal */ trapdoor.set(0); + + Little_Mech_Mac.set(0); + pros::delay(100); chassis.pid_drive_set(-29.5, 60, true); chassis.pid_wait(); - Little_Mech_Mac.set(0); - top_intake(-15); bottom_intake(-15); @@ -1240,13 +1135,13 @@ void skills() { //intake_top.move(127); //intake_bottom.move(127); - chassis.pid_drive_set(13, 60, true); - pros::delay(1000); + pros::delay(1300); + + Little_Mech_Mac.set(0); chassis.pid_drive_set(-20, 60, true); chassis.pid_wait(); - Little_Mech_Mac.set(0); top_intake(0); bottom_intake(0); @@ -1297,12 +1192,10 @@ void skills() { Little_Mech_Mac.set(1); - chassis.pid_drive_set(45, 80, true); - chassis.pid_wait(); + chassis.pid_drive_set(35, 80, true); trapdoor.set(0); - chassis.pid_drive_set(13, 60, true); - chassis.pid_wait(); - pros::delay(1000); + + pros::delay(2200); /* Score second 6 on long goal @@ -1341,6 +1234,402 @@ void skills() { chassis.pid_drive_set(100, 127, false); chassis.pid_wait_quick(); +} +// 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 +// */ + +// // 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(); + +// chassis.pid_drive_set(11.5, 90, true); +// chassis.pid_wait(); + +// //intake_bottom.move(127); +// //intake_top.move(127); +// chassis.pid_drive_set(13, 60, true); +// pros::delay(1000); + +// /* +// Back up from match loader +// */ + +// Little_Mech_Mac.set(0); +// pros::delay(100); + +// chassis.pid_drive_set(-9.8, 90, true); +// chassis.pid_wait_quick(); + +// bottom_intake(0); +// top_intake(0); + +// chassis.pid_turn_set(-90, 60, true); +// chassis.pid_wait(); + +// /* +// Relocate to blue side +// */ + +// chassis.pid_odom_set({{-45_in, 0_in}, fwd, 100}, true); +// chassis.pid_wait(); + +// chassis.pid_turn_set(0, 60, true); +// chassis.pid_wait(); + +// /* +// Reset location to 0, 0 +// */ + +// 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); + +// // 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; + +// p_x = position_x; +// p_y = position_y; + +// chassis.odom_xy_set(position_x, position_y); +// pros::delay(150); + +// /* +// Setup for match loader +// */ + +// chassis.pid_turn_set(90, 60, true); +// chassis.pid_wait(); + +// // 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_wait(); +// /* +// Score first match loader +// */ + +// Little_Mech_Mac.set(1); + +// chassis.pid_turn_set(0, 60, true); +// chassis.pid_wait(); + +// top_intake(-20); +// bottom_intake(-20); +// //intake_top.move(127); +// //intake_bottom.move(127); + +// chassis.pid_drive_set(-16, 90, true); +// chassis.pid_wait_quick(); + +// top_intake(127); +// bottom_intake(127); + +// chassis.pid_drive_set(-100, 10, true); +// trapdoor.set(1); +// pros::delay(100); +// trapdoor.set(1); +// pros::delay(1900); + +// /* +// 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); + +// chassis.pid_drive_set(-29.5, 60, true); +// chassis.pid_wait(); + +// Little_Mech_Mac.set(0); + +// top_intake(-15); +// bottom_intake(-15); + +// chassis.pid_drive_set(-5, 20, true); +// pros::delay(150); + +// top_intake(127); +// bottom_intake(127); + +// trapdoor.set(1); + +// pros::delay(1000); + + +// /* +// Set up for first 2 middle balls +// */ + +// // Activate color sort + + +// chassis.pid_drive_set(10, 110, true); +// chassis.pid_wait(); + +// chassis.pid_turn_set(88, 80, true); +// chassis.pid_wait(); +// trapdoor.set(0); + +// // chassis.pid_odom_set({{26.8, -10.5}, fwd, 80}, true); +// // chassis.pid_wait_quick(); + +// chassis.pid_drive_set(80, 80, true); +// chassis.pid_wait(); + +// chassis.pid_turn_set(-1.5, 60); +// chassis.pid_wait(); + +// 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); + + +// //drive_wall(450); + +// chassis.pid_turn_set(88.5, 60); +// chassis.pid_wait(); + +// 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); + +// //drive_wall(450); + +// chassis.pid_turn_set(-3, 60); +// chassis.pid_wait(); + +// pros::delay(150); + +// /* +// reset position +// */ + +// 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; + +// p_x = position_x_1; +// p_y = position_y_1; + +// pros::delay(150); + +// chassis.odom_xy_set(position_x_1, position_y_1); +// chassis.pid_wait(); + +// /* +// grab third match load +// */ + +// Little_Mech_Mac.set(1); + +// chassis.pid_drive_set(24, 60, true); +// chassis.pid_wait(); + +// top_intake(127); +// bottom_intake(127); + +// //intake_top.move(127); +// //intake_bottom.move(127); + +// chassis.pid_drive_set(13, 60, true); +// pros::delay(1000); + +// chassis.pid_drive_set(-20, 60, true); +// chassis.pid_wait(); + +// Little_Mech_Mac.set(0); +// top_intake(0); +// bottom_intake(0); + +// chassis.pid_turn_set(-88, 60, true); +// chassis.pid_wait(); + +// chassis.pid_drive_set(-11.9, 60, true); +// chassis.pid_wait(); + +// /* +// go to blue side +// */ + +// chassis.pid_turn_set(-3, 60, true); +// chassis.pid_wait_quick(); + +// chassis.pid_drive_set(-85, 80, true); +// chassis.pid_wait(); + +// chassis.pid_turn_set(86, 60, true); +// chassis.pid_wait(); + +// chassis.pid_drive_set(-11, 80, true); +// chassis.pid_wait(); + +// /* +// score third set of match load +// */ + +// chassis.pid_turn_set(176, 60, true); +// chassis.pid_wait(); + +// chassis.pid_drive_set(-19.5, 60, true); +// chassis.pid_wait(); + +// top_intake(127); +// bottom_intake(127); + +// trapdoor.set(1); + +// pros::delay(2000); + +// /* +// empty second match loader +// */ + + + +// Little_Mech_Mac.set(1); + +// 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); + +// /* +// Score second 6 on long goal +// */ + +// Little_Mech_Mac.set(0); + +// pros::delay(100); + +// chassis.pid_turn_set(179, 60, true); + +// chassis.pid_drive_set(-33.5, 60, true); +// chassis.pid_wait(); + +// top_intake(127); +// bottom_intake(127); + +// trapdoor.set(1); + +// pros::delay(2000); + +// chassis.odom_xyt_set(0, 0, 180); + +// chassis.pid_drive_set(5, 127, true); +// chassis.pid_wait(); + +// chassis.pid_turn_set(-95, 127, true); +// chassis.pid_wait(); + +// chassis.pid_drive_set(48, 127, true); +// chassis.pid_wait(); + +// chassis.pid_turn_set(178, 127, true); +// chassis.pid_wait(); + +// chassis.pid_drive_set(100, 127, false); +// chassis.pid_wait_quick(); + +// } + +void skills() { + chassis.pid_drive_set(21, 90, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(10.1, 70, true); + Little_Mech_Mac.set(1); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + chassis.pid_wait(); + + pros::delay(1000); + + chassis.pid_drive_set(-26, 75, true); + pros::delay(200); + trapdoor.set(1); + chassis.pid_wait(); + + Little_Mech_Mac.set(0); + + pros::delay(2500); + + chassis.pid_drive_set(10, 80, true); + chassis.pid_wait(); + + chassis.pid_turn_set(105, 80, true); + chassis.pid_wait(); + + chassis.pid_swing_set(LEFT_SWING, 170, 127, 35, true); + chassis.pid_wait(); + + Little_Mech_Mac.set(1); + + chassis.pid_drive_set(50, 127, true); + chassis.pid_wait(); + } /* old diff --git a/src/main.cpp b/src/main.cpp index 2bc99b9..a83eb98 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -20,15 +20,15 @@ */ ez::Drive chassis( - {10, -7, -4}, //left - {-3, 6, 1}, //right - 2, + {-11, -15, -16}, //left + {5, 8, 21}, //right + 1, 3.25, 450 ); //ez::tracking_wheel horiz_tracker(9, 2, 0); -ez::tracking_wheel vert_tracker(-12, 2, 0); +// ez::tracking_wheel vert_tracker(-12, 2, 0); bool anti_jam_w = false; void anti_jam(){ @@ -126,7 +126,7 @@ void initialize() { 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); @@ -136,10 +136,10 @@ void initialize() { pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"Skills", skills}, - {"Left Safe", solo_left}, + {"Left Safe", skills}, + {"Right Safe", skills_before_changing_the_wall}, + {"Skills", left_safe}, {"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}, @@ -256,16 +256,17 @@ double avg_motor_temps() { double mean = sum / 6; return mean; -} +} 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_S); + color = "x"; while (true) { chassis.opcontrol_arcade_standard(ez::SPLIT); @@ -273,23 +274,32 @@ void opcontrol() { 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_bottom.move(127); intake_top.move(127); + intake_top_score.move(127); + } else if (master.get_digital(DIGITAL_R1)) { - intake_bottom.move(-60); - intake_top.move(-127); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(-75); } else if (master.get_digital(DIGITAL_R2)) { - intake_bottom.move(127); - intake_top.move(0); + intake_bottom.move(-127); + intake_top.move(-127); + intake_top_score.move(-127); + intake_piston.set(1); } else { intake_bottom.move(0); intake_top.move(0); + intake_top_score.move(0); + intake_piston.set(0); } // } // else { @@ -307,10 +317,10 @@ void opcontrol() { } if (master.get_digital(DIGITAL_Y)) { - middle_stage.set(1); + middle_stage.set(0); } else { - middle_stage.set(0); + middle_stage.set(1); } if (master.get_digital(DIGITAL_B)) { @@ -360,7 +370,7 @@ void opcontrol() { 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/%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); } count++; From faab0b49145b6f39be81cde2b4d4e550577b1851 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Fri, 21 Nov 2025 17:53:11 -0800 Subject: [PATCH 03/15] skills --- include/autons.hpp | 2 + include/subsystems.hpp | 6 +- project.pros | 2 +- src/autons.cpp | 693 +++++++++++++++++++---------------------- src/main.cpp | 84 +++-- 5 files changed, 384 insertions(+), 403 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index 4d22024..a9ccd78 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -24,6 +24,8 @@ void left_elims(); void red_top_elims(); void blue_top_quals(); +void intake_test(); + void blue_bottom_elims(); void red_bottom_elims(); void blue_bottom_quals(); diff --git a/include/subsystems.hpp b/include/subsystems.hpp index 9b57c16..4c0bf86 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -29,9 +29,9 @@ 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_front(13); -inline pros::Optical color_sort(15); +inline pros::Optical color_sort(9); inline pros::Motor intake_bottom(7); inline pros::Motor intake_top(-17); @@ -46,7 +46,7 @@ inline ez::Piston intake_piston('H'); inline ez::Piston right_rush_mech('F'); inline ez::Piston left_rush_mech('B'); -inline ez::Piston discore_mech('A'); +inline ez::Piston discore_mech('B'); inline pros::Distance intake_distance(16); diff --git a/project.pros b/project.pros index cc1c53f..a06e614 100644 --- a/project.pros +++ b/project.pros @@ -425,7 +425,7 @@ }, "upload_options": { "description": "roboticsisez.com", - "icon": "alien", + "icon": "clawbot", "slot": 1 }, "use_early_access": false diff --git a/src/autons.cpp b/src/autons.cpp index 5b85b2e..e6080f9 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -74,49 +74,28 @@ void anti_jam_auton(){ int current_top; int velocity_bottom; int current_bottom; - int current_threshold = 2300; + int current_threshold = 2000; bool is_jammed_fwd = false; bool is_jammed_bcwd = 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(); + 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; - } - } - 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_bottom > current_threshold && velocity_bottom < 10){ + is_jammed_fwd = true; } if (is_jammed_fwd){ - 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); - } - is_jammed_fwd = false; - intake_top.move(top_stage_intake); - intake_bottom.move(bottom_stage_intake); - pros::delay(300); - } - if (is_jammed_bcwd){ 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); } - is_jammed_bcwd = false; - intake_top.move(top_stage_intake); + is_jammed_fwd = false; intake_bottom.move(bottom_stage_intake); pros::delay(300); } - pros::delay(40); } } @@ -204,18 +183,29 @@ void right_elims_quick(){ trapdoor.set(false); } +void intake_test(){ + pros::Task anti_jam_auton1 (anti_jam_auton); + + + bottom_intake(127); + intake_top.move(127); + intake_top_score.move(127); +} + // FINISHED void left_safe(){ chassis.odom_xyt_set(0_in, 0_in, -30_deg); + trapdoor.set(1); + intake_bottom.move(127); intake_top.move(127); chassis.pid_drive_set(24.8, 100, true); - pros::delay(500); - // Little_Mech_Mac.set(true); + pros::delay(450); + Little_Mech_Mac.set(true); chassis.pid_wait_quick(); - //Little_Mech_Mac.set(0); + Little_Mech_Mac.set(0); chassis.pid_turn_set(-135, 80, true); chassis.pid_wait_quick_chain(); @@ -244,7 +234,7 @@ void left_safe(){ chassis.pid_drive_set(-26, 75, true); pros::delay(600); - trapdoor.set(1); + trapdoor.set(0); chassis.pid_wait(); pros::delay(1600); @@ -257,7 +247,7 @@ void left_safe(){ chassis.pid_turn_set(90, 90, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(6.6, 90, true); + chassis.pid_drive_set(6.9, 90, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(0, 80, true); @@ -269,6 +259,7 @@ void left_safe(){ // FINISHED void left_elims_quick(){ + trapdoor.set(1); chassis.odom_xyt_set(0_in, 0_in, -30_deg); intake_bottom.move(127); @@ -290,38 +281,46 @@ void left_elims_quick(){ chassis.pid_turn_set(180, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(14, 70, true); + chassis.pid_drive_set(15.1, 70, true); Little_Mech_Mac.set(1); intake_bottom.move(127); intake_top.move(127); intake_top_score.move(127); chassis.pid_wait(); - pros::delay(370); + pros::delay(50); chassis.pid_drive_set(-26, 75, true); pros::delay(600); - trapdoor.set(1); + trapdoor.set(0); chassis.pid_wait(); - pros::delay(1600); + // int hue_lower = 210; + // int hue_higher = 250; + // int current_time = pros::millis(); + // bool in_proximity = color_sort.get_proximity() > 50; + // while ((pros::millis() - current_time < 1800) || (in_proximity && !(hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher))){ + // 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(2200); Little_Mech_Mac.set(0); - chassis.pid_drive_set(5, 80, true); + chassis.pid_drive_set(10.5, 80, true); chassis.pid_wait_quick_chain(); + trapdoor.set(1); - chassis.pid_turn_set(90, 90, true); - chassis.pid_wait_quick_chain(); + pros::delay(200); - chassis.pid_drive_set(6.6, 90, true); - chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(0, 80, true); - chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(-15, 80, true); + chassis.pid_wait_quick(); - chassis.pid_drive_set(27, 60, true); - chassis.pid_wait_quick_chain(); @@ -429,13 +428,15 @@ void right_safe1(){ void right_safe(){ + trapdoor.set(1); + chassis.pid_drive_set(21, 90, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(10.1, 70, true); + chassis.pid_drive_set(10.2, 70, true); Little_Mech_Mac.set(1); intake_bottom.move(127); intake_top.move(127); @@ -445,18 +446,18 @@ void right_safe(){ pros::delay(350); chassis.pid_drive_set(-26, 75, true); - pros::delay(200); - trapdoor.set(1); + pros::delay(250); + trapdoor.set(0); chassis.pid_wait(); - pros::delay(1000); + pros::delay(1300); // chassis.pid_swing_set(LEFT_SWING, -135, 127, -20, true); // chassis.pid_wait_quick(); - chassis.pid_drive_set(5, 80, true); + chassis.pid_drive_set(7, 80, true); chassis.pid_wait_quick_chain(); - trapdoor.set(0); + trapdoor.set(1); chassis.pid_turn_set(-142, 80, true); Little_Mech_Mac.set(0); @@ -467,26 +468,26 @@ void right_safe(){ Little_Mech_Mac.set(1); chassis.pid_wait(); - chassis.pid_drive_set(23, 60, true); + chassis.pid_drive_set(20.5, 40, true); pros::delay(100); Little_Mech_Mac.set(0); chassis.pid_wait(); - intake_bottom.move(-127); - intake_top.move(-127); + intake_bottom.move(-90); + intake_top.move(-90); intake_top_score.move(-127); intake_piston.set(1); - pros::delay(900); + pros::delay(1800); - chassis.pid_drive_set(-25, 80, true); + chassis.pid_drive_set(-24, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-24, 60, true); + chassis.pid_drive_set(-20, 60, true); chassis.pid_wait_quick_chain(); @@ -502,15 +503,6 @@ void solo_left() { 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(); intake_bottom.move(127); chassis.pid_drive_set(36.5, 80, true); chassis.pid_wait(); @@ -535,8 +527,6 @@ void solo_left() { // chassis.pid_wait_quick(); chassis.pid_drive_set(-32, 70, 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); @@ -735,15 +725,17 @@ void solo_left1() { //done void solo_right (){ + //pros::Task anti_jam_auton1 (anti_jam_auton); + trapdoor.set(1); chassis.pid_drive_set(21, 90, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(10.5, 70, true); + chassis.pid_drive_set(10.8, 70, true); Little_Mech_Mac.set(1); - intake_bottom.move(127); + bottom_intake(127); intake_top.move(127); intake_top_score.move(127); chassis.pid_wait(); @@ -751,9 +743,8 @@ void solo_right (){ pros::delay(350); chassis.pid_drive_set(-26, 75, true); - pros::delay(500); - trapdoor.set(1); chassis.pid_wait(); + trapdoor.set(0); pros::delay(1000); @@ -762,16 +753,17 @@ void solo_right (){ chassis.pid_drive_set(5, 80, true); chassis.pid_wait_quick_chain(); - trapdoor.set(0); + trapdoor.set(1); - chassis.pid_turn_set(-142, 80, true); + chassis.pid_turn_set(-143, 80, true); Little_Mech_Mac.set(0); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(24, 80, true); + chassis.pid_drive_set(23.7, 80, true); pros::delay(650); Little_Mech_Mac.set(1); chassis.pid_wait_quick_chain(); + Little_Mech_Mac.set(0); chassis.pid_turn_set(179, 80, true); chassis.pid_wait_quick_chain(); @@ -783,40 +775,48 @@ void solo_right (){ chassis.pid_wait_quick(); chassis.pid_turn_set(132, 80, true); + pros::delay(200); + intake_top.move(-30); + intake_top_score.move(-30); + bottom_intake(-30); chassis.pid_wait_quick_chain(); - // Little_Mech_Mac.set(1); - chassis.pid_drive_set(-10, 60, true); - chassis.pid_wait(); - + pros::delay(400); intake_top.move(127); intake_top_score.move(-127); + bottom_intake(127); + chassis.pid_wait(); - pros::delay(750); + pros::delay(950); - chassis.pid_drive_set(39, 90, true); + intake_top.move(0); + bottom_intake(0); + + chassis.pid_drive_set(40, 90, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(1); + intake_top_score.move(127); - chassis.pid_drive_set(11, 60, true); + chassis.pid_drive_set(10.5, 60, true); + intake_top.move(127); + intake_top_score + .move(127); + bottom_intake(127); + Little_Mech_Mac.set(1); chassis.pid_wait(); pros::delay(350); - chassis.pid_drive_set(-25, 75, true); - pros::delay(500); - trapdoor.set(1); + chassis.pid_drive_set(-26, 75, true); chassis.pid_wait(); + trapdoor.set(0); pros::delay(1100); - chassis.pid_drive_set(12, 60, true); - chassis.pid_wait(); } @@ -887,7 +887,7 @@ void skills_before_changing_the_wall() { pros::Task task1(controller_update); //pros::Task color_sort_task_running(color_sort_S); drive_wall(510); - discore_mech.set(1); + //discore_mech.set(1); chassis.odom_xyt_set(0_in, 0_in, -90_deg); trapdoor.set(0); color = "x"; @@ -1235,401 +1235,338 @@ void skills_before_changing_the_wall() { chassis.pid_wait_quick(); } -// 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 -// */ - -// // 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(); - -// chassis.pid_drive_set(11.5, 90, true); -// chassis.pid_wait(); - -// //intake_bottom.move(127); -// //intake_top.move(127); -// chassis.pid_drive_set(13, 60, true); -// pros::delay(1000); -// /* -// Back up from match loader -// */ - -// Little_Mech_Mac.set(0); -// pros::delay(100); +void skills() { -// chassis.pid_drive_set(-9.8, 90, true); -// chassis.pid_wait_quick(); + //grab middle balls -// bottom_intake(0); -// top_intake(0); + trapdoor.set(1); + intake_top.move(80); + intake_bottom.move(127); + intake_top_score.move(127); -// chassis.pid_turn_set(-90, 60, true); -// chassis.pid_wait(); + chassis.pid_turn_set(-44, 80, true); + chassis.pid_wait_quick_chain(); -// /* -// Relocate to blue side -// */ + chassis.pid_drive_set(28, 80, true); + pros::delay(400); + Little_Mech_Mac.set(1); + chassis.pid_wait(); -// chassis.pid_odom_set({{-45_in, 0_in}, fwd, 100}, true); -// chassis.pid_wait(); + chassis.pid_drive_set(-3, 80, true); + chassis.pid_wait_quick_chain(); -// chassis.pid_turn_set(0, 60, true); -// chassis.pid_wait(); + //score middle balls + Little_Mech_Mac.set(0); + chassis.pid_turn_set(-140, 80, true); + chassis.pid_wait_quick_chain(); -// /* -// Reset location to 0, 0 -// */ + chassis.pid_drive_set(-14, 60, true); + chassis.pid_wait_quick_chain(); -// 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); + intake_top.move(127); + intake_bottom.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; + //Little_Mech_Mac.set(0); -// p_x = position_x; -// p_y = position_y; + pros::delay(1500); -// chassis.odom_xy_set(position_x, position_y); -// pros::delay(150); + //line up with first match loader -// /* -// Setup for match loader -// */ - -// chassis.pid_turn_set(90, 60, true); -// chassis.pid_wait(); + chassis.pid_drive_set(25, 80, 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_wait(); -// /* -// Score first match loader -// */ + intake_top_score.move(127); -// Little_Mech_Mac.set(1); + chassis.pid_turn_set(-90, 80, true); + chassis.pid_wait_quick_chain(); -// chassis.pid_turn_set(0, 60, true); -// chassis.pid_wait(); + chassis.pid_drive_set(9.5, 80, true); + chassis.pid_wait_quick_chain(); -// top_intake(-20); -// bottom_intake(-20); -// //intake_top.move(127); -// //intake_bottom.move(127); + chassis.pid_turn_set(-179, 60, true); + chassis.pid_wait_quick_chain(); -// chassis.pid_drive_set(-16, 90, true); -// chassis.pid_wait_quick(); + Little_Mech_Mac.set(1); -// top_intake(127); -// bottom_intake(127); + trapdoor.set(1); -// chassis.pid_drive_set(-100, 10, true); -// trapdoor.set(1); -// pros::delay(100); -// trapdoor.set(1); -// pros::delay(1900); + chassis.pid_drive_set(25, 60, true); + chassis.pid_wait(); + pros::delay(600); -// /* -// empty second match loader -// */ + //cross to other side -// chassis.pid_drive_set(45.5, 100, true); -// chassis.pid_wait(); + chassis.pid_drive_set(-10, 80, true); + chassis.pid_wait_quick_chain(); -// 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); + Little_Mech_Mac.set(0); -// chassis.pid_drive_set(-29.5, 60, true); -// chassis.pid_wait(); + chassis.pid_turn_set(-90, 80, true); + chassis.pid_wait_quick_chain(); -// Little_Mech_Mac.set(0); + chassis.pid_drive_set(7/*5.5*/, 80, true); + chassis.pid_wait_quick_chain(); -// top_intake(-15); -// bottom_intake(-15); + chassis.pid_turn_set(1, 80, true); + chassis.pid_wait_quick_chain(); -// chassis.pid_drive_set(-5, 20, true); -// pros::delay(150); + intake_top.move(0); + intake_bottom.move(0); + intake_top_score.move(0); -// top_intake(127); -// bottom_intake(127); + chassis.pid_drive_set(78, 80, true); + chassis.pid_wait(); -// trapdoor.set(1); + //score first match loader -// pros::delay(1000); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(5, 80, true); + chassis.pid_wait_quick_chain(); -// /* -// Set up for first 2 middle balls -// */ + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait_quick_chain(); -// // Activate color sort - + chassis.pid_drive_set(-10, 80, true); -// chassis.pid_drive_set(10, 110, true); -// chassis.pid_wait(); - -// chassis.pid_turn_set(88, 80, true); -// chassis.pid_wait(); -// trapdoor.set(0); + pros::delay(100); + intake_top.move(-40); + intake_bottom.move(-40); + intake_top_score.move(-40); + chassis.pid_wait(); -// // chassis.pid_odom_set({{26.8, -10.5}, fwd, 80}, true); -// // chassis.pid_wait_quick(); + trapdoor.set(0); -// chassis.pid_drive_set(80, 80, true); -// chassis.pid_wait(); + intake_top.move(127); + intake_bottom.move(127); + intake_top_score.move(127); -// chassis.pid_turn_set(-1.5, 60); -// chassis.pid_wait(); + pros::delay(1500); -// 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); + //grab second match loader - -// //drive_wall(450); + Little_Mech_Mac.set(1); -// chassis.pid_turn_set(88.5, 60); -// chassis.pid_wait(); + trapdoor.set(1); -// 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(28, 80, true); + chassis.pid_wait(); -// //drive_wall(450); + pros::delay(600); -// chassis.pid_turn_set(-3, 60); -// chassis.pid_wait(); + //score second match loader -// pros::delay(150); + chassis.pid_turn_set(2, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-35, 60, true); + pros::delay(800); + intake_top.move(-30); + intake_bottom.move(-30); + intake_top_score.move(-30); + chassis.pid_wait(); -// /* -// reset position -// */ + trapdoor.set(0); + Little_Mech_Mac.set(0); + intake_top.move(127); + intake_bottom.move(127); + intake_top_score.move(127); + pros::delay(2000); -// 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; + //The part were we line up for clearing -// p_x = position_x_1; -// p_y = position_y_1; + chassis.pid_drive_set(8, 80, true); + chassis.pid_wait_quick_chain(); -// pros::delay(150); + trapdoor.set(1); -// chassis.odom_xy_set(position_x_1, position_y_1); -// chassis.pid_wait(); + chassis.pid_turn_set(50, 80, true); + chassis.pid_wait_quick_chain(); -// /* -// grab third match load -// */ + chassis.pid_swing_set(LEFT_SWING, 90, 85, 60, true); + chassis.pid_wait_quick_chain(); -// Little_Mech_Mac.set(1); + pros::delay(300); + chassis.pid_drive_set(-2.5, 60, true); + chassis.pid_wait(); + pros::delay(100); + Little_Mech_Mac.set(1); + pros::delay(200); -// chassis.pid_drive_set(24, 60, true); -// chassis.pid_wait(); - -// top_intake(127); -// bottom_intake(127); + intake_bottom.move(127); + chassis.pid_drive_set(60, 127, true); + chassis.pid_wait(); -// //intake_top.move(127); -// //intake_bottom.move(127); + pros::delay(100); + Little_Mech_Mac.set(0); -// chassis.pid_drive_set(13, 60, true); -// pros::delay(1000); + pros::delay(300); -// chassis.pid_drive_set(-20, 60, true); -// chassis.pid_wait(); + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait_quick_chain(); -// Little_Mech_Mac.set(0); -// top_intake(0); -// bottom_intake(0); -// chassis.pid_turn_set(-88, 60, true); -// chassis.pid_wait(); + while (distance_front.get_distance() < 600){ + chassis.pid_drive_set(-1000000, 40); + } + L1.brake(); + L2.brake(); + L3.brake(); + R1.brake(); + R2.brake(); + R3.brake(); -// chassis.pid_drive_set(-11.9, 60, true); -// chassis.pid_wait(); + chassis.pid_turn_set(90, 60, true); + chassis.pid_wait_quick_chain(); -// /* -// go to blue side -// */ + while (distance_front.get_distance() > 600){ + chassis.pid_drive_set(1000000, 40); + } + L1.brake(); + L2.brake(); + L3.brake(); + R1.brake(); + R2.brake(); + R3.brake(); -// chassis.pid_turn_set(-3, 60, true); -// chassis.pid_wait_quick(); + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait_quick_chain(); -// chassis.pid_drive_set(-85, 80, true); -// chassis.pid_wait(); + Little_Mech_Mac.set(1); -// chassis.pid_turn_set(86, 60, true); -// chassis.pid_wait(); + chassis.pid_drive_set(20, 60, true); + chassis.pid_wait(); +//c/p from above -// chassis.pid_drive_set(-11, 80, true); -// chassis.pid_wait(); + pros::delay(600); -// /* -// score third set of match load -// */ + //cross to other side -// chassis.pid_turn_set(176, 60, true); -// chassis.pid_wait(); + chassis.pid_drive_set(-10, 80, true); + chassis.pid_wait_quick_chain(); -// chassis.pid_drive_set(-19.5, 60, true); -// chassis.pid_wait(); + Little_Mech_Mac.set(0); -// top_intake(127); -// bottom_intake(127); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); -// trapdoor.set(1); + chassis.pid_drive_set(5.5, 80, true); + chassis.pid_wait_quick_chain(); -// pros::delay(2000); + chassis.pid_turn_set(179, 80, true); + chassis.pid_wait_quick_chain(); -// /* -// empty second match loader -// */ + chassis.pid_drive_set(78, 80, true); + chassis.pid_wait(); - - -// Little_Mech_Mac.set(1); + //score first match loader -// 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); + chassis.pid_turn_set(-90, 80, true); + chassis.pid_wait_quick_chain(); -// /* -// Score second 6 on long goal -// */ + chassis.pid_drive_set(5, 80, true); + chassis.pid_wait_quick_chain(); -// Little_Mech_Mac.set(0); + chassis.pid_turn_set(180, 80, true); + chassis.pid_wait_quick_chain(); -// pros::delay(100); + chassis.pid_drive_set(-10, 80, true); + pros::delay(200); + intake_top.move(-40); + intake_bottom.move(-40); + intake_top_score.move(-40); + chassis.pid_wait(); -// chassis.pid_turn_set(179, 60, true); + trapdoor.set(0); -// chassis.pid_drive_set(-33.5, 60, true); -// chassis.pid_wait(); + intake_top.move(127); + intake_bottom.move(127); + intake_top_score.move(127); -// top_intake(127); -// bottom_intake(127); + pros::delay(1500); -// trapdoor.set(1); + //grab second match loader -// pros::delay(2000); + Little_Mech_Mac.set(1); -// chassis.odom_xyt_set(0, 0, 180); + trapdoor.set(1); -// chassis.pid_drive_set(5, 127, true); -// chassis.pid_wait(); + chassis.pid_drive_set(28, 80, true); + chassis.pid_wait(); -// chassis.pid_turn_set(-95, 127, true); -// chassis.pid_wait(); + pros::delay(600); -// chassis.pid_drive_set(48, 127, true); -// chassis.pid_wait(); + //score second match loader + + chassis.pid_drive_set(-35, 60, true); + pros::delay(800); + intake_top.move(-40); + intake_bottom.move(-40); + intake_top_score.move(-40); + chassis.pid_wait(); -// chassis.pid_turn_set(178, 127, true); -// chassis.pid_wait(); + trapdoor.set(0); -// chassis.pid_drive_set(100, 127, false); -// chassis.pid_wait_quick(); + intake_top.move(127); + intake_bottom.move(127); + intake_top_score.move(127); -// } + trapdoor.set(0); + Little_Mech_Mac.set(0); + pros::delay(1500); + + //score in middle goal + trapdoor.set(1); -void skills() { - chassis.pid_drive_set(21, 90, true); + chassis.pid_drive_set(12, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(90, 80, true); + chassis.pid_turn_set(-46, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(10.1, 70, true); - Little_Mech_Mac.set(1); - intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); + chassis.pid_drive_set(45, 80, true); chassis.pid_wait(); - pros::delay(1000); + //The hard stop is broken, power on once we have it + //intake_piston.set(1); - chassis.pid_drive_set(-26, 75, true); - pros::delay(200); - trapdoor.set(1); + intake_bottom.move(0); + intake_top.move(0); + intake_top_score.move(0); + + chassis.pid_drive_set(6, 60, true); chassis.pid_wait(); - Little_Mech_Mac.set(0); + intake_bottom.move(-127); + intake_top.move(-127); + intake_top_score.move(-127); - pros::delay(2500); + pros::delay(1000); + + chassis.pid_drive_set(-15, 127, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(10, 80, true); - chassis.pid_wait(); + chassis.pid_turn_set(-180, 127, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(105, 80, true); - chassis.pid_wait(); + chassis.pid_drive_set(19, 127, true); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 170, 127, 35, true); - chassis.pid_wait(); + chassis.pid_swing_set(LEFT_SWING, -95, 85, 0, true); + chassis.pid_wait_quick_chain(); Little_Mech_Mac.set(1); + pros::delay(100); + intake_bottom.move(127); - chassis.pid_drive_set(50, 127, true); + chassis.pid_drive_set(42, 127, true); chassis.pid_wait(); + + } /* old diff --git a/src/main.cpp b/src/main.cpp index a83eb98..4594e17 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -31,8 +31,10 @@ ez::Drive chassis( // ez::tracking_wheel vert_tracker(-12, 2, 0); 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,11 @@ 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; + +void color_sort_top() { color_sort.set_integration_time(3); while (true) { int hue_lower; @@ -110,6 +113,36 @@ void color_sort_S() { continue; } + bool in_proximity = color_sort.get_proximity() > 220; + + if (in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)) { + control_to_controller = false; + intake_top_score.move(-127); + intake_top.move(30); + pros::delay(300); + control_to_controller = true; + + } + } +} + + +void color_sort_bottom() { + color_sort.set_integration_time(3); + while (true) { + int hue_lower; + int hue_higher; + color_sort.set_led_pwm(100); + if (color == "B") { + hue_lower = 180; + hue_higher = 260; + } else if (color == "R") { + hue_lower = 0; + hue_higher = 30; + } else { + continue; + } + bool in_proximity = color_sort.get_proximity() > 50; 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) { @@ -122,6 +155,7 @@ void color_sort_S() { } void initialize() { + discore_mech.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 @@ -136,7 +170,7 @@ void initialize() { pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"Left Safe", skills}, + {"solo_right", skills}, {"Right Safe", skills_before_changing_the_wall}, {"Skills", left_safe}, {"Left Side Solo", left_elims_quick}, @@ -179,7 +213,10 @@ void odom_reset(){ void disabled() { } -void competition_initialize() { } +void competition_initialize() { + + discore_mech.set(1); + } void autonomous() { chassis.pid_targets_reset(); @@ -265,7 +302,7 @@ void opcontrol() { int count = 0; bool intake_auto_reverse_enabled = false; // pros::Task anti_jam_T(anti_jam); - // pros::Task color_sort_task_running (color_sort_S); + pros::Task color_sort_task_running (color_sort_top); color = "x"; while (true) { chassis.opcontrol_arcade_standard(ez::SPLIT); @@ -279,8 +316,8 @@ void opcontrol() { } else if (master.get_digital(DIGITAL_L2)) { intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); + if (control_to_controller)(intake_top.move(127)); + if (control_to_controller)(intake_top_score.move(127)); } else if (master.get_digital(DIGITAL_R1)) { @@ -295,13 +332,13 @@ void opcontrol() { intake_piston.set(1); } - else { - intake_bottom.move(0); - intake_top.move(0); - intake_top_score.move(0); - intake_piston.set(0); - } - // } + else if (control_to_controller){ + intake_bottom.move(0); + intake_top.move(0); + intake_top_score.move(0); + intake_piston.set(0); + } + // } // else { // intake_bottom.move(-40); // intake_top.move(-60); @@ -310,10 +347,10 @@ void opcontrol() { if (master.get_digital(DIGITAL_RIGHT)) { - trapdoor.set(1); + trapdoor.set(0); } else { - trapdoor.set(0); + trapdoor.set(1); } if (master.get_digital(DIGITAL_Y)) { @@ -329,11 +366,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)) { @@ -343,6 +381,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) @@ -370,7 +412,7 @@ void opcontrol() { intake_back = "N"; } - master.print(0, 0, "%d/%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 ", /*L1.get_temperature(int)color_sort.get_hue()*//*anti_jam_is_working(int)vertical_tracker.get_position()/100 */dt_temps , color_sort.get_proximity()/*top_temp*/, bottom_temp, color, intake_back); } count++; From e1c7ab26059b794098d216773b33a0cf002e6291 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Fri, 23 Jan 2026 18:55:46 -0800 Subject: [PATCH 04/15] one auton done --- include/autons.hpp | 6 +- include/main.h | 1 + include/subsystems.hpp | 53 ++--- src/autons.cpp | 487 ++++++++++++++++++++++++++--------------- src/main.cpp | 54 +++-- 5 files changed, 387 insertions(+), 214 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index a9ccd78..f99b302 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -24,6 +24,8 @@ void left_elims(); void red_top_elims(); void blue_top_quals(); +void left_elims_7ball(); + void intake_test(); void blue_bottom_elims(); @@ -35,6 +37,8 @@ void new_elim_auton(); void solo_left(); +void pid_test(); + /* Odom TESTING Functions */ void odom_pure_pursuit_wait_until_example(); @@ -51,7 +55,7 @@ void wall_alignment_test(); void pid_tune(); /* safe routes */ -void left_safe(); +void left_middle_top(); void right_safe(); /* old */ 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 4c0bf86..88b75c5 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -15,39 +15,42 @@ inline pros::Controller master(pros::E_CONTROLLER_MASTER); extern Drive chassis; -inline pros::Motor L1(-11); -inline pros::Motor L2(-15); -inline pros::Motor L3(-16); +inline pros::Motor L1(-6); +inline pros::Motor L2(-5); +inline pros::Motor L3(-8); -inline pros::Motor R1(5); -inline pros::Motor R2(8); -inline pros::Motor R3(21); +inline pros::Motor R1(12); +inline pros::Motor R2(19); +inline pros::Motor R3(20); -inline pros::Imu inertial(1); +inline pros::Imu inertial(11); -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(13); +inline pros::Distance distance_back_l(13); // removed sensor +inline pros::Distance distance_front_l(9); +inline pros::Distance distance_back_r(17); // removed sensor +inline pros::Distance distance_front_r(13); // removed sensor +inline pros::Distance distance_front(13); // removed sensor -inline pros::Optical color_sort(9); +inline pros::Optical color_sort(16); -inline pros::Motor intake_bottom(7); -inline pros::Motor intake_top(-17); -inline pros::Motor intake_top_score(-20); +inline pros::Motor intake_bottom(21); +inline pros::Motor intake_top(-2); +inline pros::Motor intake_top_score(-10); +inline pros::Motor vertical_tracker('Z'); -inline ez::Piston trapdoor('A'); -inline ez::Piston middle_stage('C'); -inline ez::Piston Little_Mech_Mac('C'); -inline ez::Piston color_sort_piston('D'); -inline ez::Piston intake_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('B'); +inline ez::Piston mid_descore('G'); -inline ez::Piston discore_mech('B'); +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/src/autons.cpp b/src/autons.cpp index e6080f9..6d9fbbe 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -64,7 +64,8 @@ void intake_counter_spin(){ } -int top_stage_intake = 0; +int top_speed_intake = 0; +int top_speed_score_intake = 0; int bottom_stage_intake = 0; bool change = false; void anti_jam_auton(){ @@ -99,6 +100,34 @@ void anti_jam_auton(){ } } +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)) { + intake_top_score.move(-127); + intake_top.move(30); + pros::delay(300); + intake_top.move(top_speed_intake); + intake_top_score.move(top_speed_score_intake); + } + } +} + void timer(int timeout){ if (timeout > pros::millis()/100){ L1.brake(); @@ -116,8 +145,15 @@ 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); @@ -125,10 +161,12 @@ void bottom_intake(int speed_bt){ } + void default_constants() { - chassis.pid_drive_constants_set(22, 0, 130); + chassis.pid_drive_constants_set(22, 0, 150); 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_turn_constants_set(3.2, 0, 22, 12.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); @@ -193,72 +231,158 @@ void intake_test(){ } // FINISHED -void left_safe(){ +void left_middle_top(){ chassis.odom_xyt_set(0_in, 0_in, -30_deg); trapdoor.set(1); - intake_bottom.move(127); + intake_bottom.move(127); intake_top.move(127); - chassis.pid_drive_set(24.8, 100, true); - pros::delay(450); + chassis.pid_drive_set(24.8, 80, true); + pros::delay(500); Little_Mech_Mac.set(true); chassis.pid_wait_quick(); Little_Mech_Mac.set(0); + pros::delay(400); + + // chassis.pid_drive_set(-2, 80, true); + // chassis.pid_wait(); + chassis.pid_turn_set(-135, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-13, 80, true); + chassis.pid_drive_set(-16, 80, true); chassis.pid_wait(); intake_top_score.move(-127); - pros::delay(900); + pros::delay(1200); - chassis.pid_drive_set(37, 80, true); + chassis.pid_drive_set(42, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 80, true); + chassis.pid_turn_set(179, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(14, 70, true); + chassis.pid_drive_set(15.7, 70, true); Little_Mech_Mac.set(1); intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); + top_intake(127); + top_intake_score(127); + //intake_top.move(127); + //intake_top_score.move(127); chassis.pid_wait(); - pros::delay(370); + pros::delay(200); + + chassis.pid_turn_set(179, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-26, 75, true); - pros::delay(600); + chassis.pid_drive_set(-27.8, 75, true); + intake_bottom.move(-20); + pros::delay(300); + // pros::Task color_sort_safe(color_sort_top_auton); trapdoor.set(0); + intake_bottom.move(127); chassis.pid_wait(); - pros::delay(1600); + Little_Mech_Mac.set(0); + + pros::delay(1100); + + chassis.pid_swing_set(RIGHT_SWING, 100, 80, -10, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); + + right_rush_mech.set(1); + + chassis.pid_drive_set(-20, 80, true); + chassis.pid_wait_quick(); +} + + +void left_elims_7ball(){ + trapdoor.set(1); + chassis.odom_xyt_set(0_in, 0_in, -30_deg); + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_drive_set(24.8, 100, true); + pros::delay(500); + Little_Mech_Mac.set(true); + chassis.pid_wait_quick(); Little_Mech_Mac.set(0); - chassis.pid_drive_set(5, 80, true); + pros::delay(150); + + chassis.pid_turn_set(-130, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(90, 90, true); + chassis.pid_drive_set(21, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(6.9, 90, true); + chassis.pid_turn_set(180, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(0, 80, true); + chassis.pid_drive_set(18.5, 80, true); + Little_Mech_Mac.set(1); + chassis.pid_wait(); + pros::delay(150); + + chassis.pid_drive_set(-35, 100, true); + + + // int hue_lower = 210; + // int hue_higher = 250; + // int current_time = pros::millis(); + // bool in_proximity = color_sort.get_proximity() > 50; + // while ((pros::millis() - current_time < 1800) || (in_proximity && !(hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher))){ + // intake_bottom.move(127); + // intake_top.move(127); + // intake_top_score.move(127); + // } + + intake_bottom.move(-20); + top_intake(127); + top_intake_score(127); + //intake_top.move(127); + //intake_top_score.move(127); + pros::delay(450); + intake_bottom.move(127); + trapdoor.set(0); + // pros::Task color_sort_left(color_sort_top_auton); + pros::delay(2200); + Little_Mech_Mac.set(0); + + chassis.pid_drive_set(3, 100, true); + chassis.pid_wait_quick_chain(); + + trapdoor.set(1); + + chassis.pid_turn_set(-90, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(27, 60, true); + chassis.pid_swing_set(LEFT_SWING, 180, 80, 0, true); chassis.pid_wait_quick_chain(); + + right_rush_mech.set(1); + + chassis.pid_drive_set(-23, 80, true); + chassis.pid_wait_quick(); + + chassis.pid_turn_set(-160, 10, false); } // FINISHED void left_elims_quick(){ + trapdoor.set(1); chassis.odom_xyt_set(0_in, 0_in, -30_deg); @@ -266,34 +390,22 @@ void left_elims_quick(){ intake_top.move(127); intake_top_score.move(127); - chassis.pid_drive_set(25, 100, true); + chassis.pid_drive_set(24.8, 100, true); pros::delay(500); Little_Mech_Mac.set(true); - chassis.pid_wait_quick_chain(); + chassis.pid_wait_quick(); Little_Mech_Mac.set(0); - chassis.pid_turn_set(-145, 80, true); + chassis.pid_turn_set(-130, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(29, 80, true); + chassis.pid_drive_set(21, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 80, true); + chassis.pid_turn_set(180, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(15.1, 70, true); - Little_Mech_Mac.set(1); - intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); - chassis.pid_wait(); - - pros::delay(50); - - chassis.pid_drive_set(-26, 75, true); - pros::delay(600); - trapdoor.set(0); - chassis.pid_wait(); + chassis.pid_drive_set(-15, 100, true); // int hue_lower = 210; @@ -304,23 +416,37 @@ void left_elims_quick(){ // intake_bottom.move(127); // intake_top.move(127); // intake_top_score.move(127); - // } + // } + + intake_bottom.move(-20); + top_intake(127); + top_intake_score(127); + //intake_top.move(127); + //intake_top_score.move(127); + pros::delay(200); intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); - pros::delay(2200); + trapdoor.set(0); + pros::delay(1600); Little_Mech_Mac.set(0); - chassis.pid_drive_set(10.5, 80, true); + chassis.pid_drive_set(3, 100, true); chassis.pid_wait_quick_chain(); + trapdoor.set(1); - pros::delay(200); + chassis.pid_turn_set(-90, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_swing_set(LEFT_SWING, 180, 80, 0, true); + chassis.pid_wait_quick_chain(); + right_rush_mech.set(1); - chassis.pid_drive_set(-15, 80, true); + chassis.pid_drive_set(-23, 80, true); chassis.pid_wait_quick(); + chassis.pid_turn_set(-160, 10, false); + @@ -328,8 +454,7 @@ void left_elims_quick(){ /* old -void right_safe1(){ - // get 3 middle balls +- // get 3 middle balls //pros::Task controller (controller_update); chassis.pid_odom_set({{0_in, 14_in}, fwd, 100}, true); @@ -430,69 +555,76 @@ void right_safe1(){ void right_safe(){ trapdoor.set(1); - chassis.pid_drive_set(21, 90, true); + chassis.pid_drive_set(22, 90, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(10.2, 70, true); - Little_Mech_Mac.set(1); + //Going into the machloader + Little_Mech_Mac.set(1); + chassis.pid_drive_set(11, 60, true); intake_bottom.move(127); intake_top.move(127); intake_top_score.move(127); chassis.pid_wait(); - pros::delay(350); + //intaking the balls from the machloader - chassis.pid_drive_set(-26, 75, true); - pros::delay(250); - trapdoor.set(0); + pros::delay(300); + + + chassis.pid_drive_set(-28.5, 75, true); chassis.pid_wait(); + Little_Mech_Mac.set(0); + //Scoring the balls + trapdoor.set(0); pros::delay(1300); - // chassis.pid_swing_set(LEFT_SWING, -135, 127, -20, true); - // chassis.pid_wait_quick(); - - chassis.pid_drive_set(7, 80, true); + chassis.pid_drive_set(9, 80, true); chassis.pid_wait_quick_chain(); trapdoor.set(1); - chassis.pid_turn_set(-142, 80, true); - Little_Mech_Mac.set(0); - chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(-138.5, 80, true); - chassis.pid_drive_set(24, 80, true); - pros::delay(650); - Little_Mech_Mac.set(1); - chassis.pid_wait(); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(20.5, 40, true); - pros::delay(100); - Little_Mech_Mac.set(0); + //Intaking the balls + chassis.pid_drive_set(30, 60, true); chassis.pid_wait(); - intake_bottom.move(-90); - intake_top.move(-90); - intake_top_score.move(-127); - - intake_piston.set(1); - - pros::delay(1800); + pros::delay(1000); - chassis.pid_drive_set(-24, 80, true); - chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(20, 80, true); + intake_piston.set(1); - chassis.pid_turn_set(90, 80, true); - chassis.pid_wait_quick_chain(); + pros::delay(100); + intake_bottom.move(-70); + intake_top.move(-100); + intake_top_score.move(-100); + //Little_Mech_Mac.set(1); + chassis.pid_wait(); - chassis.pid_drive_set(-20, 60, true); - chassis.pid_wait_quick_chain(); + //Scoring in the middle goal + + pros::delay(700); + intake_piston.set(0); + chassis.pid_drive_set(-34, 127, true); + chassis.pid_wait(); + chassis.pid_turn_set(-92, 80, true); + chassis.pid_wait(); + + chassis.pid_drive_set(21, 127, true); + chassis.pid_wait(); + + chassis.pid_turn_set(-135, 80, true); + chassis.pid_wait(); + } @@ -733,16 +865,16 @@ void solo_right (){ chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(10.8, 70, true); + chassis.pid_drive_set(11.2, 70, true); Little_Mech_Mac.set(1); bottom_intake(127); intake_top.move(127); intake_top_score.move(127); chassis.pid_wait(); - pros::delay(350); + pros::delay(200); - chassis.pid_drive_set(-26, 75, true); + chassis.pid_drive_set(-26.5, 75, true); chassis.pid_wait(); trapdoor.set(0); @@ -788,7 +920,7 @@ void solo_right (){ bottom_intake(127); chassis.pid_wait(); - pros::delay(950); + pros::delay(850); intake_top.move(0); bottom_intake(0); @@ -801,17 +933,16 @@ void solo_right (){ intake_top_score.move(127); - chassis.pid_drive_set(10.5, 60, true); + chassis.pid_drive_set(10.8, 60, true); intake_top.move(127); - intake_top_score - .move(127); + intake_top_score.move(127); bottom_intake(127); Little_Mech_Mac.set(1); chassis.pid_wait(); - pros::delay(350); + pros::delay(200); - chassis.pid_drive_set(-26, 75, true); + chassis.pid_drive_set(-26.5, 75, true); chassis.pid_wait(); trapdoor.set(0); @@ -1237,11 +1368,10 @@ void skills_before_changing_the_wall() { } void skills() { - + discore_mech.set(0); //grab middle balls - trapdoor.set(1); - intake_top.move(80); + intake_top.move(127); intake_bottom.move(127); intake_top_score.move(127); @@ -1252,6 +1382,8 @@ void skills() { pros::delay(400); Little_Mech_Mac.set(1); chassis.pid_wait(); + // TO make shure that we are grabbing all 4 balls + pros::delay(100); chassis.pid_drive_set(-3, 80, true); chassis.pid_wait_quick_chain(); @@ -1260,6 +1392,9 @@ void skills() { Little_Mech_Mac.set(0); chassis.pid_turn_set(-140, 80, true); chassis.pid_wait_quick_chain(); + //Moving the intake backwards to prevent jamming + intake_top.move(-10); + intake_top_score.move(-10); chassis.pid_drive_set(-14, 60, true); chassis.pid_wait_quick_chain(); @@ -1292,9 +1427,11 @@ void skills() { trapdoor.set(1); - chassis.pid_drive_set(25, 60, true); + chassis.pid_drive_set(25.6, 60, true); chassis.pid_wait(); - pros::delay(600); + + chassis.pid_drive_set(1000, 60, true); + pros::delay(1100); //cross to other side @@ -1306,7 +1443,7 @@ void skills() { chassis.pid_turn_set(-90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(7/*5.5*/, 80, true); + chassis.pid_drive_set(6.5, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(1, 80, true); @@ -1324,7 +1461,7 @@ void skills() { chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(5, 80, true); + chassis.pid_drive_set(4.7, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(0, 80, true); @@ -1333,9 +1470,9 @@ void skills() { chassis.pid_drive_set(-10, 80, true); pros::delay(100); - intake_top.move(-40); - intake_bottom.move(-40); - intake_top_score.move(-40); + intake_top.move(-20); + intake_bottom.move(-20); + intake_top_score.move(30); chassis.pid_wait(); trapdoor.set(0); @@ -1344,7 +1481,7 @@ void skills() { intake_bottom.move(127); intake_top_score.move(127); - pros::delay(1500); + pros::delay(1700); //grab second match loader @@ -1352,21 +1489,22 @@ void skills() { trapdoor.set(1); - chassis.pid_drive_set(28, 80, true); + chassis.pid_drive_set(28.6, 80, true); chassis.pid_wait(); - pros::delay(600); + chassis.pid_drive_set(1000, 60, true); + pros::delay(1100); //score second match loader chassis.pid_turn_set(2, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-35, 60, true); + chassis.pid_drive_set(-35.5, 60, true); pros::delay(800); intake_top.move(-30); intake_bottom.move(-30); - intake_top_score.move(-30); + intake_top_score.move(30); chassis.pid_wait(); trapdoor.set(0); @@ -1378,32 +1516,37 @@ void skills() { //The part were we line up for clearing - chassis.pid_drive_set(8, 80, true); + chassis.pid_drive_set(8, 127, true); chassis.pid_wait_quick_chain(); - trapdoor.set(1); - chassis.pid_turn_set(50, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 90, 85, 60, true); + chassis.pid_drive_set(19.5, 127, true); chassis.pid_wait_quick_chain(); - pros::delay(300); - chassis.pid_drive_set(-2.5, 60, true); - chassis.pid_wait(); - pros::delay(100); - Little_Mech_Mac.set(1); - pros::delay(200); + chassis.pid_swing_set(LEFT_SWING, 87, 85, 0, true); + chassis.pid_wait_quick_chain(); + + // chassis.pid_drive_set(5, 127, true); + // chassis.pid_wait_quick_chain(); + + intake_bottom.move(127); - chassis.pid_drive_set(60, 127, true); + chassis.pid_drive_set(70, 127, true); + pros::delay(250); + Little_Mech_Mac.set(1); + pros::delay(800); + Little_Mech_Mac.set(0); chassis.pid_wait(); - pros::delay(100); Little_Mech_Mac.set(0); + chassis.pid_turn_set(95, 127, true); + pros::delay(200); + Little_Mech_Mac.set(0); + pros::delay(100); - pros::delay(300); chassis.pid_turn_set(0, 80, true); chassis.pid_wait_quick_chain(); @@ -1437,14 +1580,17 @@ void skills() { Little_Mech_Mac.set(1); - chassis.pid_drive_set(20, 60, true); + chassis.pid_drive_set(20.6, 60, true); chassis.pid_wait(); -//c/p from above + trapdoor.set(1); + + chassis.pid_drive_set(1000, 60, true); + pros::delay(1000); - pros::delay(600); //cross to other side + chassis.pid_drive_set(-10, 80, true); chassis.pid_wait_quick_chain(); @@ -1459,6 +1605,10 @@ void skills() { chassis.pid_turn_set(179, 80, true); chassis.pid_wait_quick_chain(); + intake_top.move(0); + intake_bottom.move(0); + intake_top_score.move(0); + chassis.pid_drive_set(78, 80, true); chassis.pid_wait(); @@ -1477,7 +1627,7 @@ void skills() { pros::delay(200); intake_top.move(-40); intake_bottom.move(-40); - intake_top_score.move(-40); + intake_top_score.move(40); chassis.pid_wait(); trapdoor.set(0); @@ -1486,7 +1636,7 @@ void skills() { intake_bottom.move(127); intake_top_score.move(127); - pros::delay(1500); + pros::delay(1700); //grab second match loader @@ -1494,18 +1644,20 @@ void skills() { trapdoor.set(1); - chassis.pid_drive_set(28, 80, true); + chassis.pid_drive_set(28.6, 80, true); chassis.pid_wait(); - pros::delay(600); + chassis.pid_drive_set(1000, 60, true); + pros::delay(1000); + //score second match loader - chassis.pid_drive_set(-35, 60, true); + chassis.pid_drive_set(-35.8, 60, true); pros::delay(800); - intake_top.move(-40); - intake_bottom.move(-40); - intake_top_score.move(-40); + intake_top.move(-20); + intake_bottom.move(-20); + intake_top_score.move(20); chassis.pid_wait(); trapdoor.set(0); @@ -1516,55 +1668,38 @@ void skills() { trapdoor.set(0); Little_Mech_Mac.set(0); - pros::delay(1500); + pros::delay(2000); - //score in middle goal - trapdoor.set(1); - chassis.pid_drive_set(12, 80, true); + //The part were we line up for parking + + chassis.pid_drive_set(8, 127, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-46, 80, true); + chassis.pid_turn_set(-130, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(45, 80, true); - chassis.pid_wait(); - - //The hard stop is broken, power on once we have it - //intake_piston.set(1); - - intake_bottom.move(0); - intake_top.move(0); - intake_top_score.move(0); - - chassis.pid_drive_set(6, 60, true); - chassis.pid_wait(); - - intake_bottom.move(-127); - intake_top.move(-127); - intake_top_score.move(-127); - - pros::delay(1000); - - chassis.pid_drive_set(-15, 127, true); + chassis.pid_drive_set(19.5, 127, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-180, 127, true); + chassis.pid_swing_set(LEFT_SWING, -93, 85, 0, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(19, 127, true); - chassis.pid_wait_quick_chain(); + // chassis.pid_drive_set(5, 127, true); + // chassis.pid_wait_quick_chain(); + - chassis.pid_swing_set(LEFT_SWING, -95, 85, 0, true); - chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(1); - pros::delay(100); intake_bottom.move(127); + chassis.pid_drive_set(45, 127, true); + pros::delay(250); + Little_Mech_Mac.set(1); + pros::delay(1000); - chassis.pid_drive_set(42, 127, true); chassis.pid_wait(); + Little_Mech_Mac.set(0); + } @@ -1946,10 +2081,16 @@ void auton_setup_right(){ } /* TESTS */ -void empty(){ - pros::delay(100); - L1.brake(); +void pid_test(){ + + chassis.pid_drive_set(24, 127, true); + chassis.pid_wait(); + + chassis.pid_drive_set(-12, 127, true); + chassis.pid_wait(); + chassis.pid_drive_set(-12, 127, true); + chassis.pid_wait(); } void wall_tracking_test() { drive_wall(450); @@ -1966,9 +2107,11 @@ void wall_alignment_test() { R3.brake(); } void pid_tune(){ - pros::Task controller (controller_update); - chassis.pid_drive_set(odom_scaling * 24, 90, true); - chassis.pid_wait(); + pros::Task controller (color_sort_top_auton); + intake_bottom.move(127); + top_intake(127); + top_intake_score(127); + } void color_sort_test(){ diff --git a/src/main.cpp b/src/main.cpp index 4594e17..7f1eb9f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -20,15 +20,15 @@ */ ez::Drive chassis( - {-11, -15, -16}, //left - {5, 8, 21}, //right - 1, + {-6, -5, -8}, //left + {12, 19, 20}, //right + 11, 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; @@ -98,14 +98,14 @@ std::string color = "x"; // against R or B; press UP+X to change; x for disabled bool control_to_controller = true; void color_sort_top() { - color_sort.set_integration_time(3); + 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 = 250; + hue_higher = 240; } else if (color == "R") { hue_lower = 0; hue_higher = 10; @@ -155,7 +155,15 @@ void color_sort_bottom() { } void initialize() { + + // Set the color of the balls you want to throw out here + + color = "R"; + + + 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 @@ -170,9 +178,10 @@ void initialize() { pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"solo_right", skills}, - {"Right Safe", skills_before_changing_the_wall}, - {"Skills", left_safe}, + {"3 4 push", left_middle_top}, + {"left side 7 ball", left_elims_7ball}, + {"left side 4 push", left_elims_quick}, + {"Skills", left_elims_quick}, {"Left Side Solo", left_elims_quick}, {"Left Elims Quick", left_elims_quick}, {"Right Elims Quick", right_elims_quick}, @@ -183,8 +192,13 @@ void initialize() { }); 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() { @@ -215,7 +229,8 @@ void disabled() { } void competition_initialize() { - discore_mech.set(1); + // discore_mech.set(1); + // intake_piston.set(1); } void autonomous() { @@ -303,7 +318,7 @@ void opcontrol() { bool intake_auto_reverse_enabled = false; // pros::Task anti_jam_T(anti_jam); pros::Task color_sort_task_running (color_sort_top); - color = "x"; + //color = "B"; while (true) { chassis.opcontrol_arcade_standard(ez::SPLIT); @@ -315,6 +330,7 @@ void opcontrol() { } else if (master.get_digital(DIGITAL_L2)) { + intake_piston.set(0); intake_bottom.move(127); if (control_to_controller)(intake_top.move(127)); if (control_to_controller)(intake_top_score.move(127)); @@ -323,7 +339,12 @@ void opcontrol() { else if (master.get_digital(DIGITAL_R1)) { intake_bottom.move(127); intake_top.move(127); - intake_top_score.move(-75); + intake_top_score.move(-60); + } + else if (master.get_digital(DIGITAL_A)) { + intake_bottom.move(127); + intake_top.move(60); + intake_top_score.move(-40); } else if (master.get_digital(DIGITAL_R2)) { intake_bottom.move(-127); @@ -354,10 +375,10 @@ void opcontrol() { } if (master.get_digital(DIGITAL_Y)) { - middle_stage.set(0); + mid_descore.set(1); } else { - middle_stage.set(1); + mid_descore.set(0); } if (master.get_digital(DIGITAL_B)) { @@ -406,13 +427,14 @@ void opcontrol() { 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 = ""; if (intake_auto_reverse_enabled){ intake_back = "N"; } - master.print(0, 0, "%d/%d/%d/%s ", /*L1.get_temperature(int)color_sort.get_hue()*//*anti_jam_is_working(int)vertical_tracker.get_position()/100 */dt_temps , color_sort.get_proximity()/*top_temp*/, bottom_temp, color, intake_back); + master.print(0, 0, "%d/%d/%d/%s ", /*L1.get_temperature(int)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, intake_back); } count++; From 1a24fc090adfe0ef40d4dba5d0602d2c7491eaff Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Fri, 6 Feb 2026 16:13:08 -0800 Subject: [PATCH 05/15] prepare for norcal --- include/autons.hpp | 2 + include/subsystems.hpp | 4 +- project.pros | 2 +- src/autons.cpp | 515 +++++++++++++++++++++++++++++++++-------- src/main.cpp | 103 +++++---- 5 files changed, 486 insertions(+), 140 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index f99b302..ec70d90 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -19,6 +19,7 @@ void empty(); void skills(); void skills_before_changing_the_wall(); void skills_without_odom(); +void new_skills(); void left_elims(); void red_top_elims(); @@ -47,6 +48,7 @@ void square_odom_test(); void auton_setup_left(); void auton_setup_right(); void solo_right(); +void elims_mid_control(); /* Wall Tracking TEST */ diff --git a/include/subsystems.hpp b/include/subsystems.hpp index 88b75c5..f451195 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -28,8 +28,8 @@ inline pros::Imu inertial(11); inline pros::Distance distance_back_l(13); // removed sensor inline pros::Distance distance_front_l(9); inline pros::Distance distance_back_r(17); // removed sensor -inline pros::Distance distance_front_r(13); // removed sensor -inline pros::Distance distance_front(13); // removed sensor +inline pros::Distance distance_front_r(99); // removed sensor +inline pros::Distance distance_front(99); // removed sensor inline pros::Optical color_sort(16); diff --git a/project.pros b/project.pros index a06e614..e42173b 100644 --- a/project.pros +++ b/project.pros @@ -425,7 +425,7 @@ }, "upload_options": { "description": "roboticsisez.com", - "icon": "clawbot", + "icon": "ufo", "slot": 1 }, "use_early_access": false diff --git a/src/autons.cpp b/src/autons.cpp index 6d9fbbe..cd42add 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -66,8 +66,9 @@ void intake_counter_spin(){ int top_speed_intake = 0; int top_speed_score_intake = 0; -int bottom_stage_intake = 0; +int bottom_speed_intake = 0; bool change = false; + void anti_jam_auton(){ float spin_time = 200; int v_threshold_top; @@ -75,28 +76,46 @@ void anti_jam_auton(){ int current_top; int velocity_bottom; int current_bottom; - int current_threshold = 2000; - 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){ velocity_bottom = intake_bottom.get_actual_velocity(); + velocity_top = intake_top.get_actual_velocity(); if (change){pros::delay(300); change = false;} current_bottom = intake_bottom.get_current_draw(); + current_top = intake_top.get_current_draw(); + + if (current_bottom > current_threshold && abs(velocity_bottom) < 10){ + is_jammed_bottom = true; + } - if (current_bottom > current_threshold && velocity_bottom < 10){ - is_jammed_fwd = 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_bottom.move(bottom_stage_intake); + is_jammed_top = false; + intake_top.move(bottom_speed_intake); pros::delay(300); } + + // This one checks the bottom motor + if (is_jammed_bottom){ + float start_time = pros::millis(); + while ( ((float)pros::millis() - start_time) < spin_time){ + intake_bottom.move(-bottom_speed_intake*127); + } + is_jammed_bottom = false; + intake_bottom.move(top_speed_intake); + pros::delay(300); + } + } } @@ -157,7 +176,7 @@ void top_intake_score(int 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; } @@ -232,6 +251,11 @@ void intake_test(){ // FINISHED void left_middle_top(){ + //Colorsort color set + color = "B"; + pros::Task color_sor(color_sort_top_auton); + + chassis.odom_xyt_set(0_in, 0_in, -30_deg); trapdoor.set(1); @@ -239,9 +263,9 @@ void left_middle_top(){ intake_bottom.move(127); intake_top.move(127); - chassis.pid_drive_set(24.8, 80, true); + chassis.pid_drive_set(25.5, 80, true); //24.8 with little bill activation pros::delay(500); - Little_Mech_Mac.set(true); + //temp test Little_Mech_Mac.set(true); chassis.pid_wait_quick(); Little_Mech_Mac.set(0); @@ -256,17 +280,17 @@ void left_middle_top(){ chassis.pid_drive_set(-16, 80, true); chassis.pid_wait(); - intake_top_score.move(-127); + intake_top_score.move(-100); pros::delay(1200); - chassis.pid_drive_set(42, 80, true); + chassis.pid_drive_set(38.5, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(179, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(15.7, 70, true); + chassis.pid_drive_set(12.5, 70, true); Little_Mech_Mac.set(1); intake_bottom.move(127); top_intake(127); @@ -275,24 +299,23 @@ void left_middle_top(){ //intake_top_score.move(127); chassis.pid_wait(); - pros::delay(200); + chassis.pid_drive_set(6.8, 50, true); - chassis.pid_turn_set(179, 80, true); - chassis.pid_wait_quick_chain(); + pros::delay(850); chassis.pid_drive_set(-27.8, 75, true); intake_bottom.move(-20); pros::delay(300); // pros::Task color_sort_safe(color_sort_top_auton); - trapdoor.set(0); intake_bottom.move(127); chassis.pid_wait(); + trapdoor.set(0); Little_Mech_Mac.set(0); - pros::delay(1100); + pros::delay(1400); - chassis.pid_swing_set(RIGHT_SWING, 100, 80, -10, true); + chassis.pid_swing_set(RIGHT_SWING, 68, 90, 2.5, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(180, 100, true); @@ -300,8 +323,11 @@ void left_middle_top(){ right_rush_mech.set(1); - chassis.pid_drive_set(-20, 80, true); + chassis.pid_drive_set(-28, 80, true); chassis.pid_wait_quick(); + + chassis.pid_turn_set(-145, 60, true); + } @@ -553,34 +579,32 @@ void left_elims_quick(){ void right_safe(){ - trapdoor.set(1); - chassis.pid_drive_set(22, 90, true); + color = "R"; + 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(90, 80, true); chassis.pid_wait_quick_chain(); - //Going into the machloader - Little_Mech_Mac.set(1); - chassis.pid_drive_set(11, 60, true); - intake_bottom.move(127); + 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(); - //intaking the balls from the machloader - - pros::delay(300); + pros::delay(400); - - chassis.pid_drive_set(-28.5, 75, true); + chassis.pid_drive_set(-26.8, 75, true); chassis.pid_wait(); - Little_Mech_Mac.set(0); - - //Scoring the balls trapdoor.set(0); - pros::delay(1300); + + pros::delay(1100); + trapdoor.set(1); + Little_Mech_Mac.set(0); chassis.pid_drive_set(9, 80, true); chassis.pid_wait_quick_chain(); @@ -609,7 +633,7 @@ void right_safe(){ //Scoring in the middle goal - pros::delay(700); + pros::delay(900); intake_piston.set(0); @@ -619,7 +643,7 @@ void right_safe(){ chassis.pid_turn_set(-92, 80, true); chassis.pid_wait(); - chassis.pid_drive_set(21, 127, true); + chassis.pid_drive_set(22, 127, true); chassis.pid_wait(); chassis.pid_turn_set(-135, 80, true); @@ -853,19 +877,119 @@ void solo_left1() { pros::delay(100); +} + +void elims_mid_control (){ + + color = "B"; + pros::Task color_sor(color_sort_top_auton); + + trapdoor.set(1); + + chassis.pid_drive_set(21, 90, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); + + //Going into the machloader + Little_Mech_Mac.set(1); + chassis.pid_drive_set(14.5, 80, true); + intake_bottom.move(80); + intake_top.move(127); + intake_top_score.move(127); + chassis.pid_wait(); + + pros::delay(200); + + //intaking the balls from the machloader + + chassis.pid_drive_set(-28.5, 75, true); + chassis.pid_wait(); + Little_Mech_Mac.set(0); + + //Scoring the balls + trapdoor.set(0); + pros::delay(1100); + + chassis.pid_drive_set(9, 80, true); + chassis.pid_wait_quick_chain(); + trapdoor.set(1); + + chassis.pid_turn_set(-138.5, 80, true); + + chassis.pid_wait_quick_chain(); + + //Intaking the balls + chassis.pid_drive_set(30, 80, true); + pros::delay(650); + chassis.pid_drive_set(12, 60, true); + chassis.pid_wait(); + + Little_Mech_Mac.set(0); + + chassis.pid_drive_set(20, 60, true); + chassis.pid_wait(); + pros::delay(100); + intake_bottom.move(-100); + intake_top.move(-127); + intake_top_score.move(0); + //Scoring in the middle goal + + + pros::delay(1000); + + intake_piston.set(0); + + chassis.pid_drive_set(-18, 127, true); + chassis.pid_wait(); + + intake_bottom.move(127); + intake_top.move(100); + intake_top_score.move(100); + + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(43, 127, true); + pros::delay(700); + chassis.pid_drive_set(21, 60, true); + chassis.pid_wait(); + + chassis.pid_turn_set(132, 80, true); + chassis.pid_wait_quick(); + + chassis.pid_drive_set(-15, 80, true); + chassis.pid_wait(); + intake_top_score.move(-127); + pros::delay(1000); + + chassis.pid_drive_set(8, 80, true); + chassis.pid_wait(); + pros::delay(100); + mid_descore.set(1); + + chassis.pid_drive_set(-15, 40, true); + pros::delay(800); + chassis.pid_drive_set(0, 0, true); + + + } //done void solo_right (){ //pros::Task anti_jam_auton1 (anti_jam_auton); + color = "R"; + pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); - chassis.pid_drive_set(21, 90, true); + chassis.pid_drive_set(22.6, 90, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(11.2, 70, true); + chassis.pid_drive_set(12.3, 70, true); Little_Mech_Mac.set(1); bottom_intake(127); intake_top.move(127); @@ -874,18 +998,18 @@ void solo_right (){ pros::delay(200); - chassis.pid_drive_set(-26.5, 75, true); + chassis.pid_drive_set(-26.8, 75, true); chassis.pid_wait(); trapdoor.set(0); pros::delay(1000); + trapdoor.set(1); // chassis.pid_swing_set(LEFT_SWING, -135, 127, -20, true); // chassis.pid_wait_quick(); chassis.pid_drive_set(5, 80, true); chassis.pid_wait_quick_chain(); - trapdoor.set(1); chassis.pid_turn_set(-143, 80, true); Little_Mech_Mac.set(0); @@ -900,27 +1024,27 @@ void solo_right (){ chassis.pid_turn_set(179, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(48, 65, true); + chassis.pid_drive_set(47, 65, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-5.6, 80, true); + chassis.pid_drive_set(-6.5, 80, true); chassis.pid_wait_quick(); - chassis.pid_turn_set(132, 80, true); + chassis.pid_turn_set(135, 80, true); pros::delay(200); intake_top.move(-30); intake_top_score.move(-30); bottom_intake(-30); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-10, 60, true); + chassis.pid_drive_set(-14.8, 60, true); pros::delay(400); intake_top.move(127); intake_top_score.move(-127); bottom_intake(127); chassis.pid_wait(); - pros::delay(850); + pros::delay(500); intake_top.move(0); bottom_intake(0); @@ -933,7 +1057,7 @@ void solo_right (){ intake_top_score.move(127); - chassis.pid_drive_set(10.8, 60, true); + chassis.pid_drive_set(14, 60, true); intake_top.move(127); intake_top_score.move(127); bottom_intake(127); @@ -942,7 +1066,7 @@ void solo_right (){ pros::delay(200); - chassis.pid_drive_set(-26.5, 75, true); + chassis.pid_drive_set(-27, 75, true); chassis.pid_wait(); trapdoor.set(0); @@ -1367,15 +1491,126 @@ void skills_before_changing_the_wall() { } +void new_skills(){ + discore_mech.set(0); + trapdoor.set(1); + intake_piston.set(0); + + intake_top.move(127); + intake_bottom.move(127); + intake_top_score.move(127); + + chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 100_ms, 100_ms); + + //start wiggle + + chassis.pid_turn_set(5, 30, true); + chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(-5 , 30, true); + chassis.pid_wait_quick_chain(); + pros::delay(200); + chassis.pid_turn_set(0, 60, true); + chassis.pid_wait_quick_chain(); + + //drive in and back out and back in + + chassis.pid_drive_set(5, 100, true); + chassis.pid_wait_quick_chain(); + + //wiggle part 2 + + chassis.pid_turn_set(5, 30, true); + chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(-5 , 30, true); + chassis.pid_wait_quick_chain(); + pros::delay(200); + chassis.pid_turn_set(0, 60, true); + chassis.pid_wait_quick_chain(); + + //back out + + chassis.pid_drive_set(-30, 80, true); + chassis.pid_wait_quick_chain(); + + //line up on the wall + + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait(); + + chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms,500_ms); + + + while (distance_front_l.get_distance() < 800){ + chassis.pid_drive_set(-1000000, -40); + } + L1.brake(); + L2.brake(); + L3.brake(); + R1.brake(); + R2.brake(); + R3.brake(); + + pros::delay(200); + + //line up on mid goal + + chassis.pid_turn_set(-25, 80, true); + chassis.pid_wait(); + + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 200_ms, 300_ms); + + + chassis.pid_drive_set(-16, 80, 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, 45, -80, 0, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(45, 80, true); + chassis.pid_wait(); + + chassis.pid_drive_set(-5, 80, true); + chassis.pid_wait_quick_chain(); + + /*7 ball score*/ + intake_top_score.move(-80); + chassis.pid_drive_set(1, 60, true); + chassis.pid_wait(); + pros::delay(2000); + + //grab 7th ball + + chassis.pid_drive_set(5, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-4.5, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(1, 60, true); + chassis.pid_wait(); + +} + + + + + + void skills() { discore_mech.set(0); //grab middle balls trapdoor.set(1); intake_top.move(127); intake_bottom.move(127); - intake_top_score.move(127); + intake_top_score.move(100); - chassis.pid_turn_set(-44, 80, true); + chassis.pid_drive_set(2, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(-46, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_drive_set(28, 80, true); @@ -1385,12 +1620,12 @@ void skills() { // TO make shure that we are grabbing all 4 balls pros::delay(100); - chassis.pid_drive_set(-3, 80, true); + chassis.pid_drive_set(-2, 80, true); chassis.pid_wait_quick_chain(); //score middle balls Little_Mech_Mac.set(0); - chassis.pid_turn_set(-140, 80, true); + chassis.pid_turn_set(-135, 80, true); chassis.pid_wait_quick_chain(); //Moving the intake backwards to prevent jamming intake_top.move(-10); @@ -1401,7 +1636,7 @@ void skills() { intake_top.move(127); intake_bottom.move(127); - intake_top_score.move(-127); + intake_top_score.move(-100); //Little_Mech_Mac.set(0); @@ -1414,24 +1649,42 @@ void skills() { intake_top_score.move(127); - chassis.pid_turn_set(-90, 80, true); + chassis.pid_turn_set(180, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(9.5, 80, true); + while (distance_front_l.get_distance() > 600){ + chassis.pid_drive_set(1000000, 40); + } + L1.brake(); + L2.brake(); + L3.brake(); + R1.brake(); + R2.brake(); + R3.brake(); + + chassis.pid_turn_set(-90, 60, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-179, 60, true); + while (distance_front_l.get_distance() > 600){ + chassis.pid_drive_set(1000000, 40); + } + L1.brake(); + L2.brake(); + L3.brake(); + R1.brake(); + R2.brake(); + R3.brake(); + + chassis.pid_turn_set(180, 80, true); chassis.pid_wait_quick_chain(); Little_Mech_Mac.set(1); trapdoor.set(1); - chassis.pid_drive_set(25.6, 60, true); + chassis.pid_drive_set(13, 60, true); chassis.pid_wait(); - - chassis.pid_drive_set(1000, 60, true); - pros::delay(1100); + pros::delay(750); //cross to other side @@ -1461,13 +1714,13 @@ void skills() { chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(4.7, 80, true); + chassis.pid_drive_set(4.2, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(0, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-10, 80, true); + chassis.pid_drive_set(-13, 80, true); pros::delay(100); intake_top.move(-20); @@ -1489,18 +1742,16 @@ void skills() { trapdoor.set(1); - chassis.pid_drive_set(28.6, 80, true); + chassis.pid_drive_set(28, 60, true); chassis.pid_wait(); - - chassis.pid_drive_set(1000, 60, true); - pros::delay(1100); + pros::delay(750); //score second match loader chassis.pid_turn_set(2, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-35.5, 60, true); + chassis.pid_drive_set(-35.5, 80, true); pros::delay(800); intake_top.move(-30); intake_bottom.move(-30); @@ -1508,34 +1759,40 @@ void skills() { chassis.pid_wait(); trapdoor.set(0); - Little_Mech_Mac.set(0); intake_top.move(127); intake_bottom.move(127); intake_top_score.move(127); pros::delay(2000); + Little_Mech_Mac.set(0); //The part were we line up for clearing chassis.pid_drive_set(8, 127, true); chassis.pid_wait_quick_chain(); + trapdoor.set(1); + chassis.pid_turn_set(50, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(19.5, 127, true); + chassis.pid_drive_set(20, 127, true); chassis.pid_wait_quick_chain(); chassis.pid_swing_set(LEFT_SWING, 87, 85, 0, true); chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(86, 80, true); + chassis.pid_wait_quick_chain(); + // chassis.pid_drive_set(5, 127, true); // chassis.pid_wait_quick_chain(); - - intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + chassis.pid_drive_set(70, 127, true); - pros::delay(250); + pros::delay(300); Little_Mech_Mac.set(1); pros::delay(800); Little_Mech_Mac.set(0); @@ -1552,7 +1809,7 @@ void skills() { chassis.pid_wait_quick_chain(); - while (distance_front.get_distance() < 600){ + while (distance_front_l.get_distance() < 600){ chassis.pid_drive_set(-1000000, 40); } L1.brake(); @@ -1565,7 +1822,7 @@ void skills() { chassis.pid_turn_set(90, 60, true); chassis.pid_wait_quick_chain(); - while (distance_front.get_distance() > 600){ + while (distance_front_l.get_distance() > 600){ chassis.pid_drive_set(1000000, 40); } L1.brake(); @@ -1579,14 +1836,11 @@ void skills() { chassis.pid_wait_quick_chain(); Little_Mech_Mac.set(1); - - chassis.pid_drive_set(20.6, 60, true); - chassis.pid_wait(); trapdoor.set(1); - chassis.pid_drive_set(1000, 60, true); - pros::delay(1000); - + chassis.pid_drive_set(20.5, 60, true); + chassis.pid_wait(); + pros::delay(1050); //cross to other side @@ -1623,7 +1877,7 @@ void skills() { chassis.pid_turn_set(180, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-10, 80, true); + chassis.pid_drive_set(-12, 80, true); pros::delay(200); intake_top.move(-40); intake_bottom.move(-40); @@ -1644,12 +1898,9 @@ void skills() { trapdoor.set(1); - chassis.pid_drive_set(28.6, 80, true); + chassis.pid_drive_set(27.5, 80, true); chassis.pid_wait(); - - chassis.pid_drive_set(1000, 60, true); - pros::delay(1000); - + pros::delay(1450); //score second match loader @@ -1666,7 +1917,7 @@ void skills() { intake_bottom.move(127); intake_top_score.move(127); - trapdoor.set(0); + trapdoor.set(1); Little_Mech_Mac.set(0); pros::delay(2000); @@ -1704,6 +1955,7 @@ void skills() { } + /* old void new_elim_auton(){ // pros::Task anti (anti_jam_auton); @@ -2107,14 +2359,89 @@ void wall_alignment_test() { R3.brake(); } void pid_tune(){ - pros::Task controller (color_sort_top_auton); - intake_bottom.move(127); + //pros::Task controller (color_sort_top_auton); + //pros::Task controller1 (anti_jam_auton); + int intake1 = 60; + trapdoor.set(1); + bottom_intake(127); top_intake(127); top_intake_score(127); + + pros::delay(3000); - + top_intake(-50); + top_intake_score(-50); + pros::delay(600); + top_intake_score(0); + top_intake(-80); + pros::delay(300); + //0 pauses + top_intake(intake1); + top_intake_score(-40); + + + // //1 pause + // top_intake(0); + // top_intake_score(0); + // pros::delay(200); + // //Continue + // top_intake(intake1); + // top_intake_score(-40); + // pros::delay(300); + // //second pause + // top_intake(0); + // top_intake_score(0); + // pros::delay(200); + // //Continue + // top_intake(intake1); + // top_intake_score(-40); + // pros::delay(300); + // //third pause + // top_intake(0); + // top_intake_score(0); + // pros::delay(200); + // //Continue + // top_intake(intake1); + // top_intake_score(-40); + // pros::delay(300); + // //fourth pause + // top_intake(0); + // top_intake_score(0); + // pros::delay(200); + // //Continue + // top_intake(intake1); + // top_intake_score(-40); + // pros::delay(300); + // //fifth pause + // top_intake(0); + // top_intake_score(0); + // pros::delay(200); + // //Continue + // top_intake(intake1); + // top_intake_score(-40); + // pros::delay(300); + // // sixth pause + // top_intake(0); + // top_intake_score(0); + // pros::delay(200); + // //Continue + // top_intake(intake1); + // top_intake_score(-40); + // pros::delay(300); + // //seventh pause + // top_intake(0); + // top_intake_score(0); + // pros::delay(200); + // //Continue + // top_intake(intake1); + // top_intake_score(-40); + // pros::delay(300); + // //final stop + // top_intake(0); + // top_intake_score(0); } void color_sort_test(){ + color = "B"; pros::Task color_sor(color_sort_S); pros::Task antij(anti_jam_auton); top_intake(120); diff --git a/src/main.cpp b/src/main.cpp index 7f1eb9f..863b2eb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -94,9 +94,9 @@ void anti_jam(){ } } -std::string color = "x"; // against R or B; press UP+X to change; x for disabled +std::string color = "R"; // against R or B; press UP+X to change; x for disabled bool control_to_controller = true; - +int middgoal_Srore = 0; void color_sort_top() { color_sort.set_integration_time(10); while (true) { @@ -113,52 +113,43 @@ void color_sort_top() { continue; } + if (master.get_digital(DIGITAL_R1)){ + middgoal_Srore = 1; + } else { + middgoal_Srore = 0; + } + bool in_proximity = color_sort.get_proximity() > 220; - if (in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)) { + if (middgoal_Srore == 0 && in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)) { control_to_controller = false; intake_top_score.move(-127); intake_top.move(30); pros::delay(300); control_to_controller = true; + } else if (middgoal_Srore == 1 && in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)){ + 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 color_sort_bottom() { - color_sort.set_integration_time(3); - while (true) { - int hue_lower; - int hue_higher; - color_sort.set_led_pwm(100); - if (color == "B") { - hue_lower = 180; - hue_higher = 260; - } else if (color == "R") { - hue_lower = 0; - hue_higher = 30; - } else { - continue; - } - - bool in_proximity = color_sort.get_proximity() > 50; - 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; - } - } -} void initialize() { // Set the color of the balls you want to throw out here color = "R"; + intake_piston.set(1); @@ -178,8 +169,10 @@ void initialize() { pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"3 4 push", left_middle_top}, - {"left side 7 ball", left_elims_7ball}, + {"right safe", new_skills}, + {"right solo", solo_right}, + {"elims auton 3 goals", elims_mid_control}, + {"elims left", left_elims_7ball}, {"left side 4 push", left_elims_quick}, {"Skills", left_elims_quick}, {"Left Side Solo", left_elims_quick}, @@ -321,7 +314,6 @@ void opcontrol() { //color = "B"; while (true) { chassis.opcontrol_arcade_standard(ez::SPLIT); - if (master.get_digital(DIGITAL_L1)) { intake_bottom.move(-127); @@ -338,25 +330,31 @@ void opcontrol() { } else if (master.get_digital(DIGITAL_R1)) { intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(-60); - } - else if (master.get_digital(DIGITAL_A)) { - intake_bottom.move(127); - intake_top.move(60); - intake_top_score.move(-40); + if (control_to_controller)(intake_top.move(127)); + if (control_to_controller)(intake_top_score.move(-60)); } + // else if (master.get_digital(DIGITAL_A)) { + // intake_bottom.move(127); + // intake_top.move(60); + // intake_top_score.move(-40); + // } else if (master.get_digital(DIGITAL_R2)) { - intake_bottom.move(-127); + intake_bottom.move(-50); intake_top.move(-127); intake_top_score.move(-127); intake_piston.set(1); - } - + } else if (control_to_controller){ intake_bottom.move(0); intake_top.move(0); intake_top_score.move(0); + //intake_piston.set(0); + } + + if (master.get_digital(DIGITAL_R2)) { + intake_piston.set(1); + } + else{ intake_piston.set(0); } // } @@ -371,7 +369,7 @@ void opcontrol() { trapdoor.set(0); } else { - trapdoor.set(1); + if(control_to_controller)(trapdoor.set(1)); } if (master.get_digital(DIGITAL_Y)) { @@ -395,6 +393,9 @@ void opcontrol() { discore_mech.set(0); } + if (master.get_digital_new_press(DIGITAL_A)) { + intake_piston.set(!intake_piston.get()); + } if (master.get_digital_new_press(DIGITAL_X)) { Digital_X += 1; if (Digital_X == 4){Digital_X = 1;} @@ -430,11 +431,27 @@ void opcontrol() { 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, "%d/%d/%d/%s ", /*L1.get_temperature(int)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, intake_back); + + + + + master.print(0, 0, "%d/%d/%d/%s/%d ", /*L1.get_temperature(int)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++; From 6881bb17738891075f32b141f89ec62e6c6b9145 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Fri, 6 Feb 2026 22:02:07 -0800 Subject: [PATCH 06/15] pre-anti jam test --- include/autons.hpp | 1 + src/autons.cpp | 158 +++++++++++++++++++++++++++++++-------------- src/main.cpp | 6 +- 3 files changed, 112 insertions(+), 53 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index ec70d90..8b827e2 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -20,6 +20,7 @@ void skills(); void skills_before_changing_the_wall(); void skills_without_odom(); void new_skills(); +void nor_call_skills(); void left_elims(); void red_top_elims(); diff --git a/src/autons.cpp b/src/autons.cpp index cd42add..f740347 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -1492,40 +1492,65 @@ void skills_before_changing_the_wall() { } void new_skills(){ - discore_mech.set(0); + discore_mech.set(0); trapdoor.set(1); intake_piston.set(0); - - intake_top.move(127); - intake_bottom.move(127); - intake_top_score.move(127); + /* Taking the balls from the side + chassis.pid_drive_set(-5, 100, true); + chassis.pid_wait(); + chassis.pid_drive_set(50, 65, true); + pros::delay(700); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); + Little_Mech_Mac.set(1); + chassis.pid_drive_set(10, 100, true); + chassis.pid_wait(); +*/ chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 100_ms, 100_ms); - //start wiggle - chassis.pid_turn_set(5, 30, true); - chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-5 , 30, true); - chassis.pid_wait_quick_chain(); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + /* Taking the working 6 balls*/ + chassis.pid_drive_set(10, 127, true); + pros::delay(800); + chassis.pid_drive_set(-3, 127, true); + pros::delay(200); + chassis.pid_drive_set(10, 127, true); + pros::delay(800); + chassis.pid_drive_set(-3, 127, true); pros::delay(200); - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(10, 127, true); + pros::delay(200); + chassis.pid_drive_set(-3, 127, true); + pros::delay(400); - //drive in and back out and back in + // //start wiggle - chassis.pid_drive_set(5, 100, true); - chassis.pid_wait_quick_chain(); + // chassis.pid_turn_set(5, 30, true); + // chassis.pid_wait_quick_chain(); + // chassis.pid_turn_set(-5 , 30, true); + // chassis.pid_wait_quick_chain(); + // pros::delay(200); + // chassis.pid_turn_set(0, 60, true); + // chassis.pid_wait_quick_chain(); - //wiggle part 2 + // //drive in and back out and back in - chassis.pid_turn_set(5, 30, true); - chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-5 , 30, true); - chassis.pid_wait_quick_chain(); - pros::delay(200); - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait_quick_chain(); + // chassis.pid_drive_set(5, 100, true); + // chassis.pid_wait_quick_chain(); + + // //wiggle part 2 + + // chassis.pid_turn_set(5, 30, true); + // chassis.pid_wait_quick_chain(); + // chassis.pid_turn_set(-5 , 30, true); + // chassis.pid_wait_quick_chain(); + // pros::delay(200); + // chassis.pid_turn_set(0, 60, true); + // chassis.pid_wait_quick_chain(); //back out @@ -1554,16 +1579,15 @@ void new_skills(){ //line up on mid goal - chassis.pid_turn_set(-25, 80, true); + chassis.pid_turn_set(-30, 80, true); chassis.pid_wait(); - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 200_ms, 300_ms); - + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); chassis.pid_drive_set(-16, 80, 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_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); chassis.pid_swing_set(RIGHT_SWING, 45, -80, 0, true); @@ -1575,22 +1599,35 @@ void new_skills(){ chassis.pid_drive_set(-5, 80, true); chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1, 80, true); /*7 ball score*/ - intake_top_score.move(-80); - chassis.pid_drive_set(1, 60, true); - chassis.pid_wait(); - pros::delay(2000); + top_intake(-50); + top_intake_score(-50); + pros::delay(600); + top_intake_score(0); + top_intake(-80); + pros::delay(300); + + for (int i = 0; i < 8; i++){ + top_intake(60); + top_intake_score(-40); + pros::delay(300); + top_intake(0); + top_intake_score(0); + pros::delay(100); + } + + top_intake(70); + top_intake_score(-35); //grab 7th ball - chassis.pid_drive_set(5, 80, true); + chassis.pid_drive_set(6.5, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-4.5, 80, true); + chassis.pid_drive_set(-5, 30, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(1, 60, true); - chassis.pid_wait(); } @@ -1600,6 +1637,7 @@ void new_skills(){ void skills() { + pros::Task task_anti_jam(anti_jam_auton); discore_mech.set(0); //grab middle balls trapdoor.set(1); @@ -1620,7 +1658,7 @@ void skills() { // TO make shure that we are grabbing all 4 balls pros::delay(100); - chassis.pid_drive_set(-2, 80, true); + chassis.pid_drive_set(-1, 80, true); chassis.pid_wait_quick_chain(); //score middle balls @@ -1792,7 +1830,7 @@ void skills() { intake_top_score.move(127); chassis.pid_drive_set(70, 127, true); - pros::delay(300); + pros::delay(200); Little_Mech_Mac.set(1); pros::delay(800); Little_Mech_Mac.set(0); @@ -1917,7 +1955,7 @@ void skills() { intake_bottom.move(127); intake_top_score.move(127); - trapdoor.set(1); + trapdoor.set(0); Little_Mech_Mac.set(0); pros::delay(2000); @@ -1930,7 +1968,7 @@ void skills() { chassis.pid_turn_set(-130, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(19.5, 127, true); + chassis.pid_drive_set(22.5, 127, true); chassis.pid_wait_quick_chain(); chassis.pid_swing_set(LEFT_SWING, -93, 85, 0, true); @@ -1943,7 +1981,7 @@ void skills() { intake_bottom.move(127); chassis.pid_drive_set(45, 127, true); - pros::delay(250); + pros::delay(200); Little_Mech_Mac.set(1); pros::delay(1000); @@ -1955,6 +1993,35 @@ void skills() { } +void nor_call_skills() { + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + /* Try #1*/ + chassis.pid_drive_set(10, 127, true); + pros::delay(1000); + chassis.pid_drive_set(-2, 127, true); + pros::delay(100); + chassis.pid_drive_set(10, 127, true); + pros::delay(1000); + chassis.pid_drive_set(-2, 127, true); + pros::delay(100); + chassis.pid_drive_set(10, 127, true); + pros::delay(500); + chassis.pid_drive_set(-2, 127, true); + pros::delay(100); + + + /*Try #2 + chassis.pid_swing_set(LEFT_SWING, 90, 127, 100, false); + pros::delay(1000); + chassis.pid_swing_set(RIGHT_SWING, -90, 127, 100, false); + pros::delay(1000); + chassis.pid_drive_set(10, 127, true); + pros::delay(5000); + */ + +} /* old void new_elim_auton(){ @@ -2367,17 +2434,8 @@ void pid_tune(){ top_intake(127); top_intake_score(127); - pros::delay(3000); + pros::delay(2000); - top_intake(-50); - top_intake_score(-50); - pros::delay(600); - top_intake_score(0); - top_intake(-80); - pros::delay(300); - //0 pauses - top_intake(intake1); - top_intake_score(-40); // //1 pause diff --git a/src/main.cpp b/src/main.cpp index 863b2eb..d8200c4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -149,7 +149,7 @@ void initialize() { // Set the color of the balls you want to throw out here color = "R"; - intake_piston.set(1); + //intake_piston.set(1); @@ -169,8 +169,8 @@ void initialize() { pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"right safe", new_skills}, - {"right solo", solo_right}, + {"right safe", skills}, + {"right solo", pid_tune}, {"elims auton 3 goals", elims_mid_control}, {"elims left", left_elims_7ball}, {"left side 4 push", left_elims_quick}, From a31b66f18f71e8ed442e2647c6c346afeed33c0b Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Sun, 8 Feb 2026 14:01:47 -0800 Subject: [PATCH 07/15] unfinished skills --- include/subsystems.hpp | 2 +- include/wall_tracking.hpp | 5 +- project.pros | 2 +- src/autons.cpp | 400 +++++++++++++++++--------------------- src/main.cpp | 16 +- src/wall_tracking.cpp | 56 ++++-- 6 files changed, 234 insertions(+), 247 deletions(-) diff --git a/include/subsystems.hpp b/include/subsystems.hpp index f451195..6f0e7f0 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -19,7 +19,7 @@ inline pros::Motor L1(-6); inline pros::Motor L2(-5); inline pros::Motor L3(-8); -inline pros::Motor R1(12); +inline pros::Motor R1(14); inline pros::Motor R2(19); inline pros::Motor R3(20); diff --git a/include/wall_tracking.hpp b/include/wall_tracking.hpp index d4623c6..6b9527f 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); +void chassis_drive_wall(float distance, float DRIVE_SPEED); +void chassis_brake(); +void drive_wall_task(); \ No newline at end of file diff --git a/project.pros b/project.pros index e42173b..05d07d4 100644 --- a/project.pros +++ b/project.pros @@ -1,7 +1,7 @@ { "py/object": "pros.conductor.project.Project", "py/state": { - "project_name": "Best Pog", + "project_name": "blue left 4", "target": "v5", "templates": { "EZ-Template": { diff --git a/src/autons.cpp b/src/autons.cpp index f740347..a527e56 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -333,6 +333,9 @@ void left_middle_top(){ void left_elims_7ball(){ + color = "R"; + pros::Task color_sor(color_sort_top_auton); + trapdoor.set(1); chassis.odom_xyt_set(0_in, 0_in, -30_deg); @@ -348,21 +351,22 @@ void left_elims_7ball(){ pros::delay(150); - chassis.pid_turn_set(-130, 100, true); + chassis.pid_turn_set(-135, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(21, 100, true); + chassis.pid_drive_set(23, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 100, true); + chassis.pid_turn_set(-180, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(18.5, 80, true); + chassis.pid_drive_set(15.5, 70, true); Little_Mech_Mac.set(1); chassis.pid_wait(); - pros::delay(150); + pros::delay(300); - chassis.pid_drive_set(-35, 100, true); + chassis.pid_drive_set(-30, 100, true); + chassis.pid_wait_quick_chain(); // int hue_lower = 210; @@ -374,25 +378,20 @@ void left_elims_7ball(){ // intake_top.move(127); // intake_top_score.move(127); // } + // pros::Task color_sort_left(color_sort_top_auton); - intake_bottom.move(-20); + bottom_intake(127); top_intake(127); top_intake_score(127); - //intake_top.move(127); - //intake_top_score.move(127); - pros::delay(450); - intake_bottom.move(127); trapdoor.set(0); - // pros::Task color_sort_left(color_sort_top_auton); pros::delay(2200); + trapdoor.set(1); Little_Mech_Mac.set(0); - chassis.pid_drive_set(3, 100, true); + chassis.pid_drive_set(3, 127, true); chassis.pid_wait_quick_chain(); - trapdoor.set(1); - - chassis.pid_turn_set(-90, 100, true); + chassis.pid_turn_set(-95, 100, true); chassis.pid_wait_quick_chain(); chassis.pid_swing_set(LEFT_SWING, 180, 80, 0, true); @@ -403,67 +402,48 @@ void left_elims_7ball(){ chassis.pid_drive_set(-23, 80, true); chassis.pid_wait_quick(); - chassis.pid_turn_set(-160, 10, false); + chassis.pid_turn_set(-160, 40, false); + chassis.pid_wait(); + + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); } // FINISHED void left_elims_quick(){ + color = "R"; + pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); - chassis.odom_xyt_set(0_in, 0_in, -30_deg); - - intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); - - chassis.pid_drive_set(24.8, 100, true); - pros::delay(500); - Little_Mech_Mac.set(true); - chassis.pid_wait_quick(); - Little_Mech_Mac.set(0); - - chassis.pid_turn_set(-130, 100, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(21, 100, true); + chassis.pid_drive_set(22.6, 90, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 100, true); + chassis.pid_turn_set(-90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-15, 100, true); + 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(); + pros::delay(400); - // int hue_lower = 210; - // int hue_higher = 250; - // int current_time = pros::millis(); - // bool in_proximity = color_sort.get_proximity() > 50; - // while ((pros::millis() - current_time < 1800) || (in_proximity && !(hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher))){ - // intake_bottom.move(127); - // intake_top.move(127); - // intake_top_score.move(127); - // } - - intake_bottom.move(-20); - top_intake(127); - top_intake_score(127); - //intake_top.move(127); - //intake_top_score.move(127); - pros::delay(200); - intake_bottom.move(127); + chassis.pid_drive_set(-26.8, 75, true); + chassis.pid_wait(); trapdoor.set(0); - pros::delay(1600); + + pros::delay(1100); + trapdoor.set(1); Little_Mech_Mac.set(0); chassis.pid_drive_set(3, 100, true); chassis.pid_wait_quick_chain(); - trapdoor.set(1); - - chassis.pid_turn_set(-90, 100, true); + chassis.pid_turn_set(175, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 180, 80, 0, true); + chassis.pid_swing_set(LEFT_SWING, 90, 80, 0, true); chassis.pid_wait_quick_chain(); right_rush_mech.set(1); @@ -471,9 +451,10 @@ void left_elims_quick(){ chassis.pid_drive_set(-23, 80, true); chassis.pid_wait_quick(); - chassis.pid_turn_set(-160, 10, false); - + chassis.pid_turn_set(70, 40, false); + chassis.pid_wait(); + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); } @@ -580,7 +561,7 @@ void left_elims_quick(){ void right_safe(){ - color = "R"; + color = "B"; pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); chassis.pid_drive_set(22.6, 90, true); @@ -931,7 +912,7 @@ void elims_mid_control (){ chassis.pid_drive_set(20, 60, true); chassis.pid_wait(); pros::delay(100); - intake_bottom.move(-100); + intake_bottom.move(-80); intake_top.move(-127); intake_top_score.move(0); //Scoring in the middle goal @@ -1492,121 +1473,87 @@ void skills_before_changing_the_wall() { } void new_skills(){ - discore_mech.set(0); - trapdoor.set(1); - intake_piston.set(0); - /* Taking the balls from the side - chassis.pid_drive_set(-5, 100, true); - chassis.pid_wait(); - chassis.pid_drive_set(50, 65, true); - pros::delay(700); - Little_Mech_Mac.set(1); - chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(1); - chassis.pid_drive_set(10, 100, true); - chassis.pid_wait(); -*/ - chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 100_ms, 100_ms); + chassis.odom_xyt_set(0_in, 0_in, -90_deg); + discore_mech.set(0); + trapdoor.set(1); + intake_piston.set(0); intake_bottom.move(127); intake_top.move(127); intake_top_score.move(127); /* Taking the working 6 balls*/ - chassis.pid_drive_set(10, 127, true); - pros::delay(800); - chassis.pid_drive_set(-3, 127, true); - pros::delay(200); - chassis.pid_drive_set(10, 127, true); - pros::delay(800); - chassis.pid_drive_set(-3, 127, true); - pros::delay(200); - chassis.pid_drive_set(10, 127, true); - pros::delay(200); - chassis.pid_drive_set(-3, 127, true); - pros::delay(400); - - // //start wiggle - - // chassis.pid_turn_set(5, 30, true); - // chassis.pid_wait_quick_chain(); - // chassis.pid_turn_set(-5 , 30, true); - // chassis.pid_wait_quick_chain(); - // pros::delay(200); - // chassis.pid_turn_set(0, 60, true); - // chassis.pid_wait_quick_chain(); - - // //drive in and back out and back in + chassis.pid_drive_set(2, 10, true); + chassis.pid_wait(); - // chassis.pid_drive_set(5, 100, true); - // chassis.pid_wait_quick_chain(); + pros::delay(400); - // //wiggle part 2 + chassis.pid_drive_set(50, 60, true); + pros::delay(500); + Little_Mech_Mac.set(1); + pros::delay(300); + Little_Mech_Mac.set(0); + chassis.pid_wait(); - // chassis.pid_turn_set(5, 30, true); - // chassis.pid_wait_quick_chain(); - // chassis.pid_turn_set(-5 , 30, true); - // chassis.pid_wait_quick_chain(); - // pros::delay(200); - // chassis.pid_turn_set(0, 60, true); - // chassis.pid_wait_quick_chain(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); - //back out + chassis.pid_drive_set(-15, 20, true); + chassis.pid_wait(); - chassis.pid_drive_set(-30, 80, true); - chassis.pid_wait_quick_chain(); + 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_set(0, 80, true); - chassis.pid_wait(); - chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms,500_ms); - - while (distance_front_l.get_distance() < 800){ - chassis.pid_drive_set(-1000000, -40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + chassis.pid_turn_set(180, 60, true); + chassis.pid_wait(); - pros::delay(200); + //Replaced by a new function + // while (distance_front_l.get_distance() < 500){ + // chassis.pid_drive_set(-1000000, -40); + // } + // L1.brake(); + // L2.brake(); + // L3.brake(); + // R1.brake(); + // R2.brake(); + // R3.brake(); - //line up on mid goal + chassis_drive_wall(500, 100); - chassis.pid_turn_set(-30, 80, true); + chassis.pid_turn_set(-88, 60, true); chassis.pid_wait(); - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); - - chassis.pid_drive_set(-16, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); + //Replaced by a new function + // while (distance_front_l.get_distance() < 1200){ + // chassis.pid_drive_set(-1000000, -40); + // } + // L1.brake(); + // L2.brake(); + // L3.brake(); + // R1.brake(); + // R2.brake(); + // R3.brake(); + chassis_drive_wall(1200, 100); - chassis.pid_swing_set(RIGHT_SWING, 45, -80, 0, true); - chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(176, 60, true); + chassis.pid_wait(); - chassis.pid_turn_set(45, 80, true); + chassis.pid_drive_set(-21.2, 80, true); chassis.pid_wait(); - chassis.pid_drive_set(-5, 80, true); - chassis.pid_wait_quick_chain(); + chassis.pid_swing_set(RIGHT_SWING, -127, 50, -15, true); + chassis.pid_wait(); - chassis.pid_drive_set(1, 80, true); - /*7 ball score*/ + chassis.pid_drive_set(-4, 80, true); top_intake(-50); top_intake_score(-50); - pros::delay(600); - top_intake_score(0); - top_intake(-80); - pros::delay(300); + chassis.pid_wait(); + +/*7 ball score*/ for (int i = 0; i < 8; i++){ top_intake(60); @@ -1622,13 +1569,14 @@ void new_skills(){ //grab 7th ball - chassis.pid_drive_set(6.5, 80, true); - chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(7.5, 80, true); + chassis.pid_wait(); chassis.pid_drive_set(-5, 30, true); chassis.pid_wait_quick_chain(); + } @@ -1638,12 +1586,13 @@ void new_skills(){ void skills() { pros::Task task_anti_jam(anti_jam_auton); + discore_mech.set(0); //grab middle balls trapdoor.set(1); - intake_top.move(127); - intake_bottom.move(127); - intake_top_score.move(100); + top_intake(127); + bottom_intake(127); + top_intake_score(127); chassis.pid_drive_set(2, 80, true); chassis.pid_wait_quick_chain(); @@ -1666,15 +1615,15 @@ void skills() { chassis.pid_turn_set(-135, 80, true); chassis.pid_wait_quick_chain(); //Moving the intake backwards to prevent jamming - intake_top.move(-10); - intake_top_score.move(-10); + top_intake(-10); + top_intake_score(-10); chassis.pid_drive_set(-14, 60, true); chassis.pid_wait_quick_chain(); - intake_top.move(127); - intake_bottom.move(127); - intake_top_score.move(-100); + top_intake(127); + bottom_intake(127); + top_intake_score(-80); //Little_Mech_Mac.set(0); @@ -1685,7 +1634,7 @@ void skills() { chassis.pid_drive_set(25, 80, true); chassis.pid_wait_quick_chain(); - intake_top_score.move(127); + top_intake_score(127); chassis.pid_turn_set(180, 80, true); chassis.pid_wait_quick_chain(); @@ -1703,7 +1652,7 @@ void skills() { chassis.pid_turn_set(-90, 60, true); chassis.pid_wait_quick_chain(); - while (distance_front_l.get_distance() > 600){ + while (distance_front_l.get_distance() > 580){ chassis.pid_drive_set(1000000, 40); } L1.brake(); @@ -1722,7 +1671,7 @@ void skills() { chassis.pid_drive_set(13, 60, true); chassis.pid_wait(); - pros::delay(750); + pros::delay(950); //cross to other side @@ -1740,9 +1689,9 @@ void skills() { chassis.pid_turn_set(1, 80, true); chassis.pid_wait_quick_chain(); - intake_top.move(0); - intake_bottom.move(0); - intake_top_score.move(0); + top_intake(0); + bottom_intake(0); + top_intake_score(0); chassis.pid_drive_set(78, 80, true); chassis.pid_wait(); @@ -1760,17 +1709,18 @@ void skills() { chassis.pid_drive_set(-13, 80, true); - pros::delay(100); - intake_top.move(-20); - intake_bottom.move(-20); - intake_top_score.move(30); + // pros::delay(100); + // top_intake(-20); + // bottom_intake(-20); + // top_intake_score(30); + // chassis.pid_wait(); chassis.pid_wait(); trapdoor.set(0); - intake_top.move(127); - intake_bottom.move(127); - intake_top_score.move(127); + top_intake(127); + bottom_intake(127); + top_intake_score(127); pros::delay(1700); @@ -1789,21 +1739,22 @@ void skills() { chassis.pid_turn_set(2, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-35.5, 80, true); - pros::delay(800); - intake_top.move(-30); - intake_bottom.move(-30); - intake_top_score.move(30); + chassis.pid_drive_set(-33.5, 80, true); + // pros::delay(800); + // top_intake(-30); + // bottom_intake(-30); + // top_intake_score(30); chassis.pid_wait(); trapdoor.set(0); - intake_top.move(127); - intake_bottom.move(127); - intake_top_score.move(127); + top_intake(127); + bottom_intake(127); + top_intake_score(127); pros::delay(2000); Little_Mech_Mac.set(0); //The part were we line up for clearing + /* clear chassis.pid_drive_set(8, 127, true); chassis.pid_wait_quick_chain(); @@ -1825,12 +1776,12 @@ void skills() { // chassis.pid_drive_set(5, 127, true); // chassis.pid_wait_quick_chain(); - intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); + bottom_intake(127); + top_intake(127); + top_intake_score(127); - chassis.pid_drive_set(70, 127, true); - pros::delay(200); + chassis.pid_drive_set(70, 100, true); + pros::delay(250); Little_Mech_Mac.set(1); pros::delay(800); Little_Mech_Mac.set(0); @@ -1842,13 +1793,22 @@ void skills() { Little_Mech_Mac.set(0); pros::delay(100); + */ - chassis.pid_turn_set(0, 80, true); + chassis.pid_drive_set(6, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(60, 80, true); chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait_quick_chain(); - while (distance_front_l.get_distance() < 600){ - chassis.pid_drive_set(-1000000, 40); + while (distance_front_l.get_distance() > 600){ + chassis.pid_drive_set(1000000, 40); } L1.brake(); L2.brake(); @@ -1860,7 +1820,7 @@ void skills() { chassis.pid_turn_set(90, 60, true); chassis.pid_wait_quick_chain(); - while (distance_front_l.get_distance() > 600){ + while (distance_front_l.get_distance() > 610){ chassis.pid_drive_set(1000000, 40); } L1.brake(); @@ -1876,7 +1836,7 @@ void skills() { Little_Mech_Mac.set(1); trapdoor.set(1); - chassis.pid_drive_set(20.5, 60, true); + chassis.pid_drive_set(21, 60, true); chassis.pid_wait(); pros::delay(1050); @@ -1897,9 +1857,9 @@ void skills() { chassis.pid_turn_set(179, 80, true); chassis.pid_wait_quick_chain(); - intake_top.move(0); - intake_bottom.move(0); - intake_top_score.move(0); + top_intake(0); + bottom_intake(0); + top_intake_score(0); chassis.pid_drive_set(78, 80, true); chassis.pid_wait(); @@ -1916,17 +1876,17 @@ void skills() { chassis.pid_wait_quick_chain(); chassis.pid_drive_set(-12, 80, true); - pros::delay(200); - intake_top.move(-40); - intake_bottom.move(-40); - intake_top_score.move(40); + // pros::delay(200); + // intake_top.move(-40); + // intake_bottom.move(-40); + // intake_top_score.move(40); chassis.pid_wait(); trapdoor.set(0); - intake_top.move(127); - intake_bottom.move(127); - intake_top_score.move(127); + top_intake(127); + bottom_intake(127); + top_intake_score(127); pros::delay(1700); @@ -1936,24 +1896,24 @@ void skills() { trapdoor.set(1); - chassis.pid_drive_set(27.5, 80, true); + chassis.pid_drive_set(28, 80, true); chassis.pid_wait(); pros::delay(1450); //score second match loader chassis.pid_drive_set(-35.8, 60, true); - pros::delay(800); - intake_top.move(-20); - intake_bottom.move(-20); - intake_top_score.move(20); + // pros::delay(800); + // top_intake(-20); + // bottom_intake(-20); + // top_intake_score(20); chassis.pid_wait(); trapdoor.set(0); - intake_top.move(127); - intake_bottom.move(127); - intake_top_score.move(127); + top_intake(127); + bottom_intake(127); + top_intake_score(127); trapdoor.set(0); Little_Mech_Mac.set(0); @@ -1962,32 +1922,34 @@ void skills() { //The part were we line up for parking - chassis.pid_drive_set(8, 127, true); + chassis.pid_drive_set(8.5, 127, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(-130, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(22.5, 127, true); + chassis.pid_drive_set(20, 127, true); chassis.pid_wait_quick_chain(); chassis.pid_swing_set(LEFT_SWING, -93, 85, 0, true); chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(-94, 80, true); + chassis.pid_wait_quick_chain(); + // chassis.pid_drive_set(5, 127, true); // chassis.pid_wait_quick_chain(); + bottom_intake(127); + top_intake(127); + top_intake_score(127); - - intake_bottom.move(127); chassis.pid_drive_set(45, 127, true); - pros::delay(200); + pros::delay(250); Little_Mech_Mac.set(1); - pros::delay(1000); - - chassis.pid_wait(); - + pros::delay(800); Little_Mech_Mac.set(0); + chassis.pid_wait(); @@ -2428,14 +2390,10 @@ void wall_alignment_test() { void pid_tune(){ //pros::Task controller (color_sort_top_auton); //pros::Task controller1 (anti_jam_auton); - int intake1 = 60; - trapdoor.set(1); - bottom_intake(127); - top_intake(127); - top_intake_score(127); - - pros::delay(2000); + chassis_drive_wall(900,60); + chassis.pid_turn_set(90, 60, true); + chassis.pid_wait(); // //1 pause diff --git a/src/main.cpp b/src/main.cpp index d8200c4..2b3f73e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -21,7 +21,7 @@ ez::Drive chassis( {-6, -5, -8}, //left - {12, 19, 20}, //right + {14, 19, 20}, //right 11, 3.25, 450 @@ -166,11 +166,11 @@ void initialize() { 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({ - {"right safe", skills}, - {"right solo", pid_tune}, + {"right safe", new_skills }, + {"right solo", pid_tune }, {"elims auton 3 goals", elims_mid_control}, {"elims left", left_elims_7ball}, {"left side 4 push", left_elims_quick}, @@ -344,6 +344,11 @@ void opcontrol() { intake_top_score.move(-127); intake_piston.set(1); } + else if (master.get_digital(DIGITAL_A)) { + intake_bottom.move(127); + intake_top.move(50); + intake_top_score.move(-40); + } else if (control_to_controller){ intake_bottom.move(0); intake_top.move(0); @@ -393,9 +398,6 @@ void opcontrol() { discore_mech.set(0); } - if (master.get_digital_new_press(DIGITAL_A)) { - intake_piston.set(!intake_piston.get()); - } if (master.get_digital_new_press(DIGITAL_X)) { Digital_X += 1; if (Digital_X == 4){Digital_X = 1;} diff --git a/src/wall_tracking.cpp b/src/wall_tracking.cpp index 43c0f23..8ba74dd 100644 --- a/src/wall_tracking.cpp +++ b/src/wall_tracking.cpp @@ -23,7 +23,31 @@ 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){ + + 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); + 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(); +} + + +void drive_wall_task() { + stop_task = true; float error; float new_error; float prev_error; @@ -32,14 +56,9 @@ void drive_wall(float distance) { 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; + chassis_brake(); + while (distance_front_l.get_distance() > targer_distance) { + error = distance_front_l.get_distance() - targer_distance; derivative = error - prev_error; // if (error == 0){ // error = 300; @@ -56,6 +75,7 @@ void drive_wall(float distance) { R1.move_velocity(output); R2.move_velocity(output); R3.move_velocity(output); + prev_error = error; if (error < 600){ @@ -65,13 +85,8 @@ void drive_wall(float distance) { pros::delay(50); } - intake_top.move_velocity(127); - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); + stop_task = false; + chassis_brake(); } @@ -260,3 +275,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(); +} From 5c9996357a8b8686f89fc7a8b42e21e88b543564 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Tue, 10 Feb 2026 19:08:10 -0800 Subject: [PATCH 08/15] 3/4 auton for norcal working --- src/autons.cpp | 348 ++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 300 insertions(+), 48 deletions(-) diff --git a/src/autons.cpp b/src/autons.cpp index a527e56..96d088a 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -1483,13 +1483,13 @@ void new_skills(){ intake_bottom.move(127); intake_top.move(127); intake_top_score.move(127); - /* Taking the working 6 balls*/ - chassis.pid_drive_set(2, 10, true); + // Taking the working 6 balls + chassis.pid_drive_set(2, 20, true); chassis.pid_wait(); pros::delay(400); - chassis.pid_drive_set(50, 60, true); + chassis.pid_drive_set(50, 65, true); pros::delay(500); Little_Mech_Mac.set(1); pros::delay(300); @@ -1498,84 +1498,336 @@ void new_skills(){ chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); - chassis.pid_drive_set(-15, 20, true); + chassis.pid_drive_set(-10, 100, true); chassis.pid_wait(); + 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, 60, true); + chassis.pid_turn_set(180, 80, true); chassis.pid_wait(); - //Replaced by a new function - // while (distance_front_l.get_distance() < 500){ - // chassis.pid_drive_set(-1000000, -40); - // } - // L1.brake(); - // L2.brake(); - // L3.brake(); - // R1.brake(); - // R2.brake(); - // R3.brake(); + chassis_drive_wall(600, 127); + + chassis.pid_turn_set(-88, 80, true); + chassis.pid_wait(); - chassis_drive_wall(500, 100); + chassis_drive_wall(1450, 127); - chassis.pid_turn_set(-88, 60, true); + chassis.pid_turn_set(180, 80, true); chassis.pid_wait(); - //Replaced by a new function - // while (distance_front_l.get_distance() < 1200){ - // chassis.pid_drive_set(-1000000, -40); - // } - // L1.brake(); - // L2.brake(); - // L3.brake(); - // R1.brake(); - // R2.brake(); - // R3.brake(); + chassis.pid_drive_set(-26, 80, true); + chassis.pid_wait(); - chassis_drive_wall(1200, 100); + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 150_ms, 150_ms); - chassis.pid_turn_set(176, 60, true); + + chassis.pid_swing_set(RIGHT_SWING, -127, 127, -30, true); + top_intake(-70); + top_intake_score(-70); + pros::delay(450); + top_intake(60); + top_intake_score(-40); + chassis.pid_wait_quick(); + + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); + + //7 ball score + chassis.pid_drive_set(-1, 1, false); + + pros::delay(1500); + + //grab 7th ball + bottom_intake(127); + top_intake(127); + top_intake_score(0); + chassis.pid_turn_set(-135, 60, true); chassis.pid_wait(); - chassis.pid_drive_set(-21.2, 80, true); + chassis.pid_drive_set(7, 80, true); chassis.pid_wait(); - chassis.pid_swing_set(RIGHT_SWING, -127, 50, -15, true); + chassis.pid_drive_set(-5, 30, true); + chassis.pid_wait_quick_chain(); + top_intake(70); + top_intake_score(-40); + pros::delay(1000); + + //line up for first match loader + + chassis.pid_drive_set(50, 80, true); chassis.pid_wait(); + + 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(-4, 80, true); - top_intake(-50); - top_intake_score(-50); + chassis.pid_drive_set(8.9, 60, true); chassis.pid_wait(); + pros::delay(1050); -/*7 ball score*/ + //cross to other side + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); - for (int i = 0; i < 8; i++){ - top_intake(60); - top_intake_score(-40); - pros::delay(300); - top_intake(0); - top_intake_score(0); - pros::delay(100); - } + Little_Mech_Mac.set(0); + + chassis.pid_turn_set(-39, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(8, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(1, 100, true); + chassis.pid_wait_quick_chain(); + + top_intake(0); + bottom_intake(0); + top_intake_score(0); + + chassis.pid_drive_set(52, 100, true); + chassis.pid_wait_quick_chain(); + + //score first match loader + + 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(); + + chassis.pid_turn_set(0, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-8, 100, true); + pros::delay(400); + trapdoor.set(0); + chassis.pid_wait(); + + top_intake(127); + bottom_intake(127); + top_intake_score(127); + + pros::delay(2000); + + //grab second match loader + Little_Mech_Mac.set(1); + + trapdoor.set(1); + + chassis.pid_drive_set(27.7, 60, true); + chassis.pid_wait(); + pros::delay(1050); + + //score second match loader - top_intake(70); - top_intake_score(-35); + chassis.pid_drive_set(-29.5, 70, true); + pros::delay(1000); + top_intake(127); + bottom_intake(127); + top_intake_score(127); + trapdoor.set(0); + chassis.pid_wait(); - //grab 7th ball + pros::delay(1500); + Little_Mech_Mac.set(0); - chassis.pid_drive_set(7.5, 80, true); + // line up for clear + + discore_mech.set(0); + + chassis.pid_drive_set(8.3, 127, true); + chassis.pid_wait_quick_chain(); + + trapdoor.set(1); + + chassis.pid_swing_set(LEFT_SWING, 87, 85, 30, true); + chassis.pid_wait_quick_chain(); + + // chassis.pid_drive_set(5, 127, true); + // chassis.pid_wait_quick_chain(); + + bottom_intake(127); + top_intake(127); + top_intake_score(127); + + //clear the 6 ball from the park zone + + chassis.pid_drive_set(26, 70, true); + + pros::delay(1500); + + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 300_ms, 3_in, 600_ms, 600_ms); + + Little_Mech_Mac.set(1); + pros::delay(200); + Little_Mech_Mac.set(0); + + chassis.pid_drive_set(50, 75, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); + + chassis_drive_wall(560, 100); + + 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(0, 80, true); + chassis.pid_wait_quick_chain(); + + chassis_drive_wall(600, 100); + + chassis.pid_turn_set(-170, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(11, 90, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(80, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-25, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(135, 60, true); + chassis.pid_wait_quick_chain(); + + intake_piston.set(1); + + chassis.pid_drive_set(10, 40, true); + pros::delay(400); + top_intake(-127); + bottom_intake(-127); + pros::delay(100); + top_intake_score(-127); + chassis.pid_wait_quick_chain(); + bottom_intake(-53); + + chassis.pid_drive_set(-1.5, 80, true); + chassis.pid_wait_quick_chain(); + + pros::delay(2000); + + chassis.pid_drive_set(-4, 80, true); + chassis.pid_wait_quick_chain(); + + intake_piston.set(0); + + chassis.pid_turn_set(80, 60, true); + chassis.pid_wait_quick_chain(); + + top_intake(127); + top_intake_score(127); + bottom_intake(127); + + chassis.pid_drive_set(42.5, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(30, 60, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(19, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(0, 60, true); + chassis.pid_wait_quick_chain(); + + Little_Mech_Mac.set(1); + + chassis.pid_drive_set(13, 60, true); + chassis.pid_wait_quick_chain(); + + pros::delay(1100); + + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); + + Little_Mech_Mac.set(0); + + chassis.pid_turn_set(129, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(7, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(-177, 80, true); + chassis.pid_wait_quick_chain(); + + top_intake(0); + bottom_intake(0); + top_intake_score(0); + + chassis.pid_drive_set(53, 80, true); + chassis.pid_wait_quick_chain(); + + //score first match loader + + chassis.pid_turn_set(-125, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(7, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-7.8, 100, true); + pros::delay(500); + trapdoor.set(0); + top_intake(127); + bottom_intake(127); + top_intake_score(127); chassis.pid_wait(); - chassis.pid_drive_set(-5, 30, true); + + + pros::delay(1500); + + //grab second match loader + Little_Mech_Mac.set(1); + + trapdoor.set(1); + + chassis.pid_drive_set(27, 60, true); + chassis.pid_wait(); + pros::delay(750); + + //score second match loader + + chassis.pid_turn_set(-178, 80, true); chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-32.5, 70, true); + // pros::delay(800); + // top_intake(-30); + // bottom_intake(-30); + // top_intake_score(30); + chassis.pid_wait(); + trapdoor.set(0); + top_intake(127); + bottom_intake(127); + top_intake_score(127); + pros::delay(1900); + Little_Mech_Mac.set(0); + + chassis.pid_drive_set(8.3, 127, true); + chassis.pid_wait_quick_chain(); + chassis.pid_swing_set(LEFT_SWING, -3, 85, 29, true); + chassis.pid_wait_quick_chain(); } From bbf307d19ebf751ab91dbda89fe3d219b411595f Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Sat, 14 Feb 2026 08:57:10 -0800 Subject: [PATCH 09/15] Solo_not+wotrking --- include/subsystems.hpp | 7 +- include/wall_tracking.hpp | 2 +- src/autons.cpp | 142 +++++++++++++++++++++++++------------- src/main.cpp | 88 ++++++++++++++--------- src/wall_tracking.cpp | 25 +++++-- 5 files changed, 172 insertions(+), 92 deletions(-) diff --git a/include/subsystems.hpp b/include/subsystems.hpp index 6f0e7f0..9d1525a 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -21,19 +21,20 @@ inline pros::Motor L3(-8); inline pros::Motor R1(14); inline pros::Motor R2(19); -inline pros::Motor R3(20); +inline pros::Motor R3(18); inline pros::Imu inertial(11); inline pros::Distance distance_back_l(13); // removed sensor inline pros::Distance distance_front_l(9); -inline pros::Distance distance_back_r(17); // removed sensor +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(16); -inline pros::Motor intake_bottom(21); +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'); diff --git a/include/wall_tracking.hpp b/include/wall_tracking.hpp index 6b9527f..0c7a5c8 100644 --- a/include/wall_tracking.hpp +++ b/include/wall_tracking.hpp @@ -20,6 +20,6 @@ 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); -void chassis_drive_wall(float distance, float DRIVE_SPEED); +void chassis_drive_wall(float distance, float DRIVE_SPEED, bool match_loader); void chassis_brake(); void drive_wall_task(); \ No newline at end of file diff --git a/src/autons.cpp b/src/autons.cpp index 96d088a..9e66d6e 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -961,7 +961,7 @@ void elims_mid_control (){ //done void solo_right (){ //pros::Task anti_jam_auton1 (anti_jam_auton); - color = "R"; + color = "B"; pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); chassis.pid_drive_set(22.6, 90, true); @@ -1473,7 +1473,7 @@ void skills_before_changing_the_wall() { } void new_skills(){ - + // pros::Task anti_jam_auton1(anti_jam_auton); chassis.odom_xyt_set(0_in, 0_in, -90_deg); discore_mech.set(0); @@ -1489,8 +1489,10 @@ void new_skills(){ pros::delay(400); + 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(500); + pros::delay(200); Little_Mech_Mac.set(1); pros::delay(300); Little_Mech_Mac.set(0); @@ -1511,12 +1513,12 @@ void new_skills(){ chassis.pid_turn_set(180, 80, true); chassis.pid_wait(); - chassis_drive_wall(600, 127); + chassis_drive_wall(600, 127, false); chassis.pid_turn_set(-88, 80, true); chassis.pid_wait(); - chassis_drive_wall(1450, 127); + chassis_drive_wall(1450, 127, false); chassis.pid_turn_set(180, 80, true); chassis.pid_wait(); @@ -1528,9 +1530,12 @@ void new_skills(){ chassis.pid_swing_set(RIGHT_SWING, -127, 127, -30, true); - top_intake(-70); + pros::delay(200); + top_intake(-80); top_intake_score(-70); - pros::delay(450); + bottom_intake(-80); + pros::delay(300); + bottom_intake(60); top_intake(60); top_intake_score(-40); chassis.pid_wait_quick(); @@ -1540,7 +1545,7 @@ void new_skills(){ //7 ball score chassis.pid_drive_set(-1, 1, false); - pros::delay(1500); + pros::delay(1600); //grab 7th ball bottom_intake(127); @@ -1570,9 +1575,9 @@ void new_skills(){ Little_Mech_Mac.set(1); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(8.9, 60, true); + chassis.pid_drive_set(9.1, 60, true); chassis.pid_wait(); - pros::delay(1050); + pros::delay(1100); //cross to other side chassis.pid_drive_set(-8, 80, true); @@ -1607,49 +1612,61 @@ void new_skills(){ chassis.pid_turn_set(0, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-8, 100, true); - pros::delay(400); + chassis.pid_drive_set(-9.2, 100, true); + pros::delay(500); trapdoor.set(0); chassis.pid_wait(); + trapdoor.set(0); + + chassis.pid_drive_set(0.5, 60, true); top_intake(127); bottom_intake(127); top_intake_score(127); pros::delay(2000); + chassis.pid_wait(); //grab second match loader Little_Mech_Mac.set(1); trapdoor.set(1); - chassis.pid_drive_set(27.7, 60, true); + chassis.pid_drive_set(27.4, 60, true); chassis.pid_wait(); - pros::delay(1050); + pros::delay(1300); //score second match loader chassis.pid_drive_set(-29.5, 70, true); - pros::delay(1000); + pros::delay(1200); top_intake(127); bottom_intake(127); top_intake_score(127); trapdoor.set(0); chassis.pid_wait(); + top_intake(127); + bottom_intake(127); + top_intake_score(127); + trapdoor.set(0); + + chassis.pid_drive_set(0.5, 60, true); + pros::delay(1500); + chassis.pid_wait(); Little_Mech_Mac.set(0); // line up for clear discore_mech.set(0); - chassis.pid_drive_set(8.3, 127, true); + chassis.pid_drive_set(7.3, 127, true); chassis.pid_wait_quick_chain(); trapdoor.set(1); - chassis.pid_swing_set(LEFT_SWING, 87, 85, 30, true); + chassis.pid_swing_set(LEFT_SWING, 87, 84, 31, true); chassis.pid_wait_quick_chain(); // chassis.pid_drive_set(5, 127, true); @@ -1661,9 +1678,13 @@ void new_skills(){ //clear the 6 ball from the park zone - chassis.pid_drive_set(26, 70, true); + chassis.pid_drive_set(18, 70, true); - pros::delay(1500); + pros::delay(500); + Little_Mech_Mac.set(1); + pros::delay(500); + Little_Mech_Mac.set(0); + pros::delay(500); chassis.pid_drive_exit_condition_set(90_ms, 1_in, 300_ms, 3_in, 600_ms, 600_ms); @@ -1676,8 +1697,6 @@ void new_skills(){ chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); - chassis_drive_wall(560, 100); - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); //line up on the wall @@ -1687,7 +1706,10 @@ void new_skills(){ chassis.pid_turn_set(0, 80, true); chassis.pid_wait_quick_chain(); - chassis_drive_wall(600, 100); + chassis_drive_wall(400, 100, false); + + /* + LOW GOAL SCORE FOR LATER chassis.pid_turn_set(-170, 80, true); chassis.pid_wait_quick_chain(); @@ -1741,49 +1763,67 @@ void new_skills(){ chassis.pid_drive_set(19, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(0, 60, true); + */ + + ///new stuff + chassis.pid_turn_set(90, 60, true); chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(1); + chassis.pid_drive_set(11.5, 60, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(13, 60, true); + chassis.pid_turn_set(0, 60, true); chassis.pid_wait_quick_chain(); - pros::delay(1100); + chassis.pid_drive_set(-17, 60, true); + pros::delay(800); + trapdoor.set(0); + top_intake(127); + bottom_intake(127); + top_intake_score(127); + chassis.pid_wait(); + pros::delay(1200); + + Little_Mech_Mac.set(1); + + chassis.pid_drive_set(28.5, 60, true); + chassis.pid_wait(); + trapdoor.set(1); + pros::delay(1100); chassis.pid_drive_set(-8, 80, true); chassis.pid_wait_quick_chain(); Little_Mech_Mac.set(0); - chassis.pid_turn_set(129, 80, true); + chassis.pid_turn_set(133, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_drive_set(7, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-177, 80, true); + chassis.pid_turn_set(-180, 80, true); chassis.pid_wait_quick_chain(); top_intake(0); bottom_intake(0); top_intake_score(0); - chassis.pid_drive_set(53, 80, true); + chassis.pid_drive_set(56, 80, true); chassis.pid_wait_quick_chain(); //score first match loader - chassis.pid_turn_set(-125, 100, true); + chassis.pid_turn_set(-120, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(7, 100, true); + chassis.pid_drive_set(8, 100, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(180, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-7.8, 100, true); + chassis.pid_drive_set(-9, 100, true); pros::delay(500); trapdoor.set(0); top_intake(127); @@ -1791,44 +1831,52 @@ void new_skills(){ top_intake_score(127); chassis.pid_wait(); + chassis.pid_drive_set(0.5, 60, true); + pros::delay(1500); + chassis.pid_wait(); //grab second match loader Little_Mech_Mac.set(1); trapdoor.set(1); - chassis.pid_drive_set(27, 60, true); + chassis.pid_drive_set(27.4, 60, true); chassis.pid_wait(); - pros::delay(750); + pros::delay(1250); //score second match loader - - chassis.pid_turn_set(-178, 80, true); - chassis.pid_wait_quick_chain(); chassis.pid_drive_set(-32.5, 70, true); - // pros::delay(800); - // top_intake(-30); - // bottom_intake(-30); - // top_intake_score(30); - chassis.pid_wait(); - - trapdoor.set(0); + pros::delay(800); top_intake(127); bottom_intake(127); top_intake_score(127); - pros::delay(1900); + trapdoor.set(0); + chassis.pid_wait(); + + chassis.pid_drive_set(0.5, 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(); - chassis.pid_swing_set(LEFT_SWING, -3, 85, 29, true); + chassis.pid_swing_set(LEFT_SWING, -93, 85, 31, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(20, 127, true); chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(25, 127, true); + chassis.pid_wait(); + + + } @@ -2643,7 +2691,7 @@ void pid_tune(){ //pros::Task controller (color_sort_top_auton); //pros::Task controller1 (anti_jam_auton); - chassis_drive_wall(900,60); + chassis_drive_wall(900,60, false); chassis.pid_turn_set(90, 60, true); chassis.pid_wait(); diff --git a/src/main.cpp b/src/main.cpp index 2b3f73e..beedb9f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -21,7 +21,7 @@ ez::Drive chassis( {-6, -5, -8}, //left - {14, 19, 20}, //right + {14, 19, 18}, //right 11, 3.25, 450 @@ -142,18 +142,17 @@ void color_sort_top() { } - - void initialize() { // Set the color of the balls you want to throw out here - color = "R"; + color = "x"; + discore_mech.set(1); //intake_piston.set(1); - discore_mech.set(1); + // discore_mech.set(1); // intake_piston.set(1); ez::ez_template_print(); color_sort.set_led_pwm(100); @@ -169,7 +168,7 @@ void initialize() { //pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"right safe", new_skills }, + {"right safe", solo_right }, {"right solo", pid_tune }, {"elims auton 3 goals", elims_mid_control}, {"elims left", left_elims_7ball}, @@ -303,19 +302,32 @@ double avg_motor_temps() { 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 = false; // pros::Task anti_jam_T(anti_jam); - pros::Task color_sort_task_running (color_sort_top); + pros::Task color_sort_task_running(color_sort_top); //color = "B"; 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); @@ -326,42 +338,20 @@ void opcontrol() { intake_bottom.move(127); if (control_to_controller)(intake_top.move(127)); if (control_to_controller)(intake_top_score.move(127)); - - } - else if (master.get_digital(DIGITAL_R1)) { - intake_bottom.move(127); - if (control_to_controller)(intake_top.move(127)); - if (control_to_controller)(intake_top_score.move(-60)); } - // else if (master.get_digital(DIGITAL_A)) { - // intake_bottom.move(127); - // intake_top.move(60); - // intake_top_score.move(-40); - // } - else if (master.get_digital(DIGITAL_R2)) { - intake_bottom.move(-50); - intake_top.move(-127); - intake_top_score.move(-127); - intake_piston.set(1); - } + + else if (master.get_digital(DIGITAL_A)) { intake_bottom.move(127); - intake_top.move(50); + intake_top.move(65); intake_top_score.move(-40); } else if (control_to_controller){ intake_bottom.move(0); intake_top.move(0); intake_top_score.move(0); - //intake_piston.set(0); } - if (master.get_digital(DIGITAL_R2)) { - intake_piston.set(1); - } - else{ - intake_piston.set(0); - } // } // else { // intake_bottom.move(-40); @@ -369,6 +359,36 @@ void opcontrol() { // } // } + if (r2_active) { + if (pros::millis() - r2_time >= 1000) { + intake_bottom.move(-40); + intake_top.move(-127); + intake_top_score.move(-127); + } + + if (!master.get_digital(DIGITAL_R2)) { + r2_active = false; + intake_piston.set(0); + } + } + if (r1_active) { + if (pros::millis() - r1_time >= 300) { + intake_bottom.move(127); + intake_top.move(65); + intake_top_score.move(-50); + } + else { + intake_bottom.move(-80); + intake_top.move(-80); + 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(0); diff --git a/src/wall_tracking.cpp b/src/wall_tracking.cpp index 8ba74dd..f033208 100644 --- a/src/wall_tracking.cpp +++ b/src/wall_tracking.cpp @@ -26,15 +26,26 @@ float d_KD = 0; bool stop_task = false; float targer_distance = 0; -void chassis_drive_wall(float distance, float DRIVE_SPEED){ - - 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); - chassis.pid_wait(); +void chassis_drive_wall(float distance, float DRIVE_SPEED, bool match_loader) { + + if (match_loader){ + float distance_for_chassis_ml = (distance_match_loader.get_distance() - distance)/24.4; + master.print(0, 0, "%d", distance_match_loader.get_distance() ); + master.print(0, 0, "%.1f", distance_for_chassis_ml); + chassis.pid_drive_set(distance_for_chassis_ml, DRIVE_SPEED, true); + chassis.pid_wait(); + + } + if (!match_loader){ + 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); + chassis.pid_wait(); } +} + void drive_wall(float distance){ targer_distance = distance; From 342dca943addb44b63da832224587fecdbb192e4 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Sat, 14 Feb 2026 22:06:46 -0800 Subject: [PATCH 10/15] 2/14 10:06 --- include/autons.hpp | 1 + project.pros | 2 +- src/autons.cpp | 390 ++++++++++++++++++++++++--------------------- src/main.cpp | 11 +- 4 files changed, 217 insertions(+), 187 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index 8b827e2..6bfda71 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -22,6 +22,7 @@ void skills_without_odom(); void new_skills(); void nor_call_skills(); +void left_elims_quick_ml(); void left_elims(); void red_top_elims(); void blue_top_quals(); diff --git a/project.pros b/project.pros index 05d07d4..42db824 100644 --- a/project.pros +++ b/project.pros @@ -1,7 +1,7 @@ { "py/object": "pros.conductor.project.Project", "py/state": { - "project_name": "blue left 4", + "project_name": "left_ml_b", "target": "v5", "templates": { "EZ-Template": { diff --git a/src/autons.cpp b/src/autons.cpp index 9e66d6e..b2fecfb 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -138,6 +138,7 @@ void color_sort_top_auton() { bool in_proximity = color_sort.get_proximity() > 220; if (in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)) { + pros::delay(100); intake_top_score.move(-127); intake_top.move(30); pros::delay(300); @@ -262,35 +263,41 @@ void left_middle_top(){ intake_bottom.move(127); intake_top.move(127); + intake_top_score.move(127); - chassis.pid_drive_set(25.5, 80, true); //24.8 with little bill activation + chassis.pid_drive_set(25.8, 80, true); //24.8 with little bill activation pros::delay(500); //temp test Little_Mech_Mac.set(true); chassis.pid_wait_quick(); Little_Mech_Mac.set(0); - pros::delay(400); - + pros::delay(200); + // chassis.pid_drive_set(-2, 80, true); // chassis.pid_wait(); - chassis.pid_turn_set(-135, 80, true); + chassis.pid_turn_set(-138, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_drive_set(-16, 80, true); + intake_top_score.move(-80); + intake_top.move(-80); + intake_bottom.move(-80); + pros::delay(250); + intake_top_score.move(-80); + intake_bottom.move(100); + intake_top.move(100); chassis.pid_wait(); - intake_top_score.move(-100); - - pros::delay(1200); + pros::delay(800); - chassis.pid_drive_set(38.5, 80, true); + chassis.pid_drive_set(40.5, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(179, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(12.5, 70, true); + chassis.pid_drive_set(12.1, 70, true); Little_Mech_Mac.set(1); intake_bottom.move(127); top_intake(127); @@ -299,41 +306,39 @@ void left_middle_top(){ //intake_top_score.move(127); chassis.pid_wait(); - chassis.pid_drive_set(6.8, 50, true); - pros::delay(850); chassis.pid_drive_set(-27.8, 75, true); - intake_bottom.move(-20); - pros::delay(300); - // pros::Task color_sort_safe(color_sort_top_auton); - intake_bottom.move(127); - chassis.pid_wait(); - + pros::delay(1200); trapdoor.set(0); + chassis.pid_wait(); Little_Mech_Mac.set(0); - pros::delay(1400); + pros::delay(1100); - chassis.pid_swing_set(RIGHT_SWING, 68, 90, 2.5, true); + chassis.pid_drive_set(4, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 100, true); + chassis.pid_turn_set(-130, 80, true); chassis.pid_wait_quick_chain(); - right_rush_mech.set(1); + chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-28, 80, true); + chassis.pid_drive_set(-20, 80, true); chassis.pid_wait_quick(); chassis.pid_turn_set(-145, 60, true); + chassis.pid_wait(); + + } void left_elims_7ball(){ - color = "R"; + color = "B"; pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); @@ -344,12 +349,7 @@ void left_elims_7ball(){ intake_top_score.move(127); chassis.pid_drive_set(24.8, 100, true); - pros::delay(500); - Little_Mech_Mac.set(true); chassis.pid_wait_quick(); - Little_Mech_Mac.set(0); - - pros::delay(150); chassis.pid_turn_set(-135, 100, true); chassis.pid_wait_quick_chain(); @@ -360,13 +360,18 @@ void left_elims_7ball(){ chassis.pid_turn_set(-180, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(15.5, 70, true); + chassis.pid_drive_set(15.1, 70, true); Little_Mech_Mac.set(1); chassis.pid_wait(); - pros::delay(300); + pros::delay(400); chassis.pid_drive_set(-30, 100, true); - chassis.pid_wait_quick_chain(); + pros::delay(1100); + bottom_intake(127); + top_intake(127); + top_intake_score(127); + trapdoor.set(0); + chassis.pid_wait(); // int hue_lower = 210; @@ -380,82 +385,133 @@ void left_elims_7ball(){ // } // pros::Task color_sort_left(color_sort_top_auton); - bottom_intake(127); - top_intake(127); - top_intake_score(127); - trapdoor.set(0); - pros::delay(2200); + + pros::delay(1800); trapdoor.set(1); Little_Mech_Mac.set(0); - chassis.pid_drive_set(3, 127, true); + chassis.pid_drive_set(4, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-95, 100, true); + chassis.pid_turn_set(-130, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 180, 80, 0, true); + chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); chassis.pid_wait_quick_chain(); - right_rush_mech.set(1); - - chassis.pid_drive_set(-23, 80, true); + chassis.pid_drive_set(-20, 80, true); chassis.pid_wait_quick(); - chassis.pid_turn_set(-160, 40, false); + chassis.pid_turn_set(-145, 60, true); chassis.pid_wait(); - - chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); } -// FINISHED -void left_elims_quick(){ - - color = "R"; +void left_elims_quick_ml(){ + color = "R"; + 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(-90, 80, true); + chassis.pid_turn_set(180, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(12.3, 70, 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(); - pros::delay(400); + pros::delay(200); - chassis.pid_drive_set(-26.8, 75, true); + chassis.pid_drive_set(-27.3, 75, true); chassis.pid_wait(); trapdoor.set(0); - pros::delay(1100); + pros::delay(1200); trapdoor.set(1); + + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(-130, 80, true); + chassis.pid_wait_quick_chain(); + + 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(); + + chassis.pid_turn_set(-145, 60, true); + chassis.pid_wait(); + + + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); +} + +// FINISHED +void left_elims_quick(){ + + //Colorsort color set + color = "B"; + pros::Task color_sor(color_sort_top_auton); + + + + chassis.odom_xyt_set(0_in, 0_in, -30_deg); + discore_mech.set(0); + trapdoor.set(1); + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_drive_set(25.8, 80, true); //24.8 with little bill activation + pros::delay(500); + //temp test Little_Mech_Mac.set(true); + chassis.pid_wait_quick(); Little_Mech_Mac.set(0); - chassis.pid_drive_set(3, 100, true); + pros::delay(10); + + chassis.pid_turn_set(-120, 90, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(175, 100, true); + chassis.pid_drive_set(20, 90, true); chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 90, 80, 0, true); + chassis.pid_turn_set(180, 90, true); chassis.pid_wait_quick_chain(); - right_rush_mech.set(1); + chassis.pid_drive_set(-8, 60, true); + pros::delay(100); + trapdoor.set(0); + chassis.pid_wait(); + discore_mech.set(1); + + pros::delay(400); + trapdoor.set(1); + + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(-130, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-23, 80, true); + chassis.pid_drive_set(-20, 80, true); chassis.pid_wait_quick(); - chassis.pid_turn_set(70, 40, false); + chassis.pid_turn_set(-145, 60, true); chassis.pid_wait(); - chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); } @@ -977,7 +1033,7 @@ void solo_right (){ intake_top_score.move(127); chassis.pid_wait(); - pros::delay(200); + pros::delay(100); chassis.pid_drive_set(-26.8, 75, true); chassis.pid_wait(); @@ -985,7 +1041,6 @@ void solo_right (){ pros::delay(1000); trapdoor.set(1); - // chassis.pid_swing_set(LEFT_SWING, -135, 127, -20, true); // chassis.pid_wait_quick(); @@ -997,6 +1052,7 @@ void solo_right (){ chassis.pid_wait_quick_chain(); chassis.pid_drive_set(23.7, 80, true); + intake_top.move(127); pros::delay(650); Little_Mech_Mac.set(1); chassis.pid_wait_quick_chain(); @@ -1005,27 +1061,29 @@ void solo_right (){ chassis.pid_turn_set(179, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(47, 65, true); + chassis.pid_drive_set(47, 70, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-6.5, 80, true); + chassis.pid_drive_set(-4, 80, true); chassis.pid_wait_quick(); chassis.pid_turn_set(135, 80, true); pros::delay(200); - intake_top.move(-30); - intake_top_score.move(-30); - bottom_intake(-30); + intake_top.move(-20); + intake_top_score.move(-20); + bottom_intake(-20); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-14.8, 60, true); - pros::delay(400); - intake_top.move(127); - intake_top_score.move(-127); + chassis.pid_drive_set(-15, 60, true); + pros::delay(600); + intake_top.move(100); + intake_top_score.move(-90); bottom_intake(127); chassis.pid_wait(); - pros::delay(500); + chassis.pid_drive_set(1, 30, true); + + pros::delay(300); intake_top.move(0); bottom_intake(0); @@ -1038,7 +1096,7 @@ void solo_right (){ intake_top_score.move(127); - chassis.pid_drive_set(14, 60, true); + chassis.pid_drive_set(13.8, 60, true); intake_top.move(127); intake_top_score.move(127); bottom_intake(127); @@ -1048,8 +1106,12 @@ void solo_right (){ pros::delay(200); chassis.pid_drive_set(-27, 75, true); + pros::delay(1100); + trapdoor.set(0); chassis.pid_wait(); - trapdoor.set(0); + + chassis.pid_drive_set(0.5, 60, true); + pros::delay(1100); @@ -1546,6 +1608,14 @@ void new_skills(){ chassis.pid_drive_set(-1, 1, false); pros::delay(1600); + top_intake(-80); + top_intake_score(-70); + bottom_intake(-80); + pros::delay(300); + bottom_intake(60); + top_intake(60); + top_intake_score(-40); + pros::delay(500); //grab 7th ball bottom_intake(127); @@ -1575,7 +1645,7 @@ void new_skills(){ Little_Mech_Mac.set(1); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(9.1, 60, true); + chassis.pid_drive_set(9.2, 60, true); chassis.pid_wait(); pros::delay(1100); @@ -1613,21 +1683,25 @@ void new_skills(){ chassis.pid_wait_quick_chain(); chassis.pid_drive_set(-9.2, 100, true); - pros::delay(500); - trapdoor.set(0); chassis.pid_wait(); - trapdoor.set(0); - - chassis.pid_drive_set(0.5, 60, true); - top_intake(127); - bottom_intake(127); - top_intake_score(127); + 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(2000); chassis.pid_wait(); //grab second match loader + Little_Mech_Mac.set(1); trapdoor.set(1); @@ -1639,20 +1713,18 @@ void new_skills(){ //score second match loader chassis.pid_drive_set(-29.5, 70, true); - pros::delay(1200); - top_intake(127); - bottom_intake(127); - top_intake_score(127); - trapdoor.set(0); chassis.pid_wait(); - top_intake(127); - bottom_intake(127); - 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); - chassis.pid_drive_set(0.5, 60, true); - pros::delay(1500); chassis.pid_wait(); Little_Mech_Mac.set(0); @@ -1661,6 +1733,19 @@ void new_skills(){ discore_mech.set(0); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(60, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(55, 60, true); + chassis.pid_wait_quick_chain(); + + + + + /* chassis.pid_drive_set(7.3, 127, true); chassis.pid_wait_quick_chain(); @@ -1707,8 +1792,6 @@ void new_skills(){ chassis.pid_wait_quick_chain(); chassis_drive_wall(400, 100, false); - - /* LOW GOAL SCORE FOR LATER chassis.pid_turn_set(-170, 80, true); @@ -1766,10 +1849,7 @@ void new_skills(){ */ ///new stuff - chassis.pid_turn_set(90, 60, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(11.5, 60, true); + chassis.pid_drive_set(13.5, 60, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(0, 60, true); @@ -1824,16 +1904,17 @@ void new_skills(){ chassis.pid_wait_quick_chain(); chassis.pid_drive_set(-9, 100, true); - pros::delay(500); - trapdoor.set(0); - top_intake(127); - bottom_intake(127); - top_intake_score(127); chassis.pid_wait(); - chassis.pid_drive_set(0.5, 60, true); - - + 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); pros::delay(1500); chassis.pid_wait(); @@ -1850,14 +1931,18 @@ void new_skills(){ //score second match loader chassis.pid_drive_set(-32.5, 70, true); - pros::delay(800); - top_intake(127); - bottom_intake(127); - top_intake_score(127); - trapdoor.set(0); chassis.pid_wait(); - chassis.pid_drive_set(0.5, 60, true); + 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); + pros::delay(1300); chassis.pid_wait(); @@ -1872,7 +1957,7 @@ void new_skills(){ chassis.pid_drive_set(20, 127, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(25, 127, true); + chassis.pid_drive_set(23, 127, true); chassis.pid_wait(); @@ -2688,73 +2773,16 @@ void wall_alignment_test() { R3.brake(); } void pid_tune(){ - //pros::Task controller (color_sort_top_auton); - //pros::Task controller1 (anti_jam_auton); - - chassis_drive_wall(900,60, false); - chassis.pid_turn_set(90, 60, true); - chassis.pid_wait(); - - // //1 pause - // top_intake(0); - // top_intake_score(0); - // pros::delay(200); - // //Continue - // top_intake(intake1); - // top_intake_score(-40); - // pros::delay(300); - // //second pause - // top_intake(0); - // top_intake_score(0); - // pros::delay(200); - // //Continue - // top_intake(intake1); - // top_intake_score(-40); - // pros::delay(300); - // //third pause - // top_intake(0); - // top_intake_score(0); - // pros::delay(200); - // //Continue - // top_intake(intake1); - // top_intake_score(-40); - // pros::delay(300); - // //fourth pause - // top_intake(0); - // top_intake_score(0); - // pros::delay(200); - // //Continue - // top_intake(intake1); - // top_intake_score(-40); - // pros::delay(300); - // //fifth pause - // top_intake(0); - // top_intake_score(0); - // pros::delay(200); - // //Continue - // top_intake(intake1); - // top_intake_score(-40); - // pros::delay(300); - // // sixth pause - // top_intake(0); - // top_intake_score(0); - // pros::delay(200); - // //Continue - // top_intake(intake1); - // top_intake_score(-40); - // pros::delay(300); - // //seventh pause - // top_intake(0); - // top_intake_score(0); - // pros::delay(200); - // //Continue - // top_intake(intake1); - // top_intake_score(-40); - // pros::delay(300); - // //final stop - // top_intake(0); - // top_intake_score(0); + trapdoor.set(1); + 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); } void color_sort_test(){ color = "B"; diff --git a/src/main.cpp b/src/main.cpp index beedb9f..457bf7e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -146,8 +146,9 @@ void initialize() { // Set the color of the balls you want to throw out here - color = "x"; + color = "R"; discore_mech.set(1); + trapdoor.set(1); //intake_piston.set(1); @@ -168,7 +169,7 @@ void initialize() { //pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"right safe", solo_right }, + {"right safe", new_skills}, {"right solo", pid_tune }, {"elims auton 3 goals", elims_mid_control}, {"elims left", left_elims_7ball}, @@ -372,14 +373,14 @@ void opcontrol() { } } if (r1_active) { - if (pros::millis() - r1_time >= 300) { + if (pros::millis() - r1_time >= 50) { intake_bottom.move(127); intake_top.move(65); intake_top_score.move(-50); } else { - intake_bottom.move(-80); - intake_top.move(-80); + intake_bottom.move(-75); + intake_top.move(-75); intake_top_score.move(-70); } From 659998a14853c3a3a69ed292a67668b35e5427aa Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Sat, 21 Feb 2026 21:37:24 -0800 Subject: [PATCH 11/15] fixing skills --- include/autons.hpp | 1 + include/wall_tracking.hpp | 2 +- project.pros | 6 +- src/autons.cpp | 2332 +++++++------------------------------ src/main.cpp | 27 +- src/wall_tracking.cpp | 18 +- 6 files changed, 451 insertions(+), 1935 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index 6bfda71..8ea9311 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -27,6 +27,7 @@ void left_elims(); void red_top_elims(); void blue_top_quals(); +void push_solo(); void left_elims_7ball(); void intake_test(); diff --git a/include/wall_tracking.hpp b/include/wall_tracking.hpp index 0c7a5c8..3454a06 100644 --- a/include/wall_tracking.hpp +++ b/include/wall_tracking.hpp @@ -20,6 +20,6 @@ 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); -void chassis_drive_wall(float distance, float DRIVE_SPEED, bool match_loader); +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 42db824..1ba9746 100644 --- a/project.pros +++ b/project.pros @@ -1,7 +1,7 @@ { "py/object": "pros.conductor.project.Project", "py/state": { - "project_name": "left_ml_b", + "project_name": "2550R", "target": "v5", "templates": { "EZ-Template": { @@ -424,8 +424,8 @@ } }, "upload_options": { - "description": "roboticsisez.com", - "icon": "ufo", + "description": "2550R", + "icon": "power", "slot": 1 }, "use_early_access": false diff --git a/src/autons.cpp b/src/autons.cpp index b2fecfb..ea4b4e1 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -180,8 +180,6 @@ void bottom_intake(int speed_bt){ bottom_speed_intake = speed_bt; } - - void default_constants() { chassis.pid_drive_constants_set(22, 0, 150); chassis.pid_heading_constants_set(11.0, 0.0, 20.0); @@ -214,72 +212,37 @@ 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(); - - chassis.pid_turn_set(135, 60); - chassis.pid_wait(); - - 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); -} - -void intake_test(){ - pros::Task anti_jam_auton1 (anti_jam_auton); +///////////////////////////////////////////////////////// +// AUTONS // +//////////////////////////////////////////////////////// - bottom_intake(127); - intake_top.move(127); - intake_top_score.move(127); -} -// FINISHED void left_middle_top(){ //Colorsort color set - color = "B"; - pros::Task color_sor(color_sort_top_auton); - - chassis.odom_xyt_set(0_in, 0_in, -30_deg); - - trapdoor.set(1); + //Setup + pros::Task color_sor(color_sort_top_auton); + chassis.odom_xyt_set(0_in, 0_in, -30_deg); + trapdoor.set(1); intake_bottom.move(127); intake_top.move(127); intake_top_score.move(127); - chassis.pid_drive_set(25.8, 80, true); //24.8 with little bill activation + //3 balls mid + chassis.pid_drive_set(23.8, 90, true); //24.8 with little bill activation pros::delay(500); - //temp test Little_Mech_Mac.set(true); - chassis.pid_wait_quick(); + Little_Mech_Mac.set(true); + chassis.pid_wait_quick_chain(); Little_Mech_Mac.set(0); pros::delay(200); - - // chassis.pid_drive_set(-2, 80, true); - // chassis.pid_wait(); - chassis.pid_turn_set(-138, 80, true); + chassis.pid_turn_set(-138, 90, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-16, 80, true); + chassis.pid_drive_set(-16, 90, true); intake_top_score.move(-80); intake_top.move(-80); intake_bottom.move(-80); @@ -291,10 +254,10 @@ void left_middle_top(){ pros::delay(800); - chassis.pid_drive_set(40.5, 80, true); + chassis.pid_drive_set(40.5, 90, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(179, 80, true); + chassis.pid_turn_set(179, 90, true); chassis.pid_wait_quick_chain(); chassis.pid_drive_set(12.1, 70, true); @@ -330,15 +293,9 @@ void left_middle_top(){ chassis.pid_turn_set(-145, 60, true); chassis.pid_wait(); - - - } - - void left_elims_7ball(){ - color = "B"; pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); @@ -349,6 +306,8 @@ void left_elims_7ball(){ intake_top_score.move(127); chassis.pid_drive_set(24.8, 100, true); + pros::delay(500); + Little_Mech_Mac.set(1); chassis.pid_wait_quick(); chassis.pid_turn_set(-135, 100, true); @@ -373,20 +332,7 @@ void left_elims_7ball(){ trapdoor.set(0); chassis.pid_wait(); - - // int hue_lower = 210; - // int hue_higher = 250; - // int current_time = pros::millis(); - // bool in_proximity = color_sort.get_proximity() > 50; - // while ((pros::millis() - current_time < 1800) || (in_proximity && !(hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher))){ - // intake_bottom.move(127); - // intake_top.move(127); - // intake_top_score.move(127); - // } - // pros::Task color_sort_left(color_sort_top_auton); - - - pros::delay(1800); + pros::delay(1700); trapdoor.set(1); Little_Mech_Mac.set(0); @@ -407,7 +353,7 @@ void left_elims_7ball(){ } void left_elims_quick_ml(){ - color = "R"; + chassis.odom_xyt_set(0_in, 0_in, -90_deg); pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); @@ -452,15 +398,9 @@ void left_elims_quick_ml(){ chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); } -// FINISHED void left_elims_quick(){ - //Colorsort color set - color = "B"; pros::Task color_sor(color_sort_top_auton); - - - chassis.odom_xyt_set(0_in, 0_in, -30_deg); discore_mech.set(0); trapdoor.set(1); @@ -469,155 +409,49 @@ void left_elims_quick(){ intake_top.move(127); intake_top_score.move(127); - chassis.pid_drive_set(25.8, 80, true); //24.8 with little bill activation + chassis.pid_drive_set(22.5, 127, true); //24.8 with little bill activation pros::delay(500); - //temp test Little_Mech_Mac.set(true); - chassis.pid_wait_quick(); - Little_Mech_Mac.set(0); - - pros::delay(10); - - chassis.pid_turn_set(-120, 90, true); + Little_Mech_Mac.set(true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(20, 90, true); + chassis.pid_turn_set(60, 127, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(180, 90, true); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); + + chassis.pid_drive_set(-11, 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_drive_set(-8, 60, true); - pros::delay(100); + chassis.pid_swing_set(RIGHT_SWING, 180, -127, 0, true); + pros::delay(800); trapdoor.set(0); - chassis.pid_wait(); - discore_mech.set(1); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-2, 127, true); - pros::delay(400); trapdoor.set(1); chassis.pid_drive_set(4, 80, true); chassis.pid_wait_quick_chain(); + discore_mech.set(1); + chassis.pid_turn_set(-130, 80, true); chassis.pid_wait_quick_chain(); 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(); - - chassis.pid_turn_set(-145, 60, true); - chassis.pid_wait(); + chassis.pid_drive_set(-20, 90, true); chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); - -} - - -/* old -- // get 3 middle balls - //pros::Task controller (controller_update); - - chassis.pid_odom_set({{0_in, 14_in}, fwd, 100}, true); - chassis.pid_wait(); - - chassis.pid_turn_set(45.9, 100, true); - chassis.pid_wait(); - - bottom_intake(127); - - chassis.pid_drive_set(20, 30, true); - chassis.pid_wait(); - - //score 3 in middle goal - chassis.pid_drive_set(-5.6, 60, true); - chassis.pid_wait(); - - // chassis.pid_odom_set({{-9.9_in, 26_in}, fwd, 60}, true); - // pros::delay(100); - // chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(-45, 80); - intake_bottom.move(-55); - chassis.pid_wait(); - - - chassis.pid_drive_set(15, 80); - chassis.pid_wait(); - - intake_bottom.move(-70); - pros::delay(900); - intake_top.move(0); - intake_bottom.move(0); - - // line up for match loader - - chassis.pid_drive_set(-10, 80, true); - chassis.pid_wait(); - - middle_stage.set(0); - Little_Mech_Mac.set(0); - - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait(); - - while (distance_front.get_distance() > 655){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - - pros::delay(100); - - chassis.pid_turn_set(91, 60, true); - chassis.pid_wait(); - - while (distance_front.get_distance() > 690){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - - pros::delay(50); - - Little_Mech_Mac.set(1); - - chassis.pid_turn_set(-179, 80, true); - chassis.pid_wait(); - - intake_bottom.move(127); - - chassis.pid_drive_set(13.5, 60, true); - chassis.pid_wait(); - - pros::delay(150); - - chassis.pid_drive_set(-33, 60, true); - chassis.pid_wait(); - intake_top.move(127); - pros::delay(900); - - // chassis.pid_drive_set(8, 80, true); - // chassis.pid_wait_quick(); - - // chassis.pid_drive_set(-15, 127, false); } -*/ - void right_safe(){ - color = "B"; pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); chassis.pid_drive_set(22.6, 90, true); @@ -685,240 +519,138 @@ void right_safe(){ chassis.pid_turn_set(-135, 80, true); chassis.pid_wait(); - } +void right_elims_quick(){ + chassis.odom_xyt_set(0_in, 0_in, 30_deg); -// needs updated -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); - - intake_bottom.move(127); - chassis.pid_drive_set(36.5, 80, true); + bottom_intake(127); + trapdoor.set(true); + chassis.pid_drive_set(30_in, 100); + pros::delay(450); + Little_Mech_Mac.set(true); chassis.pid_wait(); - Little_Mech_Mac.set(1); - pros::delay(50); - chassis.pid_turn_set(180, 110, true); - chassis.pid_wait_quick(); - - - - chassis.pid_drive_set(13, 90, true); - chassis.pid_wait_quick(); - 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); - - intake_bottom.move(127); - chassis.pid_drive_set(13, 60, true); - pros::delay(140); - - // chassis.pid_turn_set(180, 80, true); - // chassis.pid_wait_quick(); + chassis.pid_turn_set(135, 60); + chassis.pid_wait(); - chassis.pid_drive_set(-32, 70, true); - chassis.pid_wait_quick(); - //chassis.drive_brake_set(MOTOR_BRAKE_COAST); - chassis.pid_drive_set(1, 5, true); - trapdoor.set(1); - intake_top.move(127); - pros::delay(1600); - chassis.drive_brake_set(MOTOR_BRAKE_HOLD); + 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); +} - Little_Mech_Mac.set(0); +void solo_right (){ + //pros::Task anti_jam_auton1 (anti_jam_auton); - //intake_top.brake(); + 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(90, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(4, 100, true); + 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(); - chassis.pid_swing_set(RIGHT_SWING, 46.2, 74, -23.5, true); + pros::delay(100); + + chassis.pid_drive_set(-26.8, 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(300); + pros::delay(1000); trapdoor.set(1); - chassis.pid_wait_quick(); - - - - intake_bottom.move(127); - intake_top.move(127); - pros::delay(150); + // chassis.pid_swing_set(LEFT_SWING, -135, 127, -20, true); + // chassis.pid_wait_quick(); - trapdoor.set(0); - /* - chassis.pid_drive_set(15, 80, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(5, 80, true); + chassis.pid_wait_quick_chain(); - intake_top.move(-127); + chassis.pid_turn_set(-143, 80, true); + Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); - middle_stage.set(0); + chassis.pid_drive_set(23.7, 80, true); + intake_top.move(127); + pros::delay(650); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); Little_Mech_Mac.set(0); - 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(); + chassis.pid_turn_set(179, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(39, 90, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(47, 70, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-41,80, false); - chassis.pid_wait_quick(); - intake_bottom.move(-127); - chassis.pid_drive_set(11.5, 90, true); + chassis.pid_drive_set(-4, 80, true); chassis.pid_wait_quick(); + chassis.pid_turn_set(135, 80, true); + pros::delay(200); + intake_top.move(-20); + intake_top_score.move(-20); + bottom_intake(-20); + chassis.pid_wait_quick_chain(); - - // intake_bottom.set_brake_mode(MOTOR_BRAKE_COAST); - - // chassis.pid_swing_set(LEFT_SWING, -37, 70*1.45, 52*1.15, false); - // chassis.pid_wait_quick(); - - // chassis.pid_drive_set(-3, 60, true); - - pros::delay(100); - -} -void solo_left1() { - 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(); - intake_bottom.move(127); - chassis.pid_drive_set(37.5, 80, true); + chassis.pid_drive_set(-15, 60, true); + pros::delay(600); + intake_top.move(100); + intake_top_score.move(-90); + bottom_intake(127); chassis.pid_wait(); - Little_Mech_Mac.set(1); - pros::delay(50); - chassis.pid_turn_set(180, 110, true); - chassis.pid_wait_quick(); - - - - chassis.pid_drive_set(13, 90, true); - chassis.pid_wait_quick(); - - 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); - intake_bottom.move(127); - chassis.pid_drive_set(13, 60, true); - pros::delay(140); + chassis.pid_drive_set(1, 30, true); - // chassis.pid_turn_set(180, 80, true); - // chassis.pid_wait_quick(); - - 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); - trapdoor.set(1); - intake_top.move(127); - pros::delay(1600); - chassis.drive_brake_set(MOTOR_BRAKE_HOLD); + pros::delay(300); - Little_Mech_Mac.set(0); + intake_top.move(0); + bottom_intake(0); - //intake_top.brake(); + chassis.pid_drive_set(40, 90, true); + chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(4, 100, true); - chassis.pid_wait(); + intake_top_score.move(127); - chassis.pid_swing_set(RIGHT_SWING, 46.2, 74, -18.5, true); + chassis.pid_drive_set(13.8, 60, true); + intake_top.move(127); + intake_top_score.move(127); + bottom_intake(127); + Little_Mech_Mac.set(1); 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); - trapdoor.set(1); - chassis.pid_wait_quick(); - - - - intake_bottom.move(127); - intake_top.move(127); - pros::delay(150); + chassis.pid_drive_set(-27, 75, true); + pros::delay(1100); trapdoor.set(0); - /* - chassis.pid_drive_set(15, 80, true); - chassis.pid_wait_quick(); - - intake_top.move(-127); - - middle_stage.set(0); - Little_Mech_Mac.set(0); - - 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(); - - chassis.pid_drive_set(33, 90, true); - chassis.pid_wait_quick(); - 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(); - - - - // intake_bottom.set_brake_mode(MOTOR_BRAKE_COAST); - - // chassis.pid_swing_set(LEFT_SWING, -37, 70*1.45, 52*1.15, false); - // chassis.pid_wait_quick(); + chassis.pid_drive_set(0.5, 60, true); - // chassis.pid_drive_set(-3, 60, true); - - pros::delay(100); + pros::delay(1100); } void elims_mid_control (){ - color = "B"; pros::Task color_sor(color_sort_top_auton); trapdoor.set(1); @@ -1014,1676 +746,450 @@ void elims_mid_control (){ } -//done -void solo_right (){ - //pros::Task anti_jam_auton1 (anti_jam_auton); - color = "B"; +void push_solo(){ + pros::Task color_sor(color_sort_top_auton); - trapdoor.set(1); - chassis.pid_drive_set(22.6, 90, true); + + chassis.odom_xyt_set(0_in, 0_in, -90_deg); + discore_mech.set(0); + + top_intake(127); + top_intake_score(127); + bottom_intake(127); + + 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.pid_turn_set(90, 80, true); + chassis.pid_drive_set(-38, 100, true); chassis.pid_wait_quick_chain(); - 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(); + chassis.pid_turn_set(180, 90, true); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); - pros::delay(100); + chassis.pid_drive_set(8, 80, true); + chassis.pid_wait_quick_chain(); + + pros::delay(500); chassis.pid_drive_set(-26.8, 75, true); chassis.pid_wait(); + Little_Mech_Mac.set(0); trapdoor.set(0); pros::delay(1000); - trapdoor.set(1); - // chassis.pid_swing_set(LEFT_SWING, -135, 127, -20, true); - // chassis.pid_wait_quick(); - - chassis.pid_drive_set(5, 80, true); - chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-143, 80, true); - Little_Mech_Mac.set(0); + chassis.pid_turn_set(-88, 100, true); chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(23.7, 80, true); + trapdoor.set(1); intake_top.move(127); - pros::delay(650); - Little_Mech_Mac.set(1); - chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(0); + intake_top_score.move(127); - chassis.pid_turn_set(179, 80, true); + chassis.pid_drive_set(56, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(47, 70, true); + chassis.pid_turn_set(-135, 127, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-4, 80, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(16.5, 127, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(135, 80, true); - pros::delay(200); - intake_top.move(-20); - intake_top_score.move(-20); - bottom_intake(-20); + chassis.pid_turn_set(180, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-15, 60, true); - pros::delay(600); - intake_top.move(100); - intake_top_score.move(-90); - bottom_intake(127); + chassis.pid_drive_set(-10, 100, true); + pros::delay(500); + trapdoor.set(0); chassis.pid_wait(); + trapdoor.set(0); + pros::delay(1000); + trapdoor.set(1); - chassis.pid_drive_set(1, 30, true); - - pros::delay(300); + //Intaking the 2rd machloader - intake_top.move(0); - bottom_intake(0); + chassis.pid_drive_set(28.4, 80, true); + Little_Mech_Mac.set(1); + chassis.pid_wait(); + trapdoor.set(1); + pros::delay(200); - chassis.pid_drive_set(40, 90, true); + chassis.pid_drive_set(-3, 100, true); chassis.pid_wait_quick_chain(); + Little_Mech_Mac.set(0); - chassis.pid_turn_set(90, 80, true); + chassis.pid_turn_set(-135, 100, true); chassis.pid_wait_quick_chain(); - intake_top_score.move(127); - - chassis.pid_drive_set(13.8, 60, true); - intake_top.move(127); - intake_top_score.move(127); - bottom_intake(127); - Little_Mech_Mac.set(1); - chassis.pid_wait(); - + chassis.pid_drive_set(-50, 100, true); + top_intake(-50); + top_intake_score(-50); + bottom_intake(-50); pros::delay(200); + top_intake(0); + top_intake_score(0); + bottom_intake(0); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-27, 75, true); - pros::delay(1100); - trapdoor.set(0); - chassis.pid_wait(); - - chassis.pid_drive_set(0.5, 60, true); - + top_intake_score(-127); + top_intake(80); + bottom_intake(127); - pros::delay(1100); + // if(color == "R"){ + // if (color_sort.get_hue() < 0 || color_sort.get_hue() > 10){ + // top_intake_score(0); + // top_intake(0); + // bottom_intake(0); + // } + // } + // if(color == "B"){ + // if (color_sort.get_hue() < 210 || color_sort.get_hue() > 240){ + // top_intake_score(0); + // top_intake(0); + // bottom_intake(0); + // } + // } + } -void skills_without_odom(){ +void skills(){ + + // pros::Task anti_jam_auton1(anti_jam_auton); chassis.odom_xyt_set(0_in, 0_in, -90_deg); - trapdoor.set(1); - color = "x"; - - bottom_intake(127); - top_intake(127); - //Instead of while 735 - // chassis.pid_turn_set(0,60); - // chassis.pid_wait(); - // chassis.pid_turn_set(-90,60); - // chassis.pid_wait(); + discore_mech.set(0); + trapdoor.set(1); + intake_piston.set(0); - chassis.pid_drive_set(-10,60); + 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(); - drive_wall(500); + pros::delay(400); - /* - Empty first match loader - */ - Little_Mech_Mac.set(1); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 1000_ms); - chassis.pid_turn_set(180, 110, true); + chassis.pid_drive_set(50, 65, true); + pros::delay(200); + Little_Mech_Mac.set(1); + pros::delay(300); + Little_Mech_Mac.set(0); chassis.pid_wait(); - chassis.pid_drive_set(11.5, 90, true); - chassis.pid_wait(); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); - //intake_bottom.move(127); - //intake_top.move(127); + chassis.pid_drive_set(-10, 100, true); + chassis.pid_wait(); - pros::delay(1350); - /* - Back up from match loader - */ + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 400_ms, 300_ms); - Little_Mech_Mac.set(0); - pros::delay(100); + //line up on the wall - chassis.pid_drive_set(-9.8, 90, true); - chassis.pid_wait_quick(); + chassis.pid_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms,500_ms); - bottom_intake(0); - top_intake(0); + chassis.pid_turn_set(180, 80, true); + chassis.pid_wait(); - chassis.pid_turn_set(-90, 60, true); + chassis_drive_wall(600, 127, false, false); + + chassis.pid_turn_set(-88, 80, true); chassis.pid_wait(); - bottom_intake(0); - top_intake(0); - drive_wall(200); + chassis_drive_wall(1450, 127, false, false); - chassis.pid_turn_set(0, 60, true); + chassis.pid_turn_set(180, 80, true); chassis.pid_wait(); - + chassis.pid_drive_set(-26, 80, true); + chassis.pid_wait(); + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 150_ms, 150_ms); -} -void skills_before_changing_the_wall() { - pros::Task task1(controller_update); - //pros::Task color_sort_task_running(color_sort_S); - drive_wall(510); - //discore_mech.set(1); - chassis.odom_xyt_set(0_in, 0_in, -90_deg); - trapdoor.set(0); - color = "x"; - /* - Setup for first matchload - */ - - // 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(); - - chassis.pid_drive_set(11.5, 90, true); - chassis.pid_wait(); - - //intake_bottom.move(127); - //intake_top.move(127); - - pros::delay(1350); - - /* - Back up from match loader - */ - - Little_Mech_Mac.set(0); - pros::delay(100); - - chassis.pid_drive_set(-9.8, 90, true); - chassis.pid_wait_quick(); - - bottom_intake(0); - top_intake(0); - - chassis.pid_turn_set(-90, 60, true); - chassis.pid_wait(); - - /* - Relocate to blue side - */ - - chassis.pid_odom_set({{-45_in, 0_in}, fwd, 100}, true); - chassis.pid_wait(); - - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait(); - - /* - Reset location to 0, 0 - */ - - 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); - - // 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; - - p_x = position_x; - p_y = position_y; - - chassis.odom_xy_set(position_x, position_y); - pros::delay(150); - - /* - Setup for match loader - */ - - chassis.pid_turn_set(90, 60, true); - chassis.pid_wait(); - - // chassis.pid_odom_set({{6.7_in, 0_in}, fwd, 110}, true); - // chassis.pid_wait(); - // screen = 1; - chassis.pid_drive_set(12_in, 100); - chassis.pid_wait(); - /* - Score first match loader - */ - - Little_Mech_Mac.set(1); - - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait(); - - top_intake(-20); - bottom_intake(-20); - //intake_top.move(127); - //intake_bottom.move(127); - - chassis.pid_drive_set(-16, 90, true); - chassis.pid_wait_quick(); - - top_intake(127); - bottom_intake(127); - - chassis.pid_drive_set(-100, 10, true); - trapdoor.set(1); - pros::delay(100); - trapdoor.set(1); - pros::delay(1700); - - /* - empty second match loader - */ - - chassis.pid_drive_set(31.5, 100, true); - - pros::delay(1800); - - /* - Score second 6 on long goal - */ - trapdoor.set(0); - - Little_Mech_Mac.set(0); - - pros::delay(100); - - chassis.pid_drive_set(-29.5, 60, true); - chassis.pid_wait(); - - top_intake(-15); - bottom_intake(-15); - - chassis.pid_drive_set(-5, 20, true); - pros::delay(150); - - top_intake(127); - bottom_intake(127); - - trapdoor.set(1); - - pros::delay(1000); - - - /* - Set up for first 2 middle balls - */ - - // Activate color sort - - - chassis.pid_drive_set(10, 110, true); - chassis.pid_wait(); - - chassis.pid_turn_set(88, 80, true); - chassis.pid_wait(); - trapdoor.set(0); - - // chassis.pid_odom_set({{26.8, -10.5}, fwd, 80}, true); - // chassis.pid_wait_quick(); - - chassis.pid_drive_set(80, 80, true); - chassis.pid_wait(); - - chassis.pid_turn_set(-1.5, 60); - chassis.pid_wait(); - - 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); - - - //drive_wall(450); - - chassis.pid_turn_set(88.5, 60); - chassis.pid_wait(); - - 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); - - //drive_wall(450); - - chassis.pid_turn_set(-3, 60); - chassis.pid_wait(); - - pros::delay(150); - - /* - reset position - */ - - 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; - - p_x = position_x_1; - p_y = position_y_1; - - pros::delay(150); - - chassis.odom_xy_set(position_x_1, position_y_1); - chassis.pid_wait(); - - /* - grab third match load - */ - - Little_Mech_Mac.set(1); - - chassis.pid_drive_set(24, 60, true); - chassis.pid_wait(); - - top_intake(127); - bottom_intake(127); - - //intake_top.move(127); - //intake_bottom.move(127); - - pros::delay(1300); - - Little_Mech_Mac.set(0); - - chassis.pid_drive_set(-20, 60, true); - chassis.pid_wait(); - - top_intake(0); - bottom_intake(0); - - chassis.pid_turn_set(-88, 60, true); - chassis.pid_wait(); - - chassis.pid_drive_set(-11.9, 60, true); - chassis.pid_wait(); - - /* - go to blue side - */ - - chassis.pid_turn_set(-3, 60, true); - chassis.pid_wait_quick(); - - chassis.pid_drive_set(-85, 80, true); - chassis.pid_wait(); - - chassis.pid_turn_set(86, 60, true); - chassis.pid_wait(); - - chassis.pid_drive_set(-11, 80, true); - chassis.pid_wait(); - - /* - score third set of match load - */ - - chassis.pid_turn_set(176, 60, true); - chassis.pid_wait(); - - chassis.pid_drive_set(-19.5, 60, true); - chassis.pid_wait(); - - top_intake(127); - bottom_intake(127); - - trapdoor.set(1); - - pros::delay(2000); - - /* - empty second match loader - */ - - - - Little_Mech_Mac.set(1); - - chassis.pid_drive_set(35, 80, true); - trapdoor.set(0); - - pros::delay(2200); - - /* - Score second 6 on long goal - */ - - Little_Mech_Mac.set(0); - - pros::delay(100); - - chassis.pid_turn_set(179, 60, true); - - chassis.pid_drive_set(-33.5, 60, true); - chassis.pid_wait(); - - top_intake(127); - bottom_intake(127); - - trapdoor.set(1); - - pros::delay(2000); - - chassis.odom_xyt_set(0, 0, 180); - - chassis.pid_drive_set(5, 127, true); - chassis.pid_wait(); - - chassis.pid_turn_set(-95, 127, true); - chassis.pid_wait(); - - chassis.pid_drive_set(48, 127, true); - chassis.pid_wait(); - - chassis.pid_turn_set(178, 127, true); - chassis.pid_wait(); - - chassis.pid_drive_set(100, 127, false); - chassis.pid_wait_quick(); - -} - -void new_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); - - 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(); - - pros::delay(400); - - 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); - pros::delay(300); - Little_Mech_Mac.set(0); - chassis.pid_wait(); - - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); - - chassis.pid_drive_set(-10, 100, true); - chassis.pid_wait(); - - - 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(); - - chassis_drive_wall(600, 127, false); - - chassis.pid_turn_set(-88, 80, true); - chassis.pid_wait(); - - chassis_drive_wall(1450, 127, false); - - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait(); - - chassis.pid_drive_set(-26, 80, true); - chassis.pid_wait(); - - chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 150_ms, 150_ms); - - - chassis.pid_swing_set(RIGHT_SWING, -127, 127, -30, true); - pros::delay(200); - 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(); - - chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - - //7 ball score - chassis.pid_drive_set(-1, 1, false); - - pros::delay(1600); - top_intake(-80); - top_intake_score(-70); - bottom_intake(-80); - pros::delay(300); - bottom_intake(60); - top_intake(60); - top_intake_score(-40); - pros::delay(500); - - //grab 7th ball - bottom_intake(127); - top_intake(127); - top_intake_score(0); - chassis.pid_turn_set(-135, 60, true); - chassis.pid_wait(); - - chassis.pid_drive_set(7, 80, true); - chassis.pid_wait(); - - chassis.pid_drive_set(-5, 30, true); - chassis.pid_wait_quick_chain(); - top_intake(70); - top_intake_score(-40); - pros::delay(1000); - - //line up for first match loader - - chassis.pid_drive_set(50, 80, true); - chassis.pid_wait(); - - 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(9.2, 60, true); - chassis.pid_wait(); - pros::delay(1100); - - //cross to other side - chassis.pid_drive_set(-8, 80, true); - chassis.pid_wait_quick_chain(); - - Little_Mech_Mac.set(0); - - chassis.pid_turn_set(-39, 100, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(8, 100, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(1, 100, true); - chassis.pid_wait_quick_chain(); - - top_intake(0); - bottom_intake(0); - top_intake_score(0); - - chassis.pid_drive_set(52, 100, true); - chassis.pid_wait_quick_chain(); - - //score first match loader - - 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(); - - chassis.pid_turn_set(0, 100, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(-9.2, 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(2000); - chassis.pid_wait(); - - //grab second match loader - - Little_Mech_Mac.set(1); - - trapdoor.set(1); - - chassis.pid_drive_set(27.4, 60, true); - chassis.pid_wait(); - pros::delay(1300); - - //score second match loader - - chassis.pid_drive_set(-29.5, 70, true); - chassis.pid_wait(); - - 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); - - pros::delay(1500); - chassis.pid_wait(); - Little_Mech_Mac.set(0); - - // line up for clear - - discore_mech.set(0); - - chassis.pid_turn_set(90, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(60, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(55, 60, true); - chassis.pid_wait_quick_chain(); - - - - - /* - chassis.pid_drive_set(7.3, 127, true); - chassis.pid_wait_quick_chain(); - - trapdoor.set(1); - - chassis.pid_swing_set(LEFT_SWING, 87, 84, 31, true); - chassis.pid_wait_quick_chain(); - - // chassis.pid_drive_set(5, 127, true); - // chassis.pid_wait_quick_chain(); - - bottom_intake(127); - top_intake(127); - top_intake_score(127); - - //clear the 6 ball from the park zone - - chassis.pid_drive_set(18, 70, true); - - pros::delay(500); - Little_Mech_Mac.set(1); - pros::delay(500); - Little_Mech_Mac.set(0); - pros::delay(500); - - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 300_ms, 3_in, 600_ms, 600_ms); - - Little_Mech_Mac.set(1); - pros::delay(200); - Little_Mech_Mac.set(0); - - chassis.pid_drive_set(50, 75, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); - - 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(0, 80, true); - chassis.pid_wait_quick_chain(); - - chassis_drive_wall(400, 100, false); - LOW GOAL SCORE FOR LATER - - chassis.pid_turn_set(-170, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(11, 90, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(80, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(-25, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(135, 60, true); - chassis.pid_wait_quick_chain(); - - intake_piston.set(1); - - chassis.pid_drive_set(10, 40, true); - pros::delay(400); - top_intake(-127); - bottom_intake(-127); - pros::delay(100); - top_intake_score(-127); - chassis.pid_wait_quick_chain(); - bottom_intake(-53); - - chassis.pid_drive_set(-1.5, 80, true); - chassis.pid_wait_quick_chain(); - - pros::delay(2000); - - chassis.pid_drive_set(-4, 80, true); - chassis.pid_wait_quick_chain(); - - intake_piston.set(0); - - chassis.pid_turn_set(80, 60, true); - chassis.pid_wait_quick_chain(); - - top_intake(127); - top_intake_score(127); - bottom_intake(127); - - chassis.pid_drive_set(42.5, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(30, 60, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(19, 80, true); - chassis.pid_wait_quick_chain(); - - */ - - ///new stuff - chassis.pid_drive_set(13.5, 60, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(0, 60, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(-17, 60, true); - pros::delay(800); - trapdoor.set(0); - top_intake(127); - bottom_intake(127); - top_intake_score(127); - chassis.pid_wait(); - pros::delay(1200); - - Little_Mech_Mac.set(1); - - chassis.pid_drive_set(28.5, 60, true); - chassis.pid_wait(); - trapdoor.set(1); - pros::delay(1100); - - chassis.pid_drive_set(-8, 80, true); - chassis.pid_wait_quick_chain(); - - Little_Mech_Mac.set(0); - - chassis.pid_turn_set(133, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(7, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(-180, 80, true); - chassis.pid_wait_quick_chain(); - - top_intake(0); - bottom_intake(0); - top_intake_score(0); - - chassis.pid_drive_set(56, 80, true); - chassis.pid_wait_quick_chain(); - - //score first match loader - - chassis.pid_turn_set(-120, 100, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(8, 100, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(180, 100, true); - chassis.pid_wait_quick_chain(); - - 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); - pros::delay(200); - intake_top_score.move(-60); - pros::delay(300); - intake_top.move(127); - intake_top_score.move(127); - trapdoor.set(0); - - pros::delay(1500); - chassis.pid_wait(); - - //grab second match loader - Little_Mech_Mac.set(1); - - trapdoor.set(1); - - chassis.pid_drive_set(27.4, 60, true); - chassis.pid_wait(); - pros::delay(1250); - - //score second match loader - - chassis.pid_drive_set(-32.5, 70, true); - chassis.pid_wait(); - - 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); - - - pros::delay(1300); - chassis.pid_wait(); - Little_Mech_Mac.set(0); - - chassis.pid_drive_set(8.3, 127, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_swing_set(LEFT_SWING, -93, 85, 31, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(20, 127, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(23, 127, true); - chassis.pid_wait(); - - - -} - - - - - - -void skills() { - pros::Task task_anti_jam(anti_jam_auton); - - discore_mech.set(0); - //grab middle balls - trapdoor.set(1); - top_intake(127); - bottom_intake(127); - top_intake_score(127); - - chassis.pid_drive_set(2, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(-46, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(28, 80, true); - pros::delay(400); - Little_Mech_Mac.set(1); - chassis.pid_wait(); - // TO make shure that we are grabbing all 4 balls - pros::delay(100); - - chassis.pid_drive_set(-1, 80, true); - chassis.pid_wait_quick_chain(); - - //score middle balls - Little_Mech_Mac.set(0); - chassis.pid_turn_set(-135, 80, true); - chassis.pid_wait_quick_chain(); - //Moving the intake backwards to prevent jamming - top_intake(-10); - top_intake_score(-10); - - chassis.pid_drive_set(-14, 60, true); - chassis.pid_wait_quick_chain(); - - top_intake(127); - bottom_intake(127); - top_intake_score(-80); - - //Little_Mech_Mac.set(0); - - pros::delay(1500); - - //line up with first match loader - - chassis.pid_drive_set(25, 80, true); - chassis.pid_wait_quick_chain(); - - top_intake_score(127); - - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait_quick_chain(); - - while (distance_front_l.get_distance() > 600){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - - chassis.pid_turn_set(-90, 60, true); - chassis.pid_wait_quick_chain(); - - while (distance_front_l.get_distance() > 580){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait_quick_chain(); - - Little_Mech_Mac.set(1); - - trapdoor.set(1); - - chassis.pid_drive_set(13, 60, true); - chassis.pid_wait(); - pros::delay(950); - - //cross to other side - - chassis.pid_drive_set(-10, 80, true); - chassis.pid_wait_quick_chain(); - - Little_Mech_Mac.set(0); - - chassis.pid_turn_set(-90, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(6.5, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(1, 80, true); - chassis.pid_wait_quick_chain(); - - top_intake(0); - bottom_intake(0); - top_intake_score(0); - - chassis.pid_drive_set(78, 80, true); - chassis.pid_wait(); - - //score first match loader - - chassis.pid_turn_set(90, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(4.2, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(0, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(-13, 80, true); - - // pros::delay(100); - // top_intake(-20); - // bottom_intake(-20); - // top_intake_score(30); - // chassis.pid_wait(); - chassis.pid_wait(); - - trapdoor.set(0); - - top_intake(127); - bottom_intake(127); - top_intake_score(127); - - pros::delay(1700); - - //grab second match loader - - Little_Mech_Mac.set(1); - - trapdoor.set(1); - - chassis.pid_drive_set(28, 60, true); - chassis.pid_wait(); - pros::delay(750); - - //score second match loader - - chassis.pid_turn_set(2, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(-33.5, 80, true); - // pros::delay(800); - // top_intake(-30); - // bottom_intake(-30); - // top_intake_score(30); - chassis.pid_wait(); - - trapdoor.set(0); - top_intake(127); - bottom_intake(127); - top_intake_score(127); - pros::delay(2000); - Little_Mech_Mac.set(0); - - //The part were we line up for clearing - /* clear - - chassis.pid_drive_set(8, 127, true); - chassis.pid_wait_quick_chain(); - - trapdoor.set(1); - - chassis.pid_turn_set(50, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(20, 127, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_swing_set(LEFT_SWING, 87, 85, 0, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(86, 80, true); - chassis.pid_wait_quick_chain(); - - // chassis.pid_drive_set(5, 127, true); - // chassis.pid_wait_quick_chain(); - - bottom_intake(127); - top_intake(127); - top_intake_score(127); - - chassis.pid_drive_set(70, 100, true); - pros::delay(250); - Little_Mech_Mac.set(1); - pros::delay(800); - Little_Mech_Mac.set(0); - chassis.pid_wait(); - - Little_Mech_Mac.set(0); - chassis.pid_turn_set(95, 127, true); - pros::delay(200); - Little_Mech_Mac.set(0); - pros::delay(100); - - */ - - chassis.pid_drive_set(6, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(90, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(60, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(0, 80, true); - chassis.pid_wait_quick_chain(); - - while (distance_front_l.get_distance() > 600){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - - chassis.pid_turn_set(90, 60, true); - chassis.pid_wait_quick_chain(); - - while (distance_front_l.get_distance() > 610){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - - chassis.pid_turn_set(0, 80, true); - chassis.pid_wait_quick_chain(); - - Little_Mech_Mac.set(1); - trapdoor.set(1); - - chassis.pid_drive_set(21, 60, true); - chassis.pid_wait(); - pros::delay(1050); - - //cross to other side - - - chassis.pid_drive_set(-10, 80, true); - chassis.pid_wait_quick_chain(); - - Little_Mech_Mac.set(0); - - chassis.pid_turn_set(90, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(5.5, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(179, 80, true); - chassis.pid_wait_quick_chain(); - - top_intake(0); - bottom_intake(0); - top_intake_score(0); - - chassis.pid_drive_set(78, 80, true); - chassis.pid_wait(); - - //score first match loader - - chassis.pid_turn_set(-90, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(5, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(-12, 80, true); - // pros::delay(200); - // intake_top.move(-40); - // intake_bottom.move(-40); - // intake_top_score.move(40); - chassis.pid_wait(); - - trapdoor.set(0); - - top_intake(127); - bottom_intake(127); - top_intake_score(127); - - pros::delay(1700); - - //grab second match loader - - Little_Mech_Mac.set(1); - - trapdoor.set(1); - - chassis.pid_drive_set(28, 80, true); - chassis.pid_wait(); - pros::delay(1450); - - //score second match loader - - chassis.pid_drive_set(-35.8, 60, true); - // pros::delay(800); - // top_intake(-20); - // bottom_intake(-20); - // top_intake_score(20); - chassis.pid_wait(); - - trapdoor.set(0); - - top_intake(127); - bottom_intake(127); - top_intake_score(127); - - trapdoor.set(0); - Little_Mech_Mac.set(0); - pros::delay(2000); - - - //The part were we line up for parking - - chassis.pid_drive_set(8.5, 127, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(-130, 80, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(20, 127, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_swing_set(LEFT_SWING, -93, 85, 0, true); - chassis.pid_wait_quick_chain(); - - chassis.pid_turn_set(-94, 80, true); - chassis.pid_wait_quick_chain(); - - // chassis.pid_drive_set(5, 127, true); - // chassis.pid_wait_quick_chain(); - - bottom_intake(127); - top_intake(127); - top_intake_score(127); - - chassis.pid_drive_set(45, 127, true); - pros::delay(250); - Little_Mech_Mac.set(1); - pros::delay(800); - Little_Mech_Mac.set(0); - chassis.pid_wait(); - - - -} - -void nor_call_skills() { - intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); - /* Try #1*/ - chassis.pid_drive_set(10, 127, true); - pros::delay(1000); - chassis.pid_drive_set(-2, 127, true); - pros::delay(100); - chassis.pid_drive_set(10, 127, true); - pros::delay(1000); - chassis.pid_drive_set(-2, 127, true); - pros::delay(100); - chassis.pid_drive_set(10, 127, true); - pros::delay(500); - chassis.pid_drive_set(-2, 127, true); - pros::delay(100); - - - /*Try #2 - chassis.pid_swing_set(LEFT_SWING, 90, 127, 100, false); - pros::delay(1000); - chassis.pid_swing_set(RIGHT_SWING, -90, 127, 100, false); - pros::delay(1000); - chassis.pid_drive_set(10, 127, true); - pros::delay(5000); - */ - -} - -/* old -void new_elim_auton(){ - // pros::Task anti (anti_jam_auton); - trapdoor.set(1); - chassis.pid_drive_set(14.1, 60, true); + chassis.pid_swing_set(RIGHT_SWING, -127, 127, -30, true); + pros::delay(300); + 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(); - chassis.pid_turn_set(-45.9, 100, true); - chassis.pid_wait(); - - bottom_intake(127); - top_intake(127); - - chassis.pid_drive_set(20, 30, true); - chassis.pid_wait(); - - //score 3 in middle goal - chassis.pid_drive_set(-4, 60, true); - chassis.pid_wait(); - - chassis.pid_turn_set(180, 80, true); - chassis.pid_wait(); - - while (distance_front.get_distance() > 655){ - chassis.pid_drive_set(1000000, 40); - } - L1.brake(); - L2.brake(); - L3.brake(); - R1.brake(); - R2.brake(); - R3.brake(); - - pros::delay(50); - - chassis.pid_turn_set(-91, 60, 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(); + chassis.pid_swing_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); - pros::delay(100); + //7 ball score + chassis.pid_drive_set(-1, 1, false); - Little_Mech_Mac.set(1); + pros::delay(1600); + top_intake(-80); + top_intake_score(-70); + bottom_intake(-80); + pros::delay(300); + bottom_intake(60); + top_intake(60); + top_intake_score(-40); + pros::delay(1000); - chassis.pid_turn_set(179, 80, true); + //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(127); - - chassis.pid_drive_set(15.5, 60, true); + chassis.pid_drive_set(7, 80, true); chassis.pid_wait(); - pros::delay(90); + chassis.pid_drive_set(-5, 30, true); + chassis.pid_wait_quick_chain(); + top_intake(70); + top_intake_score(-30); + pros::delay(1200); + + //line up for first match loader - chassis.pid_drive_set(-31, 80, true); + chassis.pid_drive_set(50, 80, true); chassis.pid_wait(); - - intake_top.move(127); - intake_bottom.move(127); - trapdoor.set(0); - chassis.pid_drive_set(-2, 80, true); - chassis.pid_wait_quick(); -} -*/ - + 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(); -/* 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_drive_set(9, 60, true); + chassis.pid_wait(); + chassis.pid_drive_set(1000, 10, true); + pros::delay(1150); - chassis.pid_drive_constants_set(22, 0, 130); + //cross to other side + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); - intake_bottom.move(127); + Little_Mech_Mac.set(0); - chassis.pid_swing_set(RIGHT_SWING, 80, 60, 17); - chassis.pid_wait_quick(); + chassis.pid_turn_set(-39, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(10, 60, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(8, 100, true); + chassis.pid_wait_quick_chain(); - pros::delay(350); - //Turn that on when we have the pump - right_rush_mech.set(0); - pros::delay(250); + chassis.pid_turn_set(1, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-14, -80, true); - chassis.pid_wait_quick(); + top_intake(0); + bottom_intake(0); + top_intake_score(0); - chassis.pid_turn_set(125, 60, true); - intake_bottom.move(80); - intake_top.move(0); - chassis.pid_wait_quick(); + chassis.pid_drive_set(52, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(RIGHT_SWING, 90, 80, 40, true); - chassis.pid_wait_quick(); + //score first match loader - intake_bottom.move(0); + chassis.pid_turn_set(60, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_odom_set({{14.6, 36.8}, fwd, 100}, true); - chassis.pid_wait_quick(); + chassis.pid_drive_set(7.8, 100, true); + chassis.pid_wait_quick_chain(); - middle_stage.set(1); - Little_Mech_Mac.set(1); + chassis.pid_turn_set(0, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(45, 60, true); - chassis.pid_wait(); - - chassis.pid_drive_set(8.5, 60, true); + chassis.pid_drive_set(-9.2, 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(550); - - chassis.pid_odom_set({{-11_in, 8_in}, rev, 100}, true); - chassis.pid_wait_quick(); - - middle_stage.set(0); + pros::delay(2000); + chassis.pid_wait(); - chassis.pid_turn_set(173, 60, true); - chassis.pid_wait_quick(); + //grab second match loader Little_Mech_Mac.set(1); - chassis.pid_drive_set(13, 60, true); - chassis.pid_wait(); + trapdoor.set(1); - pros::delay(250); + chassis.pid_drive_set(27.5, 60, true); + chassis.pid_wait(); + chassis.pid_drive_set(1000, 10, true); + pros::delay(1300); - chassis.pid_drive_set(-31, 80, true); + //score second match loader + + chassis.pid_drive_set(-29.5, 70, true); chassis.pid_wait(); + 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); -} -*/ + pros::delay(1500); + chassis.pid_wait(); + Little_Mech_Mac.set(0); + + // line up for clear + + top_intake(127); + bottom_intake(127); + top_intake_score(127); + discore_mech.set(0); -/* old -void blue_bottom_elims() { + chassis.pid_turn_set(90, 80, true); + chassis.pid_wait_quick_chain(); 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_swing_set(LEFT_SWING, -105, 60); - chassis.pid_wait(); + chassis.pid_drive_set(60, 80, true); + chassis.pid_wait_quick_chain(); - intake_bottom.move(127); + chassis.pid_turn_set(35, 60, true); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(RIGHT_SWING, -150, 60, 30); - chassis.pid_wait(); + chassis.pid_drive_set(16.5, 127, true); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, -100, 60, 20); - chassis.pid_wait(); - //Turn that on when we have the pump - // right_rush_mech.set(0); - pros::delay(200); + chassis.pid_turn_set(0, 60, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-15, -40, true); - chassis.pid_wait(); + chassis.pid_drive_set(-11, 60, true); + chassis.pid_wait_quick_chain(); + trapdoor.set(0); - chassis.pid_turn_set(-140, -60, true); - chassis.pid_wait_quick(); + //Scroing on the + chassis.pid_drive_set(0.5, 60, true); + pros::delay(700); - chassis.pid_swing_set(LEFT_SWING, -90, 60, 30, true); - chassis.pid_wait_quick(); - chassis.pid_odom_set({{13.5_in, 14_in}, fwd, 80}, true); - chassis.pid_wait(); + //Intaking the 3rd machloader + Little_Mech_Mac.set(1); - chassis.pid_turn_set(-172, 60, true); - chassis.pid_wait_quick(); - //wall_alignment_R(1000); + + ///new stuff - // blooper Little_Mech_Mac.set(1); - - chassis.pid_drive_set(18, 50, true); + chassis.pid_drive_set(27.9, 60, true); chassis.pid_wait(); + trapdoor.set(1); + pros::delay(1200); - pros::delay(500); - - intake_top.move(127); - - chassis.pid_drive_set(-32, 70, true); - chassis.pid_wait(); + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); - trapdoor.set(0); -} -*/ + Little_Mech_Mac.set(0); + chassis.pid_turn_set(133, 80, 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_drive_set(7, 80, 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_turn_set(-180, 80, true); + chassis.pid_wait_quick_chain(); - right_rush_mech.set(1); - pros::delay(300); + top_intake(0); + bottom_intake(0); + top_intake_score(0); - chassis.pid_swing_set(RIGHT_SWING, -90, 60); - chassis.pid_wait(); + chassis.pid_drive_set(56, 80, true); + chassis.pid_wait_quick_chain(); - intake_top.move(127); - intake_bottom.move(127); + //score first match loader - chassis.pid_swing_set(LEFT_SWING, -160, 60); - chassis.pid_wait(); + chassis.pid_turn_set(-120, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(RIGHT_SWING, -100, 60, 20); - chassis.pid_wait(); + chassis.pid_drive_set(8, 100, true); + chassis.pid_wait_quick_chain(); - right_rush_mech.set(0); - pros::delay(350); + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-20, 40, true); + chassis.pid_drive_set(-9, 100, true); chassis.pid_wait(); - chassis.pid_turn_set(-150, 60, true); - chassis.pid_wait_quick(); - + 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_bottom.move(50); + intake_top_score.move(127); + intake_bottom.move(127); + trapdoor.set(0); - chassis.pid_swing_set(RIGHT_SWING, -80, 60, 20, true); + pros::delay(1500); chassis.pid_wait(); - intake_bottom.move(0); + //grab second match loader + Little_Mech_Mac.set(1); - chassis.pid_odom_set({{-16, 41}, fwd, 60}, true); - chassis.pid_wait(); + trapdoor.set(1); - chassis.pid_turn_set(-45, 60, true); + chassis.pid_drive_set(27.4, 60, true); chassis.pid_wait(); + chassis.pid_drive_set(1000, 10, true); + pros::delay(1250); - middle_stage.set(1); - - pros::delay(1000); + //score second match loader - chassis.pid_drive_set(8.5, 60, true); + chassis.pid_drive_set(-32.5, 70, true); chassis.pid_wait(); + 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_bottom.move(127); + intake_top_score.move(127); + trapdoor.set(0); - pros::delay(1500); - chassis.pid_odom_set({{17.5_in, 13_in}, rev, 80}, true); + pros::delay(1300); chassis.pid_wait(); + Little_Mech_Mac.set(0); - middle_stage.set(0); - - pros::delay(250); - - chassis.pid_turn_set(180, 60, true); - chassis.pid_wait(); - - // blooper - - chassis.pid_drive_set(12, 60, true); - chassis.pid_wait(); + 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 */ @@ -2784,13 +1290,23 @@ void pid_tune(){ intake_top_score.move(127); trapdoor.set(0); } + +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(){ - color = "B"; + 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 457bf7e..02e8ccc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -98,7 +98,7 @@ std::string color = "R"; // against R or B; press UP+X to change; x for disabled bool control_to_controller = true; int middgoal_Srore = 0; void color_sort_top() { - color_sort.set_integration_time(10); + color_sort.set_integration_time(3); while (true) { int hue_lower; int hue_higher; @@ -119,7 +119,7 @@ void color_sort_top() { middgoal_Srore = 0; } - bool in_proximity = color_sort.get_proximity() > 220; + bool in_proximity = color_sort.get_proximity() > 200; if (middgoal_Srore == 0 && in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)) { control_to_controller = false; @@ -146,7 +146,7 @@ void initialize() { // Set the color of the balls you want to throw out here - color = "R"; + color = "B"; discore_mech.set(1); trapdoor.set(1); //intake_piston.set(1); @@ -169,19 +169,10 @@ void initialize() { //pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"right safe", new_skills}, + {"right safe", skills}, {"right solo", pid_tune }, {"elims auton 3 goals", elims_mid_control}, {"elims left", left_elims_7ball}, - {"left side 4 push", left_elims_quick}, - {"Skills", left_elims_quick}, - {"Left Side Solo", left_elims_quick}, - {"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} }); chassis.initialize(); @@ -344,8 +335,8 @@ void opcontrol() { else if (master.get_digital(DIGITAL_A)) { intake_bottom.move(127); - intake_top.move(65); - intake_top_score.move(-40); + intake_top.move(55); + intake_top_score.move(-30); } else if (control_to_controller){ intake_bottom.move(0); @@ -364,7 +355,7 @@ void opcontrol() { if (pros::millis() - r2_time >= 1000) { intake_bottom.move(-40); intake_top.move(-127); - intake_top_score.move(-127); + intake_top_score.move(-40); } if (!master.get_digital(DIGITAL_R2)) { @@ -373,10 +364,10 @@ void opcontrol() { } } if (r1_active) { - if (pros::millis() - r1_time >= 50) { + if (pros::millis() - r1_time >= 150) { intake_bottom.move(127); intake_top.move(65); - intake_top_score.move(-50); + intake_top_score.move(-40); } else { intake_bottom.move(-75); diff --git a/src/wall_tracking.cpp b/src/wall_tracking.cpp index f033208..2679a4d 100644 --- a/src/wall_tracking.cpp +++ b/src/wall_tracking.cpp @@ -26,23 +26,31 @@ float d_KD = 0; bool stop_task = false; float targer_distance = 0; -void chassis_drive_wall(float distance, float DRIVE_SPEED, bool match_loader) { +void chassis_drive_wall(float distance, float DRIVE_SPEED, bool chain, bool back_sensor) { - if (match_loader){ + if (back_sensor){ float distance_for_chassis_ml = (distance_match_loader.get_distance() - distance)/24.4; master.print(0, 0, "%d", distance_match_loader.get_distance() ); master.print(0, 0, "%.1f", distance_for_chassis_ml); chassis.pid_drive_set(distance_for_chassis_ml, DRIVE_SPEED, true); - chassis.pid_wait(); + if (chain){ + chassis.pid_wait_quick_chain(); + } else { + chassis.pid_wait(); + } } - if (!match_loader){ + 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); - chassis.pid_wait(); + if (chain){ + chassis.pid_wait_quick_chain(); + } else { + chassis.pid_wait(); + } } } From 32984189fe95b4f593c732b735c1b3f4e6321295 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Sat, 21 Feb 2026 22:40:45 -0800 Subject: [PATCH 12/15] fixed skills --- project.pros | 4 +- src/autons.cpp | 109 +++++++++++++++++++++++++------------------------ src/main.cpp | 5 ++- 3 files changed, 60 insertions(+), 58 deletions(-) diff --git a/project.pros b/project.pros index 1ba9746..5f97dc0 100644 --- a/project.pros +++ b/project.pros @@ -1,7 +1,7 @@ { "py/object": "pros.conductor.project.Project", "py/state": { - "project_name": "2550R", + "project_name": "2550R Skills", "target": "v5", "templates": { "EZ-Template": { @@ -426,7 +426,7 @@ "upload_options": { "description": "2550R", "icon": "power", - "slot": 1 + "slot": 2 }, "use_early_access": false } diff --git a/src/autons.cpp b/src/autons.cpp index ea4b4e1..5703172 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -930,16 +930,10 @@ void skills(){ //7 ball score chassis.pid_drive_set(-1, 1, false); - pros::delay(1600); + pros::delay(1000); top_intake(-80); top_intake_score(-70); bottom_intake(-80); - pros::delay(300); - bottom_intake(60); - top_intake(60); - top_intake_score(-40); - pros::delay(1000); - //grab 7th ball bottom_intake(127); top_intake(127); @@ -954,7 +948,7 @@ void skills(){ chassis.pid_wait_quick_chain(); top_intake(70); top_intake_score(-30); - pros::delay(1200); + pros::delay(1600); //line up for first match loader @@ -968,9 +962,9 @@ void skills(){ Little_Mech_Mac.set(1); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(9, 60, true); + chassis.pid_drive_set(9, 80, true); chassis.pid_wait(); - chassis.pid_drive_set(1000, 10, true); + chassis.pid_drive_set(1000, 60, true); pros::delay(1150); //cross to other side @@ -1006,16 +1000,16 @@ void skills(){ chassis.pid_turn_set(0, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-9.2, 100, true); + chassis.pid_drive_set(-9.8, 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); + 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); @@ -1030,9 +1024,9 @@ void skills(){ trapdoor.set(1); - chassis.pid_drive_set(27.5, 60, true); + chassis.pid_drive_set(27.5, 80, true); chassis.pid_wait(); - chassis.pid_drive_set(1000, 10, true); + chassis.pid_drive_set(1000, 60, true); pros::delay(1300); //score second match loader @@ -1040,12 +1034,12 @@ void skills(){ chassis.pid_drive_set(-29.5, 70, true); chassis.pid_wait(); - 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); + 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); @@ -1077,12 +1071,11 @@ void skills(){ chassis.pid_turn_set(0, 60, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-11, 60, true); + chassis.pid_drive_set(-13, 60, true); chassis.pid_wait_quick_chain(); trapdoor.set(0); - //Scroing on the - chassis.pid_drive_set(0.5, 60, true); + chassis.pid_drive_set(-1, 10, true); pros::delay(700); @@ -1093,9 +1086,10 @@ void skills(){ ///new stuff Little_Mech_Mac.set(1); - chassis.pid_drive_set(27.9, 60, true); + chassis.pid_drive_set(27.9, 80, true); chassis.pid_wait(); trapdoor.set(1); + chassis.pid_drive_set(1000, 60, true); pros::delay(1200); chassis.pid_drive_set(-8, 80, true); @@ -1134,12 +1128,12 @@ void skills(){ 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(-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); @@ -1153,22 +1147,22 @@ void skills(){ trapdoor.set(1); - chassis.pid_drive_set(27.4, 60, true); + chassis.pid_drive_set(27.4, 80, true); chassis.pid_wait(); - chassis.pid_drive_set(1000, 10, true); + chassis.pid_drive_set(1000, 60, true); pros::delay(1250); //score second match loader chassis.pid_drive_set(-32.5, 70, true); chassis.pid_wait(); - - 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); + 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); @@ -1279,16 +1273,23 @@ void wall_alignment_test() { R3.brake(); } void pid_tune(){ + Little_Mech_Mac.set(1); + chassis.pid_drive_set(27.4, 60, true); + chassis.pid_wait(); + chassis.pid_drive_set(1000, 40, true); + bottom_intake(127); + top_intake(127); + pros::delay(1200); - trapdoor.set(1); - 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); + // trapdoor.set(1); + // 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); } void intake_test(){ diff --git a/src/main.cpp b/src/main.cpp index 02e8ccc..4537458 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -146,7 +146,7 @@ void initialize() { // Set the color of the balls you want to throw out here - color = "B"; + color = "x"; discore_mech.set(1); trapdoor.set(1); //intake_piston.set(1); @@ -355,7 +355,8 @@ void opcontrol() { if (pros::millis() - r2_time >= 1000) { intake_bottom.move(-40); intake_top.move(-127); - intake_top_score.move(-40); + pros::delay(200); + intake_top_score.move(-30); } if (!master.get_digital(DIGITAL_R2)) { From 985e290a042c53246022493f6206ed46f7ca8fc2 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Fri, 27 Feb 2026 21:18:56 -0800 Subject: [PATCH 13/15] drive_wall working, needs pid tune --- include/autons.hpp | 7 +- include/subsystems.hpp | 1 + include/wall_tracking.hpp | 2 +- project.pros | 2 +- src/autons.cpp | 440 +++++++++++++++++++++++++++++++++----- src/main.cpp | 45 ++-- src/wall_tracking.cpp | 88 +++++--- 7 files changed, 470 insertions(+), 115 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index 8ea9311..c8ba88b 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -16,11 +16,8 @@ void default_constants(); void empty(); -void skills(); -void skills_before_changing_the_wall(); -void skills_without_odom(); -void new_skills(); -void nor_call_skills(); +void norcal_skills(); +void safe_skills(); void left_elims_quick_ml(); void left_elims(); diff --git a/include/subsystems.hpp b/include/subsystems.hpp index 9d1525a..a3ff0aa 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -27,6 +27,7 @@ inline pros::Imu inertial(11); 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 diff --git a/include/wall_tracking.hpp b/include/wall_tracking.hpp index 3454a06..3be8581 100644 --- a/include/wall_tracking.hpp +++ b/include/wall_tracking.hpp @@ -19,7 +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); +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 5f97dc0..2efad5d 100644 --- a/project.pros +++ b/project.pros @@ -1,7 +1,7 @@ { "py/object": "pros.conductor.project.Project", "py/state": { - "project_name": "2550R Skills", + "project_name": "2550R S", "target": "v5", "templates": { "EZ-Template": { diff --git a/src/autons.cpp b/src/autons.cpp index 5703172..1bd4836 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -119,6 +119,8 @@ void anti_jam_auton(){ } } + +int auton_count_color = 0; void color_sort_top_auton() { color_sort.set_integration_time(10); while (true) { @@ -136,15 +138,22 @@ void color_sort_top_auton() { } 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 (in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)) { - pros::delay(100); + if (auton_count_color >= 6) { + pros::delay(50); intake_top_score.move(-127); - intake_top.move(30); + intake_top.move(20); pros::delay(300); intake_top.move(top_speed_intake); intake_top_score.move(top_speed_score_intake); } + } } @@ -222,7 +231,7 @@ void left_middle_top(){ //Colorsort color set //Setup - pros::Task color_sor(color_sort_top_auton); + // pros::Task color_sor(color_sort_top_auton); chassis.odom_xyt_set(0_in, 0_in, -30_deg); trapdoor.set(1); @@ -239,10 +248,10 @@ void left_middle_top(){ pros::delay(200); - chassis.pid_turn_set(-138, 90, true); + chassis.pid_turn_set(-136, 90, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-16, 90, true); + chassis.pid_drive_set(-16.4, 90, true); intake_top_score.move(-80); intake_top.move(-80); intake_bottom.move(-80); @@ -252,26 +261,26 @@ void left_middle_top(){ intake_top.move(100); chassis.pid_wait(); - pros::delay(800); + pros::delay(1000); - chassis.pid_drive_set(40.5, 90, true); + chassis.pid_drive_set(39, 90, true); chassis.pid_wait_quick_chain(); chassis.pid_turn_set(179, 90, true); + Little_Mech_Mac.set(1); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(12.1, 70, true); - Little_Mech_Mac.set(1); + chassis.pid_drive_set(14, 60, true); intake_bottom.move(127); top_intake(127); top_intake_score(127); //intake_top.move(127); //intake_top_score.move(127); - chassis.pid_wait(); - pros::delay(850); - chassis.pid_drive_set(-27.8, 75, true); + pros::delay(1000); + + chassis.pid_drive_set(-29.8, 75, true); pros::delay(1200); trapdoor.set(0); chassis.pid_wait(); @@ -400,7 +409,6 @@ void left_elims_quick_ml(){ void left_elims_quick(){ - pros::Task color_sor(color_sort_top_auton); chassis.odom_xyt_set(0_in, 0_in, -30_deg); discore_mech.set(0); trapdoor.set(1); @@ -414,12 +422,12 @@ void left_elims_quick(){ Little_Mech_Mac.set(true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(60, 127, true); + chassis.pid_turn_set(59, 127, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 100_ms, 100_ms); + chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 50_ms, 50_ms); - chassis.pid_drive_set(-11, 127, true); + 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); @@ -761,25 +769,32 @@ void push_solo(){ 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.pid_drive_set(-38, 100, true); + + chassis_drive_wall(460, 100, false, true); + /* + chassis.pid_drive_set(-37.5, 100, true); chassis.pid_wait_quick_chain(); + */ chassis.pid_turn_set(180, 90, true); Little_Mech_Mac.set(1); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(8, 80, true); + chassis.pid_drive_set(10, 100); + trapdoor.set(1); chassis.pid_wait_quick_chain(); - pros::delay(500); + chassis.pid_drive_set(1000, 65, true); + pros::delay(300); - chassis.pid_drive_set(-26.8, 75, true); + chassis.pid_drive_set(-30, 75, true); + pros::delay(650); + trapdoor.set(0); chassis.pid_wait(); Little_Mech_Mac.set(0); - trapdoor.set(0); - pros::delay(1000); + + pros::delay(900); chassis.pid_turn_set(-88, 100, true); chassis.pid_wait_quick_chain(); @@ -809,11 +824,13 @@ void push_solo(){ //Intaking the 2rd machloader - chassis.pid_drive_set(28.4, 80, true); + chassis.pid_drive_set(26.4, 100, true); Little_Mech_Mac.set(1); - chassis.pid_wait(); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(1000, 50); trapdoor.set(1); - pros::delay(200); + pros::delay(300); chassis.pid_drive_set(-3, 100, true); chassis.pid_wait_quick_chain(); @@ -827,36 +844,345 @@ void push_solo(){ top_intake_score(-50); bottom_intake(-50); pros::delay(200); - top_intake(0); - top_intake_score(0); - bottom_intake(0); + top_intake(127); + top_intake_score(127); + bottom_intake(127); chassis.pid_wait_quick_chain(); 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); + } + } + } + + + +} + +void safe_skills(){ + + //grab ball 2 + discore_mech.set(0); + trapdoor.set(1); + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_drive_set(1, 90, true); + chassis.pid_wait_quick_chain(); + chassis.pid_turn_set(-45, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(26, 35.67, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-0.1, 80, true); + chassis.pid_wait_quick_chain(); + + //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(-35.67); + intake_top.move(-67); + pros::delay(300); + intake_top.move(60); + + chassis.pid_drive_set(1, 60, true); - // if(color == "R"){ - // if (color_sort.get_hue() < 0 || color_sort.get_hue() > 10){ - // top_intake_score(0); - // top_intake(0); - // bottom_intake(0); - // } - // } - // if(color == "B"){ - // if (color_sort.get_hue() < 210 || color_sort.get_hue() > 240){ - // top_intake_score(0); - // top_intake(0); - // bottom_intake(0); - // } - // } + while (!(color_sort.get_hue() < 250 && color_sort.get_hue() > 160)){ + pros::delay(10); + } + pros::delay(300); + intake_top_score.move(0); + intake_top.move(0); + intake_bottom.move(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(); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + 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(1000); + chassis.pid_drive_set(-1.5, 40, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1300); + + //rotate to other side + + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); + + Little_Mech_Mac.set(0); + + chassis.pid_turn_set(-39, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(10, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(1, 100, true); + chassis.pid_wait_quick_chain(); + + 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(60, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(6.2, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(0, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-10.2, 100, true); + chassis.pid_wait_quick_chain(); + + trapdoor.set(0); + pros::delay(1950); + trapdoor.set(1); + + // grab third match loader + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_drive_set(6, 80, true); + chassis.pid_wait_quick_chain(); + + 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(1000, 40, true); + pros::delay(1000); + chassis.pid_drive_set(-1.5, 40, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1500); + + //score long goal + 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); + + trapdoor.set(0); + Little_Mech_Mac.set(0); + pros::delay(1800); + //line up for clear + + discore_mech.set(0); + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_drive_set(7.3, 127, true); + chassis.pid_wait_quick_chain(); + trapdoor.set(1); + intake_top_score.move(-127); + + trapdoor.set(1); + + chassis.pid_swing_set(LEFT_SWING, 87, 84, 31, true); + chassis.pid_wait_quick_chain(); + + // chassis.pid_drive_set(5, 127, true); + // chassis.pid_wait_quick_chain(); + + //clear the 6 ball from the park zone + + chassis.pid_drive_set(75, 65, true); + intake_top_score.move(127); + chassis.pid_wait(); + + chassis_drive_wall(1000, 80, false, false); + + //grab ball number 7 + + chassis.pid_turn_set(0, 80, true); + chassis.pid_wait_quick_chain(); + + chassis_drive_wall(1000, 80, false, false); + + //score 8 in mid + + chassis.pid_turn_set(45, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-20, 65, true); + chassis.pid_wait_quick_chain(); + + intake_top_score.move(-60); + intake_top.move(-90); + pros::delay(400); + intake_top_score.move(-33.67); + intake_top.move(57); + + pros::delay(2300); + + //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(1000); + chassis.pid_drive_set(-1.5, 40, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1300); + + //rotate to other side + + chassis.pid_drive_set(-8, 80, true); + chassis.pid_wait_quick_chain(); + + Little_Mech_Mac.set(0); + + chassis.pid_turn_set(141, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(10, 100, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(180, 100, true); + chassis.pid_wait_quick_chain(); + + 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_set(-10.2, 100, true); + chassis.pid_wait_quick_chain(); + + trapdoor.set(0); + pros::delay(1950); + trapdoor.set(1); + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_drive_set(6, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(-160, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_swing_set(RIGHT_SWING, 180, 127, 30, true); + Little_Mech_Mac.set(1); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(1000, 40, true); + pros::delay(1000); + chassis.pid_drive_set(-1.5, 40, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(1300); + + //score long goal + 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); + + trapdoor.set(0); + Little_Mech_Mac.set(0); + pros::delay(2000); + //line up for clear + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.pid_drive_set(7.3, 127, true); + pros::delay(10); + trapdoor.set(1); + chassis.pid_wait_quick_chain(); + + trapdoor.set(1); + + chassis.pid_swing_set(LEFT_SWING, -93, 84, 31, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(40, 65, true); + pros::delay(200); + Little_Mech_Mac.set(1); + chassis.pid_wait(); + Little_Mech_Mac.set(0); + } -void skills(){ +void norcal_skills(){ // pros::Task anti_jam_auton1(anti_jam_auton); chassis.odom_xyt_set(0_in, 0_in, -90_deg); @@ -915,7 +1241,7 @@ void skills(){ chassis.pid_swing_set(RIGHT_SWING, -127, 127, -30, true); - pros::delay(300); + pros::delay(500); top_intake(-80); top_intake_score(-70); bottom_intake(-80); @@ -930,7 +1256,7 @@ void skills(){ //7 ball score chassis.pid_drive_set(-1, 1, false); - pros::delay(1000); + pros::delay(1200); top_intake(-80); top_intake_score(-70); bottom_intake(-80); @@ -962,7 +1288,7 @@ void skills(){ Little_Mech_Mac.set(1); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(9, 80, true); + chassis.pid_drive_set(10, 80, true); chassis.pid_wait(); chassis.pid_drive_set(1000, 60, true); pros::delay(1150); @@ -1086,11 +1412,11 @@ void skills(){ ///new stuff Little_Mech_Mac.set(1); - chassis.pid_drive_set(27.9, 80, true); + chassis.pid_drive_set(28.3, 60, true); chassis.pid_wait(); trapdoor.set(1); chassis.pid_drive_set(1000, 60, true); - pros::delay(1200); + pros::delay(1650); chassis.pid_drive_set(-8, 80, true); chassis.pid_wait_quick_chain(); @@ -1150,7 +1476,7 @@ void skills(){ chassis.pid_drive_set(27.4, 80, true); chassis.pid_wait(); chassis.pid_drive_set(1000, 60, true); - pros::delay(1250); + pros::delay(1650); //score second match loader @@ -1259,7 +1585,7 @@ void pid_test(){ chassis.pid_wait(); } void wall_tracking_test() { - drive_wall(450); + drive_wall(450,127); } void wall_alignment_test() { pros::Task task1(controller_update); @@ -1273,14 +1599,10 @@ void wall_alignment_test() { R3.brake(); } void pid_tune(){ - Little_Mech_Mac.set(1); - chassis.pid_drive_set(27.4, 60, true); - chassis.pid_wait(); - chassis.pid_drive_set(1000, 40, true); - bottom_intake(127); - top_intake(127); - pros::delay(1200); + drive_wall(450,127); + //chassis.pid_drive_set(-30, 127, true); + //chassis.pid_wait(); // trapdoor.set(1); // intake_top.move(-100); // intake_top_score.move(127); diff --git a/src/main.cpp b/src/main.cpp index 4537458..fd70a96 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -94,9 +94,10 @@ void anti_jam(){ } } -std::string color = "R"; // against R or B; press UP+X to change; x for disabled +std::string color = "x"; // against R or B; press UP+X to change; x for disabled 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) { @@ -108,7 +109,7 @@ void color_sort_top() { hue_higher = 240; } else if (color == "R") { hue_lower = 0; - hue_higher = 10; + hue_higher = 20; } else { continue; } @@ -119,16 +120,24 @@ void color_sort_top() { middgoal_Srore = 0; } - bool in_proximity = color_sort.get_proximity() > 200; - - if (middgoal_Srore == 0 && in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)) { + 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(30); - pros::delay(300); + intake_top.move(20); + pros::delay(350); control_to_controller = true; - } else if (middgoal_Srore == 1 && in_proximity && (hue_lower < color_sort.get_hue() && color_sort.get_hue() < hue_higher)){ + } else if (middgoal_Srore == 1 && count_color >= 4){ + count_color = 0; control_to_controller = false; trapdoor.set(0); intake_top_score.move(127); @@ -169,7 +178,7 @@ void initialize() { //pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"right safe", skills}, + {"right safe", pid_tune}, {"right solo", pid_tune }, {"elims auton 3 goals", elims_mid_control}, {"elims left", left_elims_7ball}, @@ -305,7 +314,6 @@ void opcontrol() { bool intake_auto_reverse_enabled = false; // pros::Task anti_jam_T(anti_jam); pros::Task color_sort_task_running(color_sort_top); - //color = "B"; while (true) { chassis.opcontrol_arcade_standard(ez::SPLIT); @@ -326,7 +334,7 @@ void opcontrol() { } else if (master.get_digital(DIGITAL_L2)) { - intake_piston.set(0); + //intake_piston.set(0); intake_bottom.move(127); if (control_to_controller)(intake_top.move(127)); if (control_to_controller)(intake_top_score.move(127)); @@ -353,10 +361,11 @@ void opcontrol() { if (r2_active) { if (pros::millis() - r2_time >= 1000) { - intake_bottom.move(-40); - intake_top.move(-127); - pros::delay(200); - intake_top_score.move(-30); + if (!master.get_digital(DIGITAL_L1) && !master.get_digital(DIGITAL_L2)){ + intake_bottom.move(-40); + intake_top.move(-127); + intake_top_score.move(-10); + } } if (!master.get_digital(DIGITAL_R2)) { @@ -437,7 +446,7 @@ 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; @@ -466,7 +475,7 @@ void opcontrol() { - master.print(0, 0, "%d/%d/%d/%s/%d ", /*L1.get_temperature(int)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); + master.print(0, 0, "%d/%d/%d/%s/%d ", /*L1.get_temperature*/(int)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++; @@ -480,3 +489,5 @@ void opcontrol() { pros::delay(ez::util::DELAY_TIME); } } + + diff --git a/src/wall_tracking.cpp b/src/wall_tracking.cpp index 2679a4d..634b1a8 100644 --- a/src/wall_tracking.cpp +++ b/src/wall_tracking.cpp @@ -19,9 +19,7 @@ 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; + bool stop_task = false; float targer_distance = 0; @@ -29,8 +27,8 @@ 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_match_loader.get_distance() - distance)/24.4; - master.print(0, 0, "%d", distance_match_loader.get_distance() ); + 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){ @@ -55,17 +53,21 @@ void chassis_drive_wall(float distance, float DRIVE_SPEED, bool chain, bool back } -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(); -} +// 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.001; +float d_KD = 0.0021; -void drive_wall_task() { +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; @@ -73,11 +75,39 @@ void drive_wall_task() { float prev_output; float integral; float derivative; - float slue_value = 10; - pros::delay(100); + float arrival_time_B = 0; + float arrival_time_S = 0; + //float slue_value = 10; + chassis_brake(); - while (distance_front_l.get_distance() > targer_distance) { - error = distance_front_l.get_distance() - targer_distance; + while (true) { + + //Big error timeout + if (distance_front_l.get_distance() < distance + 15 && distance_front_l.get_distance() > distance - 15){ + 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) > 100){ + break; + } + + //Small error timeout + if (distance_front_l.get_distance() < distance + 5 && distance_front_l.get_distance() > distance - 5 ){ + 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) > 10){ + break; + } + + error = distance_front_l.get_distance() - distance; derivative = error - prev_error; // if (error == 0){ // error = 300; @@ -85,26 +115,20 @@ void drive_wall_task() { //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); } - stop_task = false; + chassis.drive_set(0,0); + // stop_task = false; chassis_brake(); } From 53af1ffe22af5f01ed910f90bfee4e600bdc4552 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Wed, 4 Mar 2026 22:00:28 -0800 Subject: [PATCH 14/15] WORKIG SKILLS moneymoneymoney --- include/autons.hpp | 7 +- include/subsystems.hpp | 2 +- project.pros | 4 +- src/autons.cpp | 967 ++++++++++++++++++++++++++--------------- src/main.cpp | 10 +- src/wall_tracking.cpp | 16 +- 6 files changed, 637 insertions(+), 369 deletions(-) diff --git a/include/autons.hpp b/include/autons.hpp index c8ba88b..c7011f8 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -19,11 +19,14 @@ void empty(); 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(); @@ -58,14 +61,14 @@ void pid_tune(); /* safe routes */ void left_middle_top(); -void right_safe(); +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/subsystems.hpp b/include/subsystems.hpp index a3ff0aa..e8cbb1e 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -23,7 +23,7 @@ inline pros::Motor R1(14); inline pros::Motor R2(19); inline pros::Motor R3(18); -inline pros::Imu inertial(11); +inline pros::Imu inertial(21); inline pros::Distance distance_back_l(13); // removed sensor inline pros::Distance distance_front_l(9); diff --git a/project.pros b/project.pros index 2efad5d..61b9630 100644 --- a/project.pros +++ b/project.pros @@ -1,7 +1,7 @@ { "py/object": "pros.conductor.project.Project", "py/state": { - "project_name": "2550R S", + "project_name": "2550R TMB", "target": "v5", "templates": { "EZ-Template": { @@ -426,7 +426,7 @@ "upload_options": { "description": "2550R", "icon": "power", - "slot": 2 + "slot": 1 }, "use_early_access": false } diff --git a/src/autons.cpp b/src/autons.cpp index 1bd4836..afb28e2 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -190,15 +190,15 @@ void bottom_intake(int speed_bt){ } void default_constants() { - chassis.pid_drive_constants_set(22, 0, 150); + 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); // large distance turn pid: chassis.pid_turn_constants_set(4.3, 0, 37, 15.0); - chassis.pid_turn_constants_set(3.2, 0, 22, 12.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); @@ -226,534 +226,746 @@ void default_constants() { // AUTONS // //////////////////////////////////////////////////////// - -void left_middle_top(){ - //Colorsort color set - - //Setup - // pros::Task color_sor(color_sort_top_auton); - chassis.odom_xyt_set(0_in, 0_in, -30_deg); +// unfinished +void top_middle_bottom(){ + chassis.odom_xyt_set(0_in, 0_in, -90_deg); trapdoor.set(1); + - intake_bottom.move(127); - intake_top.move(127); - intake_top_score.move(127); + pros::Task color_sor(color_sort_top_auton); + trapdoor.set(1); + chassis.pid_drive_set(21, 90, true); + chassis.pid_wait_quick_chain(); - //3 balls mid - chassis.pid_drive_set(23.8, 90, true); //24.8 with little bill activation + chassis.pid_turn_set(180, 80, true); pros::delay(500); - Little_Mech_Mac.set(true); chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(0); - - pros::delay(200); - chassis.pid_turn_set(-136, 90, true); + 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(); - chassis.pid_drive_set(-16.4, 90, true); - intake_top_score.move(-80); - intake_top.move(-80); - intake_bottom.move(-80); - pros::delay(250); - intake_top_score.move(-80); - intake_bottom.move(100); - intake_top.move(100); + chassis.pid_drive_set(1000, 60); + pros::delay(200); + + chassis.pid_drive_set(-28.8, 75, true); chassis.pid_wait(); + trapdoor.set(0); pros::delay(1000); + trapdoor.set(1); - chassis.pid_drive_set(39, 90, true); - chassis.pid_wait_quick_chain(); + //Scoring on a high goal + pros::delay(1100); + trapdoor.set(1); + Little_Mech_Mac.set(0); - chassis.pid_turn_set(179, 90, true); - Little_Mech_Mac.set(1); - chassis.pid_wait_quick_chain(); + //Getting the 3 balls in the middle + // chassis.pid_drive_set(13.5, 127, true); + // chassis.pid_wait_quick_chain(); + // trapdoor.set(1); - chassis.pid_drive_set(14, 60, true); - intake_bottom.move(127); - top_intake(127); - top_intake_score(127); - //intake_top.move(127); - //intake_top_score.move(127); + // chassis.pid_turn_set(45, 80, true); + // chassis.pid_wait_quick_chain(); - pros::delay(1000); + + chassis.pid_turn_set(-88, 100, true); + chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(16, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-29.8, 75, true); - pros::delay(1200); + //Intaking the balls + // chassis.pid_drive_set(35, 100, true); + // chassis.pid_wait(); + // pros::delay(300); + + chassis.pid_turn_set(-135, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-18, 80, true); + chassis.pid_wait_quick(); trapdoor.set(0); - chassis.pid_wait(); - Little_Mech_Mac.set(0); - pros::delay(1100); + //Scoring the balls + intake_top.move(127); + intake_top_score.move(-60); + pros::delay(600); + intake_top_score.move(0); + intake_top.move(-40); - chassis.pid_drive_set(4, 80, true); - chassis.pid_wait_quick_chain(); + //Grabbing 3 more balls + chassis.pid_drive_set(16, 100, true); + chassis.pid_wait_quick(); + trapdoor.set(0); - chassis.pid_turn_set(-130, 80, true); + chassis.pid_turn_set(90, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); + chassis.pid_drive_set(42, 100, true); + chassis.pid_wait(); + pros::delay(200); + + chassis.pid_turn_set(-45, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-20, 80, true); + //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); - chassis.pid_turn_set(-145, 60, true); - chassis.pid_wait(); } -void left_elims_7ball(){ - pros::Task color_sor(color_sort_top_auton); - +// finished +void left_4_3_push(){ + discore_mech.set(0); trapdoor.set(1); - chassis.odom_xyt_set(0_in, 0_in, -30_deg); - intake_bottom.move(127); intake_top.move(127); intake_top_score.move(127); + intake_bottom.move(127); - chassis.pid_drive_set(24.8, 100, true); + 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(); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-135, 100, true); + chassis.pid_turn_set(-138, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(23, 100, true); + chassis.pid_drive_set(24, 100, true); chassis.pid_wait_quick_chain(); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 + chassis.pid_turn_set(-180, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(15.1, 70, true); - Little_Mech_Mac.set(1); - chassis.pid_wait(); - pros::delay(400); + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_drive_set(-30, 100, true); - pros::delay(1100); - bottom_intake(127); - top_intake(127); - top_intake_score(127); - trapdoor.set(0); - chassis.pid_wait(); + chassis.pid_drive_set(11.8, 70, true); + chassis.pid_wait_quick_chain(); - pros::delay(1700); - trapdoor.set(1); + chassis.pid_drive_set(1000, 40, true); + pros::delay(680); + + 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); + pros::delay(700); + trapdoor.set(1); + + intake_top.move(-60); + intake_top_score.move(-127); + intake_bottom.move(-60); chassis.pid_drive_set(4, 80, true); chassis.pid_wait_quick_chain(); + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); + + discore_mech.set(1); + chassis.pid_turn_set(-130, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); + chassis.pid_swing_set(LEFT_SWING, 180, 90, 22, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-20, 80, true); + chassis.pid_drive_set(-12, 90, true); chassis.pid_wait_quick(); - chassis.pid_turn_set(-145, 60, true); - chassis.pid_wait(); + Little_Mech_Mac.set(0); + + chassis.pid_drive_set(10, 80, true); + chassis.pid_wait_quick_chain(); + + discore_mech.set(0); + + chassis_drive_wall(810, 80, false, false); + + chassis.pid_turn_set(-135, 60, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-35, 80, true); + chassis.pid_wait_quick(); + + 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); + + chassis.pid_drive_set(4.3, 80, true); + chassis.pid_wait_quick(); } -void left_elims_quick_ml(){ +// finished +void left_7_mid(){ - chassis.odom_xyt_set(0_in, 0_in, -90_deg); - pros::Task color_sor(color_sort_top_auton); + discore_mech.set(0); trapdoor.set(1); - chassis.pid_drive_set(22.6, 90, true); + + 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_turn_set(180, 80, true); + 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(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_turn_set(-138, 100, true); + chassis.pid_wait_quick_chain(); - pros::delay(200); + chassis.pid_drive_set(23, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-27.3, 75, true); - chassis.pid_wait(); - trapdoor.set(0); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 - pros::delay(1200); - trapdoor.set(1); + chassis.pid_turn_set(-180, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(4, 80, true); + 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(-130, 80, true); + chassis.pid_drive_set(1000, 40, true); + pros::delay(680); + chassis.pid_drive_set(-8, 40, true); chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); + Little_Mech_Mac.set(0); + + chassis.pid_turn_set(-116.5, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-20, 80, true); - chassis.pid_wait_quick(); + chassis.pid_swing_set(LEFT_SWING, 175, 90, 20, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-145, 60, true); + chassis.pid_drive_set(-18, 80, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_turn_set(-180, 60, true); chassis.pid_wait(); + pros::delay(1500); - chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); -} + discore_mech.set(1); -void left_elims_quick(){ + chassis.pid_drive_set(19, 80, true); + chassis.pid_wait_quick_chain(); - chassis.odom_xyt_set(0_in, 0_in, -30_deg); - discore_mech.set(0); - trapdoor.set(1); + chassis.pid_turn_set(-135, 60, true); + chassis.pid_wait_quick_chain(); + + chassis.pid_drive_set(-35, 80, true); + chassis.pid_wait_quick(); + + 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); + + chassis.pid_drive_set(4.3, 80, true); + chassis.pid_wait_quick(); + + +} + +// 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(22.5, 127, true); //24.8 with little bill activation - pros::delay(500); - Little_Mech_Mac.set(true); - chassis.pid_wait_quick_chain(); + 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_turn_set(59, 127, true); + chassis.pid_swing_set(RIGHT_SWING, -12, 127, 0, false); chassis.pid_wait_quick_chain(); - chassis.pid_drive_exit_condition_set(90_ms, 1_in, 200_ms, 3_in, 50_ms, 50_ms); - - chassis.pid_drive_set(-10.5, 127, true); + 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_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_turn_set(-138, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-2, 127, true); + chassis.pid_drive_set(24.5, 100, true); + chassis.pid_wait_quick_chain(); - trapdoor.set(1); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_drive_set(4, 80, true); + chassis.pid_turn_set(-180, 100, true); chassis.pid_wait_quick_chain(); - discore_mech.set(1); + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - chassis.pid_turn_set(-130, 80, true); + chassis.pid_drive_set(11.8, 70, true); chassis.pid_wait_quick_chain(); - chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); - chassis.pid_wait_quick_chain(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(660); - chassis.pid_drive_set(-20, 90, true); + 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.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); + int waittime = pros::millis(); + + 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); } -void right_safe(){ +// unfinished +void left_elims_quick_ml(){ + 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(90, 80, true); + chassis.pid_turn_set(180, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(12.3, 70, 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(); - pros::delay(400); + pros::delay(200); - chassis.pid_drive_set(-26.8, 75, true); + chassis.pid_drive_set(-27.3, 75, true); chassis.pid_wait(); trapdoor.set(0); - pros::delay(1100); + pros::delay(1200); trapdoor.set(1); - Little_Mech_Mac.set(0); - chassis.pid_drive_set(9, 80, true); + chassis.pid_drive_set(4, 80, true); chassis.pid_wait_quick_chain(); - trapdoor.set(1); - chassis.pid_turn_set(-138.5, 80, true); + chassis.pid_turn_set(-130, 80, true); + chassis.pid_wait_quick_chain(); + chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); chassis.pid_wait_quick_chain(); - //Intaking the balls - chassis.pid_drive_set(30, 60, true); + chassis.pid_drive_set(-20, 80, true); + chassis.pid_wait_quick(); + + chassis.pid_turn_set(-145, 60, true); chassis.pid_wait(); - pros::delay(1000); - chassis.pid_drive_set(20, 80, true); - intake_piston.set(1); + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); +} - pros::delay(100); - intake_bottom.move(-70); - intake_top.move(-100); - intake_top_score.move(-100); - //Little_Mech_Mac.set(1); - chassis.pid_wait(); +// finished +void left_elims_quick(){ + discore_mech.set(0); + trapdoor.set(1); - //Scoring in the middle goal + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); - - pros::delay(900); + 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); - intake_piston.set(0); + 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(-34, 127, true); - chassis.pid_wait(); + 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(); - chassis.pid_turn_set(-92, 80, true); - chassis.pid_wait(); - - chassis.pid_drive_set(22, 127, true); - chassis.pid_wait(); + chassis.pid_turn_set(59, 127, true); + pros::delay(200); Little_Mech_Mac.set(0); + chassis.pid_wait_quick_chain(); + + 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(-12.5, 127, true); + chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-135, 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_drive_constants_set(23, 0, 150); // 22 0 150 -void right_elims_quick(){ - chassis.odom_xyt_set(0_in, 0_in, 30_deg); + 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(); - bottom_intake(127); - trapdoor.set(true); - chassis.pid_drive_set(30_in, 100); - pros::delay(450); - Little_Mech_Mac.set(true); - chassis.pid_wait(); + chassis.pid_drive_set(-2, 127, true); + pros::delay(320); - chassis.pid_turn_set(135, 60); - chassis.pid_wait(); - - 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); -} + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); -void solo_right (){ - //pros::Task anti_jam_auton1 (anti_jam_auton); + discore_mech.set(1); - pros::Task color_sor(color_sort_top_auton); - trapdoor.set(1); - chassis.pid_drive_set(22.6, 90, true); + chassis.pid_turn_set(-131, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(90, 80, true); + 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_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(); - - pros::delay(100); + chassis.pid_drive_set(-20, 80, true); - chassis.pid_drive_set(-26.8, 75, true); - chassis.pid_wait(); - trapdoor.set(0); - pros::delay(1000); + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); +/* +working but a little slow + discore_mech.set(0); trapdoor.set(1); - // chassis.pid_swing_set(LEFT_SWING, -135, 127, -20, true); - // chassis.pid_wait_quick(); - chassis.pid_drive_set(5, 80, true); - chassis.pid_wait_quick_chain(); + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); - chassis.pid_turn_set(-143, 80, true); - Little_Mech_Mac.set(0); - chassis.pid_wait_quick_chain(); + 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(23.7, 80, true); - intake_top.move(127); - pros::delay(650); - Little_Mech_Mac.set(1); + chassis.pid_swing_set(RIGHT_SWING, -12, 127, 0, false); chassis.pid_wait_quick_chain(); - Little_Mech_Mac.set(0); - chassis.pid_turn_set(179, 80, true); + 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(47, 70, true); + chassis.pid_turn_set(59, 127, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-4, 80, 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_turn_set(135, 80, true); - pros::delay(200); - intake_top.move(-20); - intake_top_score.move(-20); - bottom_intake(-20); + 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_drive_set(-15, 60, true); - pros::delay(600); - intake_top.move(100); - intake_top_score.move(-90); - bottom_intake(127); - chassis.pid_wait(); + chassis.pid_swing_set(RIGHT_SWING, 180, -127, 0, true); + pros::delay(800); + trapdoor.set(0); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(1, 30, true); + chassis.pid_drive_set(-2, 127, true); + pros::delay(190); - pros::delay(300); + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); - intake_top.move(0); - bottom_intake(0); + discore_mech.set(1); - chassis.pid_drive_set(40, 90, true); + chassis.pid_turn_set(-130, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(90, 80, true); + chassis.pid_swing_set(LEFT_SWING, 180, 90, 20, true); chassis.pid_wait_quick_chain(); - intake_top_score.move(127); + chassis.pid_drive_set(-20, 90, true); - chassis.pid_drive_set(13.8, 60, true); - intake_top.move(127); - intake_top_score.move(127); - bottom_intake(127); - Little_Mech_Mac.set(1); - chassis.pid_wait(); - pros::delay(200); + chassis.drive_brake_set(pros::E_MOTOR_BRAKE_HOLD); + */ +} - chassis.pid_drive_set(-27, 75, true); - pros::delay(1100); - trapdoor.set(0); - chassis.pid_wait(); +// unfinished +void right_4_3_push(){ - chassis.pid_drive_set(0.5, 60, true); +discore_mech.set(0); + trapdoor.set(1); + intake_top.move(127); + intake_top_score.move(127); + intake_bottom.move(127); - pros::delay(1100); -} + 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 elims_mid_control (){ + chassis.pid_swing_set(LEFT_SWING, 13, 127, 0, false); + chassis.pid_wait_quick_chain(); - pros::Task color_sor(color_sort_top_auton); + 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(); - trapdoor.set(1); + chassis.pid_turn_set(138, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(21, 90, true); + chassis.pid_drive_set(24, 100, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(90, 80, true); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 + + chassis.pid_turn_set(-180, 100, true); chassis.pid_wait_quick_chain(); - //Going into the machloader - Little_Mech_Mac.set(1); - chassis.pid_drive_set(14.5, 80, true); - intake_bottom.move(80); - intake_top.move(127); - intake_top_score.move(127); - chassis.pid_wait(); + chassis.pid_turn_constants_set(3.4, 0, 22, 12.0); // 3.2 0 22 12.0 - pros::delay(200); + chassis.pid_drive_set(11.8, 70, true); + chassis.pid_wait_quick_chain(); - //intaking the balls from the machloader - - chassis.pid_drive_set(-28.5, 75, true); - chassis.pid_wait(); + chassis.pid_drive_set(1000, 40, true); + pros::delay(680); + + 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); - //Scoring the balls + Little_Mech_Mac.set(0); trapdoor.set(0); - pros::delay(1100); + pros::delay(600); + trapdoor.set(1); + + chassis.pid_drive_set(4, 80, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(9, 80, true); + chassis.pid_turn_set(-60, 80, true); chassis.pid_wait_quick_chain(); - trapdoor.set(1); - chassis.pid_turn_set(-138.5, 80, true); + chassis.pid_drive_set(30, 90, true); + chassis.pid_wait_quick(); + + intake_piston.set(1); + 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(); - //Intaking the balls - chassis.pid_drive_set(30, 80, true); - pros::delay(650); - chassis.pid_drive_set(12, 60, true); - chassis.pid_wait(); + pros::delay(2000); - Little_Mech_Mac.set(0); + /////////////////////////////// - chassis.pid_drive_set(20, 60, true); + chassis.pid_drive_set(-34, 127, true); chassis.pid_wait(); - pros::delay(100); - intake_bottom.move(-80); - intake_top.move(-127); - intake_top_score.move(0); - //Scoring in the middle goal + chassis.pid_turn_set(180, 80, true); + chassis.pid_wait(); - pros::delay(1000); - - intake_piston.set(0); - - chassis.pid_drive_set(-18, 127, true); + chassis.pid_drive_set(22, 127, true); + chassis.pid_wait(); + + chassis.pid_turn_set(-135, 80, true); chassis.pid_wait(); +} + +// finished +void right_elims_7ball(){ + discore_mech.set(0); + trapdoor.set(1); + intake_top.move(127); + intake_top_score.move(127); intake_bottom.move(127); - intake_top.move(100); - intake_top_score.move(100); - chassis.pid_turn_set(180, 100, true); + 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(LEFT_SWING, 13, 127, 0, false); chassis.pid_wait_quick_chain(); - - chassis.pid_drive_set(43, 127, true); - pros::delay(700); - chassis.pid_drive_set(21, 60, true); - chassis.pid_wait(); - chassis.pid_turn_set(132, 80, 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(); - chassis.pid_drive_set(-15, 80, true); - chassis.pid_wait(); - intake_top_score.move(-127); - pros::delay(1000); + chassis.pid_turn_set(138, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(8, 80, true); - chassis.pid_wait(); - pros::delay(100); - mid_descore.set(1); + chassis.pid_drive_set(24.5, 100, true); + chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-15, 40, true); - pros::delay(800); - chassis.pid_drive_set(0, 0, true); + chassis.pid_turn_constants_set(3.7, 0, 22, 12.0); // 3.2 0 22 12.0 + + 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); + trapdoor.set(0); + int waittime = pros::millis(); + + 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); } +// finished void push_solo(){ pros::Task color_sor(color_sort_top_auton); @@ -770,11 +982,11 @@ void push_solo(){ 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_drive_wall(460, 100, false, true); + chassis.pid_drive_set(-37.5, 100, true); chassis.pid_wait_quick_chain(); - */ + chassis.pid_turn_set(180, 90, true); Little_Mech_Mac.set(1); @@ -873,11 +1085,15 @@ void push_solo(){ } +// 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 + + //grab ball 2 discore_mech.set(0); - trapdoor.set(1); + trapdoor.set(1); intake_bottom.move(127); intake_top.move(127); @@ -888,7 +1104,7 @@ void safe_skills(){ chassis.pid_turn_set(-45, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(26, 35.67, true); + chassis.pid_drive_set(26, 40.67, true); chassis.pid_wait_quick_chain(); chassis.pid_drive_set(-0.1, 80, true); @@ -899,8 +1115,8 @@ void safe_skills(){ chassis.pid_wait_quick_chain(); chassis.pid_drive_set(-16, 60, true); chassis.pid_wait_quick_chain(); - intake_top_score.move(-35.67); - intake_top.move(-67); + intake_top_score.move(-32.67); + intake_top.move(-65); pros::delay(300); intake_top.move(60); @@ -934,8 +1150,8 @@ void safe_skills(){ chassis.pid_drive_set(1000, 70, true); pros::delay(300); chassis.pid_drive_set(1000, 40, true); - pros::delay(1000); - chassis.pid_drive_set(-1.5, 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); @@ -971,14 +1187,17 @@ void safe_skills(){ chassis.pid_turn_set(0, 100, 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_drive_set(-10.2, 100, true); - chassis.pid_wait_quick_chain(); - + pros::delay(500); trapdoor.set(0); - pros::delay(1950); + 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(1700); trapdoor.set(1); - // grab third match loader + // grab second match loader intake_bottom.move(127); intake_top.move(127); @@ -996,22 +1215,24 @@ void safe_skills(){ chassis.pid_drive_set(1000, 40, true); pros::delay(1000); - chassis.pid_drive_set(-1.5, 40, true); + 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(1800); + pros::delay(1700); //line up for clear discore_mech.set(0); @@ -1052,16 +1273,22 @@ void safe_skills(){ chassis.pid_turn_set(45, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_drive_set(-20, 65, true); + chassis.pid_drive_set(-20.5, 65, true); chassis.pid_wait_quick_chain(); - intake_top_score.move(-60); - intake_top.move(-90); - pros::delay(400); - intake_top_score.move(-33.67); - intake_top.move(57); + 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); - pros::delay(2300); + pros::delay(2100); //grab 3rd match loader @@ -1083,8 +1310,8 @@ void safe_skills(){ chassis.pid_drive_set(1000, 70, true); pros::delay(300); chassis.pid_drive_set(1000, 40, true); - pros::delay(1000); - chassis.pid_drive_set(-1.5, 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); @@ -1120,10 +1347,13 @@ void safe_skills(){ 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); - trapdoor.set(0); pros::delay(1950); trapdoor.set(1); @@ -1131,57 +1361,86 @@ void safe_skills(){ intake_top.move(127); intake_top_score.move(127); + + intake_bottom.move(127); + intake_top.move(127); + intake_top_score.move(127); + + chassis.odom_xyt_set(0_in, 0_in, 180_deg); + chassis.pid_drive_set(6, 80, true); chassis.pid_wait_quick_chain(); - chassis.pid_turn_set(-160, 80, true); + chassis.pid_turn_set(-165, 80, true); chassis.pid_wait_quick_chain(); chassis.pid_swing_set(RIGHT_SWING, 180, 127, 30, 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(1000); - chassis.pid_drive_set(-1.5, 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); //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); + 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); + trapdoor.set(0); Little_Mech_Mac.set(0); - pros::delay(2000); + pros::delay(1700); //line up for clear intake_bottom.move(127); intake_top.move(127); intake_top_score.move(127); - chassis.pid_drive_set(7.3, 127, true); - pros::delay(10); - trapdoor.set(1); + chassis.pid_drive_set(7.3, 127, true); chassis.pid_wait_quick_chain(); + trapdoor.set(1); + intake_top_score.move(-127); trapdoor.set(1); + chassis.odom_xyt_set(0_in, 0_in, 180_deg); + discore_mech.set(0); + chassis.pid_swing_set(LEFT_SWING, -93, 84, 31, true); chassis.pid_wait_quick_chain(); + // chassis.pid_drive_set(5, 127, true); + // chassis.pid_wait_quick_chain(); + + //clear the 6 ball from the park zone + + intake_top_score.move(127); chassis.pid_drive_set(40, 65, true); - pros::delay(200); - Little_Mech_Mac.set(1); chassis.pid_wait(); - Little_Mech_Mac.set(0); + + while (distance_front.get_distance() < 1660){ + chassis.pid_drive_set(1000000, 60); + } + L1.brake(); + L2.brake(); + L3.brake(); + R1.brake(); + R2.brake(); + R3.brake(); } +//old but worth keeping void norcal_skills(){ // pros::Task anti_jam_auton1(anti_jam_auton); @@ -1600,7 +1859,11 @@ void wall_alignment_test() { } void pid_tune(){ - drive_wall(450,127); + chassis.pid_drive_set(24, 85, true); + chassis.pid_wait(); + chassis.pid_turn_set(90, 85, true); + chassis.pid_wait(); + //chassis.pid_drive_set(-30, 127, true); //chassis.pid_wait(); // trapdoor.set(1); diff --git a/src/main.cpp b/src/main.cpp index fd70a96..11b23a3 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,7 +22,7 @@ ez::Drive chassis( {-6, -5, -8}, //left {14, 19, 18}, //right - 11, + 21, 3.25, 450 ); @@ -178,9 +178,9 @@ void initialize() { //pros::Task task1(anti_jam); ez::as::auton_selector.autons_add({ - {"right safe", pid_tune}, + {"right safe", safe_skills}, {"right solo", pid_tune }, - {"elims auton 3 goals", elims_mid_control}, + {"elims auton 3 goals", left_elims_quick_ml}, {"elims left", left_elims_7ball}, }); @@ -330,7 +330,7 @@ void opcontrol() { else if (master.get_digital(DIGITAL_L1)) { intake_bottom.move(-127); intake_top.move(-127); - intake_top_score.move(-127); + // intake_top_score.move(-127); } else if (master.get_digital(DIGITAL_L2)) { @@ -362,7 +362,7 @@ void opcontrol() { if (r2_active) { if (pros::millis() - r2_time >= 1000) { if (!master.get_digital(DIGITAL_L1) && !master.get_digital(DIGITAL_L2)){ - intake_bottom.move(-40); + intake_bottom.move(-44); intake_top.move(-127); intake_top_score.move(-10); } diff --git a/src/wall_tracking.cpp b/src/wall_tracking.cpp index 634b1a8..8116806 100644 --- a/src/wall_tracking.cpp +++ b/src/wall_tracking.cpp @@ -63,7 +63,7 @@ void chassis_drive_wall(float distance, float DRIVE_SPEED, bool chain, bool back // } float d_KP = 0.3; -float d_KI = 0.001; +float d_KI = 0; float d_KD = 0.0021; void drive_wall(float distance, float DRIVE_SPEED) { @@ -77,33 +77,35 @@ void drive_wall(float distance, float DRIVE_SPEED) { float derivative; 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 + 15 && distance_front_l.get_distance() > distance - 15){ + 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) > 100){ + if (arrival_time_B != 0 && (pros::millis() - arrival_time_B) > time_out_B){ break; } - //Small error timeout - if (distance_front_l.get_distance() < distance + 5 && distance_front_l.get_distance() > distance - 5 ){ + 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) > 10){ + if (arrival_time_S != 0 && (pros::millis() - arrival_time_S) > time_out_S){ break; } From 1130581217f905734f13564c77a443be384698c1 Mon Sep 17 00:00:00 2001 From: Mamba1129 Date: Thu, 5 Mar 2026 20:09:32 -0800 Subject: [PATCH 15/15] sigma --- include/subsystems.hpp | 2 +- src/autons.cpp | 60 ++++++++++++++++++++++++++++-------------- src/main.cpp | 10 +++---- 3 files changed, 46 insertions(+), 26 deletions(-) diff --git a/include/subsystems.hpp b/include/subsystems.hpp index e8cbb1e..da6c525 100644 --- a/include/subsystems.hpp +++ b/include/subsystems.hpp @@ -23,7 +23,7 @@ inline pros::Motor R1(14); inline pros::Motor R2(19); inline pros::Motor R3(18); -inline pros::Imu inertial(21); +inline pros::Imu inertial(15); inline pros::Distance distance_back_l(13); // removed sensor inline pros::Distance distance_front_l(9); diff --git a/src/autons.cpp b/src/autons.cpp index afb28e2..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(){ @@ -1236,6 +1234,7 @@ void safe_skills(){ //line up for clear discore_mech.set(0); + intake_bottom.move(127); intake_top.move(127); intake_top_score.move(127); @@ -1255,7 +1254,7 @@ void safe_skills(){ //clear the 6 ball from the park zone - chassis.pid_drive_set(75, 65, true); + chassis.pid_drive_set(75, 75, true); intake_top_score.move(127); chassis.pid_wait(); @@ -1278,6 +1277,8 @@ void safe_skills(){ chassis.pid_drive_set(2, 1, true); + + intake_bottom.move(-80); intake_top.move(-80); intake_top_score.move(-80); @@ -1833,15 +1834,24 @@ void auton_setup_right(){ /* TESTS */ 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_drive_set(24, 127, true); + chassis.pid_turn_set(180, 100, true); chassis.pid_wait(); + pros::delay(1000); - chassis.pid_drive_set(-12, 127, true); + chassis.pid_turn_set(-90, 100, true); chassis.pid_wait(); + pros::delay(1000); - chassis.pid_drive_set(-12, 127, true); + chassis.pid_turn_set(0, 100, true); chassis.pid_wait(); + } void wall_tracking_test() { drive_wall(450,127); @@ -1858,23 +1868,33 @@ void wall_alignment_test() { R3.brake(); } void pid_tune(){ + 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_drive_set(24, 85, true); + chassis.pid_turn_set(360_deg, 40, ez::raw); chassis.pid_wait(); - chassis.pid_turn_set(90, 85, true); + pros::delay(1000); + + chassis.pid_turn_set(360*2_deg, 40, ez::raw); chassis.pid_wait(); + pros::delay(1000); - //chassis.pid_drive_set(-30, 127, true); - //chassis.pid_wait(); - // trapdoor.set(1); - // 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_turn_set(360*3_deg, 80, ez::raw); + chassis.pid_wait(); + pros::delay(1000); } void intake_test(){ diff --git a/src/main.cpp b/src/main.cpp index 11b23a3..57705d0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,7 +22,7 @@ ez::Drive chassis( {-6, -5, -8}, //left {14, 19, 18}, //right - 21, + 15, 3.25, 450 ); @@ -156,8 +156,8 @@ void initialize() { // Set the color of the balls you want to throw out here color = "x"; - discore_mech.set(1); - trapdoor.set(1); + // discore_mech.set(1); + // trapdoor.set(1); //intake_piston.set(1); @@ -179,7 +179,7 @@ void initialize() { ez::as::auton_selector.autons_add({ {"right safe", safe_skills}, - {"right solo", pid_tune }, + {"right solo", pid_tune}, {"elims auton 3 goals", left_elims_quick_ml}, {"elims left", left_elims_7ball}, }); @@ -475,7 +475,7 @@ void opcontrol() { - master.print(0, 0, "%d/%d/%d/%s/%d ", /*L1.get_temperature*/(int)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); + 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++;