/**
* 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;
}
