diff --git a/include/deviceGlobals.hpp b/include/deviceGlobals.hpp index 92ce1c2..0b22495 100644 --- a/include/deviceGlobals.hpp +++ b/include/deviceGlobals.hpp @@ -30,8 +30,8 @@ inline pros::Imu inertial1(2); inline pros::Imu inertial2(18); inline pros::Rotation wallStakeRotationSensor(11); -inline pros::Rotation verticalEncoder(-13); -inline pros::Rotation horizontalEncoder(-1); +inline pros::Rotation verticalEncoder(-14); +inline pros::Rotation horizontalEncoder(1); //hello world it is 210K secret note iykyk :D // horizontal tracking wheel inline lemlib::TrackingWheel vertical_tracking_wheel(&verticalEncoder, lemlib::Omniwheel::NEW_275, 0.876); diff --git a/src/auton/autonomous.cpp b/src/auton/autonomous.cpp index 0e290a5..eda4337 100644 --- a/src/auton/autonomous.cpp +++ b/src/auton/autonomous.cpp @@ -42,13 +42,13 @@ void SIG_SAWP() { chassis.turnToHeading(45, 1000); // score alliance stake - driver.alliance_stake(); - pros::delay(500); + // driver.alliance_stake(); + // pros::delay(500); - chassis.setPose(0, 0, 0); - chassis.moveToPoint(0, -5, 600); + // chassis.setPose(0, 0, 0); + // chassis.moveToPoint(0, -5, 600); - driver.next_state(); + // driver.next_state(); chassis.turnToHeading(17, 1000); //-20 // clamp goal @@ -122,58 +122,67 @@ void SIG_SAWP() { } void ring_rush() { + if (alliance == "red" || alliance == "na"){ chassis.moveToPoint(0, 0, 1250); - chassis.moveToPoint(-1.949, 45.851, 1250); + chassis.moveToPoint(-0.8, 46.751, 1250); intake.move_voltage(12000); leftDoinkerPiston.set_value(true); - pros::delay(1350); + pros::delay(1550); intake.move_voltage(0); - chassis.moveToPoint(-1.737, 20.234, 1250, {.forwards = false, .maxSpeed = 90}); + chassis.moveToPoint(-0.037, 20.234, 1250, {.forwards = false, .maxSpeed = 90}); pros::delay(860); leftDoinkerPiston.set_value(false); // pros::delay(850); chassis.turnToHeading(230, 500); - chassis.moveToPoint(5.945, 16.287, 1250, {.forwards = false, .maxSpeed = 80}); + chassis.moveToPoint(13.945, 28.287, 1450, {.forwards = false, .maxSpeed = 80}); pros::delay(750); clampPiston.set_value(true); - pros::delay(830); + pros::delay(530); intake.move_voltage(12000); - chassis.moveToPoint(-18.634, 30.051, 1250); - pros::delay(300); + chassis.moveToPoint(-21.634, 36.051, 2050); + pros::delay(400); antiJamToggle = false; - intake.move_voltage(7000); - chassis.moveToPoint(-44.174, -14.218, 1650, {.maxSpeed= 127}); - // intakeLiftPiston.set_value(true); - // intakeLiftPiston.set_value(false); - // pros::delay(700); + intake.move_voltage(12000); + chassis.moveToPoint(-35.174, 0.218, 1650, {.maxSpeed= 127}); + intakeLiftPiston.set_value(true); + pros::delay(500); + intakeLiftPiston.set_value(false); + pros::delay(700); chassis.moveToPoint(-30, 2, 1300, {.forwards = false}); - chassis.moveToPoint(-44.174, -14.218, 1000, {.minSpeed= 127}); - antiJamToggle = true; + chassis.moveToPoint(-36.174, 2.218, 1200, {.minSpeed= 127}); intake.move_voltage(12000); - chassis.moveToPoint(-22, 10, 1000, {.forwards = false}); - pros::delay(300); + chassis.moveToPoint(-22, 10, 1500, {.forwards = false}); + pros::delay(400); + chassis.moveToPoint(-36.174, 0.218, 1200, {.minSpeed= 127}); + pros::delay(500); + antiJamToggle = true; // chassis.moveToPoint(-44.174, -15.218, 1850, {.maxSpeed= 80}); // pros::delay(2000); // chassis.moveToPoint(-22, -1, 1000, {.forwards = false}); // pros::delay(100); chassis.moveToPoint(18.174, 0.218, 1050, {.minSpeed= 127}); pros::delay(100); - chassis.moveToPoint(21, 2, 1200, {.maxSpeed = 35}); - pros::delay(900); - resetWallstakes(); + chassis.moveToPoint(31, 2, 1000, {.maxSpeed = 127}); + // chassis.moveToPoint(50, -3, 10000); + + // resetWallstakes(); antiJamToggle = false; - intakeLiftPiston.set_value(true); - pros::delay(1050); - intakeLiftPiston.set_value(false); - pros::delay(1200); - chassis.moveToPoint(21.5, -18.3, 1200); - pros::delay(100); - intake.move_voltage(-500); - wallStake.move_voltage(12000); - pros::delay(900); - wallStake.brake(); + // intakeLiftPiston.set_value(true); + // pros::delay(1050); + // intakeLiftPiston.set_value(false); + // pros::delay(1200); + // chassis.moveToPoint(21.5, -18.3, 1200); + // pros::delay(100); + // intake.move_voltage(-500); + // wallStake.move_voltage(12000); + // pros::delay(900); + // wallStake.brake(); // chassis.moveToPoint(-39.384, 5.946, 1250); // chassis.moveToPoint(24.607, 3.935, 1250); + + } else { // blue + + } } diff --git a/src/init/initialize.cpp b/src/init/initialize.cpp index 45be4d3..e0c3e33 100644 --- a/src/init/initialize.cpp +++ b/src/init/initialize.cpp @@ -19,14 +19,10 @@ void initialize() { initializeColourSort(); optical.set_led_pwm(100); // enable led on optical sensor for accuracy - optical.set_integration_time(10); // refresh every 10ms + optical.set_integration_time(5); // refresh every 10ms wallStake.set_brake_mode(pros::E_MOTOR_BRAKE_HOLD); - intake.get_brake_mode(pros::E_MOTOR_BRAKE_BRAKE); + intake.set_brake_mode(pros::E_MOTOR_BRAKE_BRAKE); - inertial1.set_data_rate(10); - inertial2.set_data_rate(10); - verticalEncoder.set_data_rate(10); - horizontalEncoder.set_data_rate(10); console.println("Robot initialized"); } diff --git a/src/subsystem/colourSort.cpp b/src/subsystem/colourSort.cpp index 87f4e93..64108c8 100644 --- a/src/subsystem/colourSort.cpp +++ b/src/subsystem/colourSort.cpp @@ -7,19 +7,23 @@ bool antiJamToggle = true; // Colour Sorter void initializeColourSort() { pros::Task([] { + double previousIntakeVel; + double intakeVel; + double derivative; while (true) { //console.printf("Hue: %d\n", optical.get_hue()); - if (colourSortToggle == true && (alliance == "red" && optical.get_hue() > 100 && optical.get_hue() < 230) || - (alliance == "blue" && optical.get_hue() > 15 && optical.get_hue() < 27)) { + if (colourSortToggle == true && (alliance == "red" && optical.get_hue() > 200 && optical.get_hue() < 230) || + (alliance == "blue" && optical.get_hue() > 8 && optical.get_hue() < 15)) { + colourSortToggle = false; // eject blue rings console.println("eject impostor"); - controller.rumble("."); intakeLock = true; - pros::delay(220); + pros::delay(225); intake.brake(); - pros::delay(190); + pros::delay(210); intake.move_voltage(12000); intakeLock = false; + colourSortToggle = true; } if (controller.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_LEFT)) { @@ -27,15 +31,22 @@ void initializeColourSort() { } // anti-jam - if (intake.get_actual_velocity() == 0 && intake.get_voltage() > 1000 && (wallStakeRotationSensor.get_angle() / 100) < 20 && antiJamToggle == true) { + intakeVel = intake.get_actual_velocity(); + derivative = previousIntakeVel - intakeVel; + + if (derivative < -50 && intake.get_voltage() > 6000 && ((wallStakeRotationSensor.get_angle() / 100) < 20 || (wallStakeRotationSensor.get_angle() / 100) > 365) && antiJamToggle == true) { + console.println("anti-jam triggered"); + antiJamToggle = false; intakeLock = true; intake.move_voltage(-8000); - pros::delay(135); + pros::delay(175); intake.move_voltage(12000); intakeLock = false; + antiJamToggle = true; } - pros::delay(10); + previousIntakeVel = intakeVel; + pros::delay(5); } }); } \ No newline at end of file