/** * Turn servo towards 'pos' in 1 microsecond steps, waiting delay_ms * milliseconds between steps (speed = 1/delay). If check_weight weight * is true, might abort with WHERE_THE_FUCK_IS_THE_CUP error. If a valid pointer * stable_weight is passed, turns bottle until a stable weight is measured * (returns WEIGHT_NOT_STABLE if pos is reached before weight stable). * * Returns 0 when the position is reached or SERVO_OUT_OF_RANGE on error. * * For details about the built-in Servo class see: * /usr/share/arduino/libraries/Servo/Servo.cpp * */ errv_t Bottle::turn_to(int pos, int delay_ms, bool check_weight, int* stable_weight, bool enable_abortcheck) { int weight_previous1 = -9999; // just any impossible value int weight_previous2 = -9999; // ..before we have real values if (pos < SERVO_MIN || pos > SERVO_MAX) { DEBUG_MSG_LN("Invalid pos"); return SERVO_OUT_OF_RANGE; } int current_pos = servo.readMicroseconds(); if (pos == current_pos) return 0; int step = (current_pos < pos) ? 1 : -1; DEBUG_START(); DEBUG_MSG("turn "); DEBUG_MSG(number); DEBUG_MSG(", params "); DEBUG_VAL(current_pos); DEBUG_VAL(step); DEBUG_VAL(pos); DEBUG_VAL(delay_ms); DEBUG_END(); unsigned long last_called = millis(); for (int i = current_pos + step; i * step <= pos * step; i += step) { // ˆˆˆˆˆˆ ˆˆˆˆˆˆ // this inverts the relation if turning down // Warning: printing to serial delays turning! // Might help to to debug servo movement. Not necessary now, commenting // out to save bytes. //if (print_steps && i % 10 == 0) { // DEBUG_VAL_LN(i); //} // check abort only if not already aborted... if (enable_abortcheck) { // turn up and return if we should abort... errv_t ret = check_aborted(); if (ret) { // turn_up might not be necessary here, called another time // later (does not matter if called twice) turn_up(FAST_TURN_UP_DELAY, false); return ret; } } if (check_weight || stable_weight) { int weight; int ret = ads1231_get_noblock(weight); if (ret == 0) { // we got a valid weight from scale if (check_weight && weight < WEIGHT_EPSILON) { return WHERE_THE_FUCK_IS_THE_CUP; } // get next weight sample and return if weight is stable if (stable_weight) { if (weight_previous2 == weight_previous1 && weight_previous1 == weight) { *stable_weight = weight; return 0; } weight_previous2 = weight_previous1; weight_previous1 = weight; } } else if (ret != ADS1231_WOULD_BLOCK) { // ignoring if it would take too long to get weight, but // return in case of other error != 0 return ret; } } // turn servo one step delay(delay_ms); servo.writeMicroseconds(i); } // pos reached before weight stable if (stable_weight) { return WEIGHT_NOT_STABLE; } return 0; }
int CALLBACK WinMain(HINSTANCE instanceHandler, HINSTANCE, LPSTR commandLineParameters, int windowDisplayFlags) { // Initialise game objects. DEBUG_ONLY(globalDebugger = new Debugger); // DO NOT CHANGE mainGameObject = new Game; // Initialise GDI+ DEBUG_OUT(TEXT("Gdiplus::GdiplusStartup() . . .")); ULONG_PTR gdiplusToken; Gdiplus::GdiplusStartupInput gdiplusStartupInput; gdiplusStartupInput.GdiplusVersion = 1; gdiplusStartupInput.DebugEventCallback = NULL; gdiplusStartupInput.SuppressBackgroundThread = FALSE; gdiplusStartupInput.SuppressExternalCodecs = TRUE; if (Gdiplus::GdiplusStartup(&gdiplusToken, &gdiplusStartupInput, NULL) != Gdiplus::Ok) TotalFailureError(); // Initialise GDI+ objects MY_BLACK_PEN = new Gdiplus::Pen(Gdiplus::Color(0xFF, 0, 0, 0)); MY_WHITE_PEN = new Gdiplus::Pen(Gdiplus::Color(0xFF, 0xFF, 0xFF, 0xFF)); MY_RED_PEN = new Gdiplus::Pen(Gdiplus::Color(0xFF, 0xFF, 0, 0)); MY_BLACK_BRUSH = new Gdiplus::SolidBrush(Gdiplus::Color(0xFF, 0, 0, 0)); MY_WHITE_BRUSH = new Gdiplus::SolidBrush(Gdiplus::Color(0xFF, 0xFF, 0xFF, 0xFF)); MY_RED_BRUSH = new Gdiplus::SolidBrush(Gdiplus::Color(0xFF, 0xFF, 0, 0)); MY_BLUE_BRUSH = new Gdiplus::SolidBrush(Gdiplus::Color(0xFF, 0, 0, 0xFF)); WEAK_BLACK_BRUSH = new Gdiplus::SolidBrush(Gdiplus::Color(0xAA, 0, 0, 0)); // Register window 'class' with OS in preparation for creating window. DEBUG_OUT(TEXT("RegisterClassEx() . . .")); WNDCLASSEX mainWindowClass = { sizeof(WNDCLASSEX), 0, MainWindowProcedure, 0, 0, instanceHandler, LoadIcon(NULL, IDI_APPLICATION), LoadCursor(NULL, IDC_ARROW), NULL, // No brush, we'll fill our own background. NULL, MAIN_WINDOW_CLASS_NAME, LoadIcon(NULL, IDI_APPLICATION)}; if (!RegisterClassEx(&mainWindowClass)) TotalFailureError(); // Start window. DEBUG_OUT(TEXT("CreateWindow() . . .")); HWND mainWindowHandle = CreateWindow( MAIN_WINDOW_CLASS_NAME, MAIN_WINDOW_TITLE, WS_CAPTION | WS_OVERLAPPED | WS_SYSMENU | WS_MINIMIZEBOX, CW_USEDEFAULT, CW_USEDEFAULT, MAIN_WINDOW_WIDTH, MAIN_WINDOW_HEIGHT, NULL, NULL, instanceHandler, NULL); if (mainWindowHandle == NULL) TotalFailureError(); // This starts the window being displayed, and invokes the first WM_PAINT message. DEBUG_OUT(TEXT("ShowWindow() . . .")); ShowWindow(mainWindowHandle, windowDisplayFlags); { BOOL result = UpdateWindow(mainWindowHandle); DEBUG_VAL(TEXT("UpdateWindow()"), result); if (!result) TotalFailureError(); } MSG mainMessage; // Main loop: receives messages and transmits them so the OS can properly give them to MainWindowProcedure. DEBUG_OUT(TEXT("Main Windows message loop starting . . .")); while (BOOL receiveStatus = GetMessage(&mainMessage, mainWindowHandle, 0, 0)) { if (receiveStatus == -1) { TotalFailureError(); } else { TranslateMessage(&mainMessage); DispatchMessage(&mainMessage); } } // Destruct GDI+ objects delete MY_BLACK_PEN; delete MY_WHITE_PEN; delete MY_RED_PEN; delete MY_BLACK_BRUSH; delete MY_WHITE_BRUSH; delete MY_RED_BRUSH; delete MY_BLUE_BRUSH; delete WEAK_BLACK_BRUSH; // Shut-down GDI+. Gdiplus::GdiplusShutdown(gdiplusToken); // Destruct game objects. delete mainGameObject; DEBUG_ONLY(delete globalDebugger); // DO NOT TOUCH return mainMessage.wParam; }
/** * Pour requested_amount grams from bottle.. * Return 0 on success, other values are return values of * delay_until (including scale error codes). */ errv_t Bottle::pour(int requested_amount, int& measured_amount) { // orig_weight is weight including ingredients poured until now int orig_weight, ret; while (1) { // get weight while turning bottle, because ads1231_stable_millis() // blocks bottle in pause position too long int below_pause = (pos_down + get_pause_pos()) / 2; ret = turn_to(below_pause, TURN_DOWN_DELAY, true, &orig_weight); // Note that checking weight here is a critical issue, allows hacking // the robot. If a heavy weight is placed while measuring and then // removed while pouring (results in more alcohol). Stable weight // should resolve most problems. if (ret == WEIGHT_NOT_STABLE) { ret = ads1231_get_stable_grams(orig_weight); } if (ret != 0 && ret != WHERE_THE_FUCK_IS_THE_CUP) { return ret; } if (ret == WHERE_THE_FUCK_IS_THE_CUP || orig_weight < WEIGHT_EPSILON) { // no cup... RETURN_IFN_0(wait_for_cup()); } else { // everything fine break; } } // loop until successfully poured or aborted or other fatal error while(1) { // petres wants POURING message also after resume... // https://github.com/rfjakob/barwin-arduino/issues/10 MSG(String("POURING ") + String(number) + String(" ") + String(orig_weight)); DEBUG_MSG_LN("Turn down"); ret = turn_down(TURN_DOWN_DELAY, true); // enable check_weight // wait for requested weight // FIXME here we do not want WEIGHT_EPSILON and sharp > if (ret == 0) { DEBUG_MSG_LN("Waiting"); ret = delay_until(POURING_TIMEOUT, orig_weight + requested_amount - UPGRIGHT_OFFSET, true); } if (ret == 0) break; // All good DEBUG_MSG_LN(String("pour: got err ") + String(ret)); // Bottle empty // Note that this does not work if requested_amount is less than // UPGRIGHT_OFFSET! if(ret == BOTTLE_EMPTY) { ERROR(strerror(BOTTLE_EMPTY) + String(" ") + String(number) ); // TODO other speed here? it is empty already! RETURN_IFN_0(turn_to(pos_up + BOTTLE_EMPTY_POS_OFFSET, TURN_UP_DELAY)); RETURN_IFN_0(wait_for_resume()); // might return ABORTED } // Cup was removed early else if(ret == WHERE_THE_FUCK_IS_THE_CUP) { ERROR(strerror(WHERE_THE_FUCK_IS_THE_CUP)); RETURN_IFN_0(turn_to_pause_pos(FAST_TURN_UP_DELAY)); RETURN_IFN_0(wait_for_cup()); } // other error - turn bottle up and return error code // includes: scale error, user abort, ... else { return ret; } } // We turn to pause pos and not completely up so we can crossfade RETURN_IFN_0(turn_to_pause_pos(TURN_UP_DELAY)); RETURN_IFN_0(ads1231_get_grams(measured_amount)); measured_amount -= orig_weight; DEBUG_START(); DEBUG_MSG("Stats: "); DEBUG_VAL(requested_amount); DEBUG_VAL(measured_amount); DEBUG_END(); return 0; }
// Handles OS messages, is driven by the 'main loop' above. LRESULT CALLBACK MainWindowProcedure(HWND windowHandle, UINT messageCode, WPARAM wParam, LPARAM lParam) { switch (messageCode) { // On window creation. case WM_CREATE: DEBUG_OUT(TEXT("WM_CREATE message")); // Start game loop timer. DEBUG_VAL(TEXT("SetTimer()"), SetTimer(windowHandle, MAIN_CYCLE_TIMER_ID, MAIN_CYCLE_WAIT, NULL)); break; // Upon redraw request or something else changing we draw the window. case WM_PAINT: { PAINTSTRUCT paintJobStruct; HDC deviceContextHandle = BeginPaint(windowHandle, &paintJobStruct); HDC bufferDeviceContextHandle = CreateCompatibleDC(deviceContextHandle); HBITMAP bufferBitmapHandle = CreateCompatibleBitmap(deviceContextHandle, MAIN_WINDOW_WIDTH, MAIN_WINDOW_HEIGHT); HGDIOBJ oldBufferBitmapHandle = SelectObject(bufferDeviceContextHandle, bufferBitmapHandle); Gdiplus::Graphics graphics(bufferDeviceContextHandle); graphics.SetSmoothingMode(GRAPHICS_SMOOTHING_MODE); DrawGame(graphics, *mainGameObject); BitBlt(deviceContextHandle, 0, 0, MAIN_WINDOW_WIDTH, MAIN_WINDOW_HEIGHT, bufferDeviceContextHandle, 0, 0, SRCCOPY); SelectObject(bufferDeviceContextHandle, oldBufferBitmapHandle); DeleteDC(bufferDeviceContextHandle); DeleteObject(bufferBitmapHandle); EndPaint(windowHandle, &paintJobStruct); } break; // When a user presses a key (can be triggered by auto-repeat). case WM_KEYDOWN: DEBUG_OUT(TEXT("WM_KEYDOWN message")); DEBUG_VAL(TEXT("wParam"), wParam); switch(wParam) { case VK_LEFT: case VK_UP: case VK_RIGHT: case VK_DOWN: DEBUG_OUT(TEXT("Arrow key pressed")); mainGameObject->Input(wParam); } break; case WM_TIMER: if (wParam == MAIN_CYCLE_TIMER_ID) { // This is where the 'main game loop' kicks in. This line should be reached at a frequency of about 60Hz. mainGameObject->Step(windowHandle); RedrawWindow(windowHandle, NULL, NULL, RDW_INVALIDATE); //InvalidateRect(windowHandle, NULL, TRUE); //UpdateWindow(windowHandle); } break; case WM_CLOSE: DEBUG_OUT(TEXT("WM_CLOSE message")); // Clean up Windows API objects and etc. KillTimer(windowHandle, MAIN_CYCLE_TIMER_ID); DestroyWindow(windowHandle); break; case WM_DESTROY: PostQuitMessage(0); break; default: return DefWindowProc(windowHandle, messageCode, wParam, lParam); break; } return 0; }