diff --git a/SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.cpp b/SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.cpp index 856bea944c..219e91346a 100644 --- a/SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.cpp +++ b/SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.cpp @@ -5,6 +5,7 @@ */ +#include "Common/Cpp/Exceptions.h" #include "CommonFramework/ImageTypes/ImageViewRGB32.h" #include "CommonFramework/VideoPipeline/VideoOverlayScopes.h" #include "CommonTools/Images/ImageFilter.h" @@ -35,6 +36,9 @@ SandwichPlateDetector::SandwichPlateDetector(Logger& logger, Color color, Langua case Side::RIGHT: m_box = ImageFloatBox(0.699, 0.269, 0.201, 0.044); break; + default: + throw InternalProgramError(&logger, PA_CURRENT_FUNCTION, + "Invalid Side for SandwichPlateDetector()"); } } diff --git a/SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h b/SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h index 41c81e12d9..993871de9e 100644 --- a/SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h +++ b/SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h @@ -32,7 +32,8 @@ class SandwichPlateDetector : public StaticScreenDetector{ enum class Side{ LEFT, MIDDLE, - RIGHT + RIGHT, + NOT_APPLICABLE, }; SandwichPlateDetector(Logger& logger, Color color, Language language, Side side); virtual ~SandwichPlateDetector(); diff --git a/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp b/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp index da2c6eb718..16e150588d 100644 --- a/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp +++ b/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp @@ -32,6 +32,9 @@ #include "PokemonSV/Programs/Sandwiches/PokemonSV_IngredientSession.h" #include "PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h" +// #include +// using std::cout; +// using std::endl; namespace PokemonAutomation{ namespace NintendoSwitch{ @@ -347,6 +350,38 @@ bool move_then_recover_sandwich_hand_position( ); } +// return true if the current plate is empty. +// i.e. the current plate label is not yellow. +// Assumes that we are in a grabbing state. +bool check_plate_empty(VideoStream& stream, SandwichPlateDetector::Side target_plate_label, Language language){ + auto screen = stream.video().snapshot(); + // screen.frame->save("test.png"); + + // switch(target_plate_label){ + // case SandwichPlateDetector::Side::LEFT: + // cout << "left" << endl; + // break; + // case SandwichPlateDetector::Side::MIDDLE: + // cout << "middle" << endl; + // break; + // case SandwichPlateDetector::Side::RIGHT: + // cout << "right" << endl; + // break; + // default: + // break; + // } + + SandwichPlateDetector plate_detector = SandwichPlateDetector(stream.logger(), COLOR_RED, language, target_plate_label); + + return !plate_detector.is_label_yellow(screen); +} + +struct HandMoveData{ + ImageFloatBox end_box; + bool plate_empty; +}; + + /* - moves the sandwich hand from start_box to end_box - It detects the location of the sandwich hand, from within the bounds of the last frame's @@ -354,7 +389,7 @@ expanded_hand_bb (i.e. m_box field in SandwichHandLocator). Then updates the current location of expanded_hand_bb. Then moves the sandwich hand closer towards end_box. */ -ImageFloatBox move_sandwich_hand( +HandMoveData move_sandwich_hand_and_check_if_plates_empty( const ProgramInfo& info, AsyncDispatcher& dispatcher, VideoStream& stream, @@ -362,7 +397,9 @@ ImageFloatBox move_sandwich_hand( SandwichHandType hand_type, bool pressing_A, const ImageFloatBox& start_box, - const ImageFloatBox& end_box + const ImageFloatBox& end_box, + SandwichPlateDetector::Side target_plate_label = SandwichPlateDetector::Side::NOT_APPLICABLE, + Language language = Language::None ){ context.wait_for_all_requests(); stream.log("Start moving sandwich hand: " + SANDWICH_HAND_TYPE_NAMES(hand_type) @@ -454,14 +491,24 @@ ImageFloatBox move_sandwich_hand( std::pair dif(target_loc.first - cur_loc.first, target_loc.second - cur_loc.second); // console.log("float diff to target: " + std::to_string(dif.first) + ", " + std::to_string(dif.second)); + + + // Reached the Target if (std::fabs(dif.first) < end_box.width/2 && std::fabs(dif.second) < end_box.height/2){ stream.log(SANDWICH_HAND_TYPE_NAMES(hand_type) + " hand reached target."); + bool plate_empty = false; + + // check if the plate is empty. but only if the target_plate_label isn't NOT_APPLICABLE. + if (target_plate_label != SandwichPlateDetector::Side::NOT_APPLICABLE){ + plate_empty = check_plate_empty(stream, target_plate_label, language); + } + move_session.stop_session_and_rethrow(); // Stop the commands if (hand_type == SandwichHandType::GRABBING){ // wait for some time to let hand release ingredient context.wait_for(std::chrono::milliseconds(100)); } - return hand_bb; + return {hand_bb, plate_empty}; } // Assume screen width is 16.0, then the screen height is 9.0 @@ -517,6 +564,19 @@ ImageFloatBox move_sandwich_hand( } } +ImageFloatBox move_sandwich_hand( + const ProgramInfo& info, + AsyncDispatcher& dispatcher, + VideoStream& stream, + ProControllerContext& context, + SandwichHandType hand_type, + bool pressing_A, + const ImageFloatBox& start_box, + const ImageFloatBox& end_box +){ + return move_sandwich_hand_and_check_if_plates_empty(info, dispatcher, stream, context, hand_type, pressing_A, start_box, end_box, SandwichPlateDetector::Side::NOT_APPLICABLE, Language::None).end_box; +} + } // end anonymous namespace void finish_sandwich_eating( @@ -1013,7 +1073,8 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl SandwichPlateDetector left_plate_detector(stream.logger(), COLOR_RED, language, SandwichPlateDetector::Side::LEFT); SandwichPlateDetector middle_plate_detector(stream.logger(), COLOR_RED, language, SandwichPlateDetector::Side::MIDDLE); SandwichPlateDetector right_plate_detector(stream.logger(), COLOR_RED, language, SandwichPlateDetector::Side::RIGHT); - + bool left_plate_absent = false; + bool right_plate_absent = false; { VideoSnapshot screen = stream.video().snapshot(); @@ -1045,7 +1106,8 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl //Get left (2nd) ingredient std::string left_filling = left_plate_detector.detect_filling_name(screen); - if (left_filling.empty()){ + left_plate_absent = left_filling.empty(); + if (left_plate_absent){ stream.log("No ingredient found on left label."); stream.overlay().add_log("No left plate"); }else{ @@ -1056,7 +1118,8 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl //Get right (3rd) ingredient std::string right_filling = right_plate_detector.detect_filling_name(screen); - if (right_filling.empty()){ + right_plate_absent = right_filling.empty(); + if (right_plate_absent){ stream.log("No ingredient found on right label."); stream.overlay().add_log("No right plate"); }else{ @@ -1139,12 +1202,14 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl //Find fillings and add them in order for (const std::string& i : fillings_sorted){ //cout << "Placing " << i << endl; + stream.log("Placing " + i, COLOR_WHITE); stream.overlay().add_log("Placing " + i, COLOR_WHITE); int times_to_place = (int)(FillingsCoordinates::instance().get_filling_information(i).piecesPerServing) * (fillings.find(i)->second); int placement_number = 0; //cout << "Times to place: " << times_to_place << endl; + stream.log("Times to place: " + std::to_string(times_to_place), COLOR_WHITE); stream.overlay().add_log("Times to place: " + std::to_string(times_to_place), COLOR_WHITE); std::vector plate_index; @@ -1159,16 +1224,20 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl for (int j = 0; j < (int)plate_index.size(); j++){ //Navigate to plate and set target plate //cout << "Target plate: " << plate_index.at(j) << endl; + stream.log("Target plate: " + std::to_string(plate_index.at(j)), COLOR_WHITE); stream.overlay().add_log("Target plate: " + std::to_string(plate_index.at(j)), COLOR_WHITE); + SandwichPlateDetector::Side target_plate_label = SandwichPlateDetector::Side::MIDDLE; switch (plate_index.at(j)){ case 0: target_plate = center_plate; break; case 1: target_plate = left_plate; + target_plate_label = SandwichPlateDetector::Side::LEFT; break; case 2: target_plate = right_plate; + target_plate_label = SandwichPlateDetector::Side::RIGHT; break; case 3: case 4: case 5: case 6: //Press R the appropriate number of times @@ -1181,10 +1250,12 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl break; } + // place down all the ingredients for the current plate. //Place the fillings until label does not light up yellow on grab/the piece count is not hit while (true){ //Break out after placing all pieces of the filling if (placement_number == times_to_place){ + stream.log("We have placed down enough ingredients of that type, so we assume our current plate is empty. Move on to the next plate/ingredient.", COLOR_ORANGE); break; } @@ -1202,26 +1273,27 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl ImageFloatBox placement_target = FillingsCoordinates::instance().get_filling_information(i).placementCoordinates.at( (int)fillings.find(i)->second).at(placement_number); - end_box = move_sandwich_hand( + HandMoveData hand_move_data = move_sandwich_hand_and_check_if_plates_empty( env.program_info(), env.realtime_dispatcher(), stream, context, SandwichHandType::GRABBING, true, expand_box(end_box), - placement_target + placement_target, + target_plate_label ); + end_box = hand_move_data.end_box; context.wait_for_all_requests(); - //If any of the labels are yellow continue. Otherwise assume plate is empty move on to the next. - auto screen = stream.video().snapshot(); - - //The label check is needed for ingredients with multiple plates as we don't know which plate has what amount - if (!left_plate_detector.is_label_yellow(screen) && !middle_plate_detector.is_label_yellow(screen) - && !right_plate_detector.is_label_yellow(screen)){ + // If the current plate is empty, break out of the loop and move on to the next plate. + if (hand_move_data.plate_empty){ context.wait_for_all_requests(); + stream.log("Our current plate label is NOT yellow, so we assume our current plate is empty. Move on to the next plate.", COLOR_ORANGE); break; } + stream.log("Our current plate label is yellow, so we assume our current plate is NOT empty. Continue with the current plate.", COLOR_YELLOW); + //If the plate is empty the increment is skipped using the above break placement_number++; } @@ -1232,8 +1304,12 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl } } } + + context.wait_for_all_requests(); + context.wait_for(Milliseconds(500)); + stream.log("All ingredients should now be empty. Wait for upper bread.", COLOR_YELLOW); // Handle top slice by tossing it away - SandwichHandWatcher grabbing_hand(SandwichHandType::FREE, { 0, 0, 1.0, 1.0 }); + SandwichHandWatcher grabbing_hand(SandwichHandType::GRABBING, { 0, 0, 1.0, 1.0 }); int ret = wait_until(stream, context, std::chrono::seconds(30), { grabbing_hand }); if (ret < 0){ OperationFailedException::fire(