From d70890e345bbf65e4082a1d3e10a0e09cad434d6 Mon Sep 17 00:00:00 2001 From: jw098 Date: Thu, 12 Jun 2025 22:56:01 -0700 Subject: [PATCH 1/5] add logging --- .../Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp b/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp index da2c6eb718..7167d5a2e1 100644 --- a/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp +++ b/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp @@ -1139,12 +1139,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,6 +1161,7 @@ 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); switch (plate_index.at(j)){ case 0: @@ -1184,7 +1187,7 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl //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){ + if (placement_number == times_to_place){ // todo: maybe swap to piecesPerServing? break; } @@ -1219,9 +1222,12 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl if (!left_plate_detector.is_label_yellow(screen) && !middle_plate_detector.is_label_yellow(screen) && !right_plate_detector.is_label_yellow(screen)){ context.wait_for_all_requests(); + stream.log("None of the labels are yellow, so we assume our current plate is empty and move on to the next plate.", COLOR_WHITE); break; } + stream.log("One of the labels are yellow, so we assume our current plate is not empty and we continue the current plate.", COLOR_WHITE); + //If the plate is empty the increment is skipped using the above break placement_number++; } From ea1ef3c34bc73f33ddba589caff1db6a7508afa3 Mon Sep 17 00:00:00 2001 From: jw098 Date: Thu, 12 Jun 2025 23:48:46 -0700 Subject: [PATCH 2/5] check if left/right plates are present. before checking if the label is yellow. --- .../Sandwiches/PokemonSV_SandwichRoutines.cpp | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp b/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp index 7167d5a2e1..9fbd356875 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{ @@ -1013,7 +1016,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 +1049,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 +1061,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{ @@ -1219,8 +1225,14 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl 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)){ + // ensure the plates aren't absent to minimize false positives. + bool is_left_plate_yellow = !left_plate_absent && left_plate_detector.is_label_yellow(screen); + bool is_middle_plate_yellow = middle_plate_detector.is_label_yellow(screen); + bool is_right_plate_yellow = !right_plate_absent && right_plate_detector.is_label_yellow(screen); + // cout << "is_left_plate_yellow: " << is_left_plate_yellow << endl; + // cout << "is_middle_plate_yellow: " << is_middle_plate_yellow << endl; + // cout << "is_right_plate_yellow: " << is_right_plate_yellow << endl; + if (!is_left_plate_yellow && !is_middle_plate_yellow && !is_right_plate_yellow){ context.wait_for_all_requests(); stream.log("None of the labels are yellow, so we assume our current plate is empty and move on to the next plate.", COLOR_WHITE); break; @@ -1238,8 +1250,12 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl } } } + + context.wait_for_all_requests(); + context.wait_for(Milliseconds(1000)); + 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( From 813ace3aa467cf094e01b9bb1b6ebfa89cc86680 Mon Sep 17 00:00:00 2001 From: jw098 Date: Fri, 13 Jun 2025 22:01:23 -0700 Subject: [PATCH 3/5] ensure we check for yellow labels before we drop the ingredient, not after --- .../Picnics/PokemonSV_SandwichPlateDetector.h | 3 +- .../Sandwiches/PokemonSV_SandwichRoutines.cpp | 95 +++++++++++++++---- 2 files changed, 76 insertions(+), 22 deletions(-) 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 9fbd356875..022a8a4fd0 100644 --- a/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp +++ b/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp @@ -350,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 @@ -357,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, @@ -365,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) @@ -457,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 @@ -520,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( @@ -1169,15 +1226,18 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl //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 @@ -1190,10 +1250,11 @@ 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){ // todo: maybe swap to piecesPerServing? + if (placement_number == times_to_place){ // todo: maybe swap to current_plate_placement_number == piecesPerServing? break; } @@ -1211,34 +1272,26 @@ 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 - // ensure the plates aren't absent to minimize false positives. - bool is_left_plate_yellow = !left_plate_absent && left_plate_detector.is_label_yellow(screen); - bool is_middle_plate_yellow = middle_plate_detector.is_label_yellow(screen); - bool is_right_plate_yellow = !right_plate_absent && right_plate_detector.is_label_yellow(screen); - // cout << "is_left_plate_yellow: " << is_left_plate_yellow << endl; - // cout << "is_middle_plate_yellow: " << is_middle_plate_yellow << endl; - // cout << "is_right_plate_yellow: " << is_right_plate_yellow << endl; - if (!is_left_plate_yellow && !is_middle_plate_yellow && !is_right_plate_yellow){ + // 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("None of the labels are yellow, so we assume our current plate is empty and move on to the next plate.", COLOR_WHITE); + stream.log("Our current plate label is NOT yellow, so we assume our current plate is empty. Move on to the next plate.", COLOR_YELLOW); break; } - stream.log("One of the labels are yellow, so we assume our current plate is not empty and we continue the current plate.", COLOR_WHITE); + 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++; @@ -1252,7 +1305,7 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl } context.wait_for_all_requests(); - context.wait_for(Milliseconds(1000)); + 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::GRABBING, { 0, 0, 1.0, 1.0 }); From e7d4cc5426c8f48da411401d135a4eedd209d1be Mon Sep 17 00:00:00 2001 From: jw098 Date: Fri, 13 Jun 2025 22:37:02 -0700 Subject: [PATCH 4/5] minor changes to logging --- .../Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp b/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp index 022a8a4fd0..16e150588d 100644 --- a/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp +++ b/SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp @@ -1254,7 +1254,8 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl //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){ // todo: maybe swap to current_plate_placement_number == piecesPerServing? + 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; } @@ -1287,7 +1288,7 @@ void run_sandwich_maker(ProgramEnvironment& env, VideoStream& stream, ProControl // 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_YELLOW); + 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; } From d32aeaae9caaea1eb43f0c197a72670712a72c06 Mon Sep 17 00:00:00 2001 From: jw098 Date: Fri, 13 Jun 2025 22:52:46 -0700 Subject: [PATCH 5/5] fix build --- .../Inference/Picnics/PokemonSV_SandwichPlateDetector.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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()"); } }