Skip to content

Fix Sandwich maker #605

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Mysticial merged 5 commits into from
Jun 14, 2025
Merged

Fix Sandwich maker #605

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
d70890e
add logging
jw098 Jun 13, 2025
ea1ef3c
check if left/right plates are present. before checking if the label …
jw098 Jun 13, 2025
813ace3
ensure we check for yellow labels before we drop the ingredient, not …
jw098 Jun 14, 2025
e7d4cc5
minor changes to logging
jw098 Jun 14, 2025
d32aeaa
fix build
jw098 Jun 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
*/


#include "Common/Cpp/Exceptions.h"
#include "CommonFramework/ImageTypes/ImageViewRGB32.h"
#include "CommonFramework/VideoPipeline/VideoOverlayScopes.h"
#include "CommonTools/Images/ImageFilter.h"
Expand Down Expand Up @@ -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()");
}
}

Expand Down
3 changes: 2 additions & 1 deletion SerialPrograms/Source/PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
106 changes: 91 additions & 15 deletions SerialPrograms/Source/PokemonSV/Programs/Sandwiches/PokemonSV_SandwichRoutines.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,9 @@
#include "PokemonSV/Programs/Sandwiches/PokemonSV_IngredientSession.h"
#include "PokemonSV/Inference/Picnics/PokemonSV_SandwichPlateDetector.h"

// #include <iostream>
// using std::cout;
// using std::endl;

namespace PokemonAutomation{
namespace NintendoSwitch{
Expand Down Expand Up @@ -347,22 +350,56 @@ 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
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,
ProControllerContext& context,
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)
Expand Down Expand Up @@ -454,14 +491,24 @@ ImageFloatBox move_sandwich_hand(

std::pair<double, double> 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
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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{
Expand All @@ -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{
Expand Down Expand Up @@ -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<int> plate_index;
Expand All @@ -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
Expand All @@ -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;
}

Expand All @@ -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++;
}
Expand All @@ -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(
Expand Down