void drive(const int x, const int y, const int phi )
{
rec::core_lt::Timer timer;
timer.start();
//while( com.isConnected()
// && false == bumper.value()
// && timer.msecsElapsed() < 10000 )
while( com.isConnected()
&& false == bumper.value()
&& odometry.x() < x )
{
omniDrive.setVelocity( 70, 0, 0 );
com.waitForUpdate();
}
while( com.isConnected()
&& false == bumper.value()
&& odometry.y() < y )
{
omniDrive.setVelocity( 0, 70, 0 );
com.waitForUpdate();
}
while( com.isConnected()
&& false == bumper.value()
&& odometry.phi() < phi )
{
omniDrive.setVelocity( 0, 0, 30 );
com.waitForUpdate();
}
}
void drive()
{
Bumper bumper;
while( com.isConnected() && false == bumper.value() && _run )
{
com.processEvents();
rec::robotino::api2::msleep( 1000 );
}
}
void drive(const int x, const int y, const int phi )
{
const int vel=90;
rec::core_lt::Timer timer;
timer.start();
//while( com.isConnected()
// && false == bumper.value()
// && timer.msecsElapsed() < 10000 )
double M=sqrt(mysqr(odometry.x()-x)+mysqr(odometry.y()-y));
while( com.isConnected()
&& false == bumper.value()
&& M>30 )
{
M=sqrt(mysqr(odometry.x()-x)+mysqr(odometry.y()-y));
//std::cout<<M<<std::endl;
omniDrive.setVelocity( vel*((double)(x-odometry.x()))/M, vel*((double)(y-odometry.y()))/M, 0 );
com.waitForUpdate();
}
/*
while( com.isConnected()
&& false == bumper.value()
&& odometry.x() < x )
{
omniDrive.setVelocity( 70, 0, 0 );
com.waitForUpdate();
}
while( com.isConnected()
&& false == bumper.value()
&& odometry.y() < y )
{
omniDrive.setVelocity( 0, 70, 0 );
com.waitForUpdate();
}*/
while( com.isConnected()
&& false == bumper.value()
&& phi>0
&& (abs(odometry.phi()-phi)>5) )
{
omniDrive.setVelocity( 0, 0, +30 );
com.waitForUpdate();
}
while( com.isConnected()
&& false == bumper.value()
&& phi<0
&& (abs(odometry.phi()-phi)>5) )
{
omniDrive.setVelocity( 0, 0, -30 );
com.waitForUpdate();
}
}
void videoInit()
{
videoSetModeSub(MODE_0_2D);
oamInit(&oamSub, SpriteMapping_Bmp_1D_128, false);
int bgId = bgInitSub(3, BgType_Text8bpp, BgSize_T_256x256, 0, 1);
bgSetPriority(bgId, 3);
dmaCopy(background.gfxData, bgGetGfxPtr(bgId), MemChunk_GetSize(background.gfxData));
dmaCopy(background.mapData, bgGetMapPtr(bgId), MemChunk_GetSize(background.mapData));
dmaCopy(background.palData, BG_PALETTE_SUB, MemChunk_GetSize(background.palData));
videoSetMode(MODE_3_2D);
int bgBmp = bgInit(3, BgType_Bmp16, BgSize_B16_256x256, 2, 0);
bmpBuf = bgGetGfxPtr(bgBmp);
clearBitmap();
int bgTop = bgInit(2, BgType_Text8bpp, BgSize_T_256x256, 0, 1);
bgSetPriority(bgTop, 3);
dmaCopy(topscr.gfxData, bgGetGfxPtr(bgTop), MemChunk_GetSize(topscr.gfxData));
dmaCopy(topscr.mapData, bgGetMapPtr(bgTop), MemChunk_GetSize(topscr.mapData));
dmaCopy(topscr.palData, BG_PALETTE, MemChunk_GetSize(topscr.palData));
u16* gfx = SPRITE_GFX_SUB;
for (int i = 0; i < 6; i ++)
{
int xPos = 16 + (i % 3) * (64+16);
int yPos = 48 + (i / 3) * (64+8);
oamSet(&oamSub, i, xPos, yPos, 1, 15, SpriteSize_64x64, SpriteColorFormat_Bmp, gfx, -1, 0, false, false, false, false);
gfx += 64*64;
}
{
dmaCopy(selection.gfxData, gfx, MemChunk_GetSize(selection.gfxData));
oamSet(&oamSub, 6, 0, 0, 0, 15, SpriteSize_16x16, SpriteColorFormat_Bmp, gfx, -1, 0, false, false, false, false);
gfx += 16*16;
}
for (int i = 0; i < MAX_RUNNING_APP_COUNT; i ++)
{
oamSet(&oamSub, 7+i, 64+(i*(16+8)), 16, 1, 15, SpriteSize_16x16, SpriteColorFormat_Bmp, gfx, -1, 0, false, false, false, false);
oamSub_mem[7+i].isHidden = true;
gfx += 16*16;
}
g_appListChanged = true;
bump.init(BG_PALETTE_SUB);
bump.SelectBump(page);
updCursor();
loadPageIcons();
updAppList();
forceTopScrRefresh = true;
}
static bool MainVBlank()
{
renderTopLines();
updAppList();
word_t kDown = keysDown();
TouchPos pos;
// Page moving code
do
{
int disp = 0;
if (kDown & KEY_LEFT) disp = -1;
else if (kDown & KEY_RIGHT) disp = +1;
else if (kDown & KEY_UP) disp = -6;
else if (kDown & KEY_DOWN) disp = +6;
else if (kDown & KEY_TOUCH)
{
if (pos.InRegion(0, 48, 8, 64*2+8)) disp = -1;
else if (pos.InRegion(256-8, 48, 8, 64*2+8)) disp = +1;
}
if (!disp) break;
int oldPage = page;
page += disp;
int nPages = bump.GetBumpCount();
while (page < 0) page += nPages;
while (page >= nPages) page -= nPages;
if (page == oldPage)
break;
unloadPageIcons(oldPage);
bump.SelectBump(page);
updCursor();
loadPageIcons();
} while(0);
// Selection code
if (kDown & KEY_TOUCH) do
{
bool bFound = false;
for (int i = 0; i < 6; i ++)
{
int xPos = 16 + (i % 3) * (64+16);
int yPos = 48 + (i / 3) * (64+8);
if (pos.InRegion(xPos, yPos, 64, 64))
{
int touchedApp = page*6 + i;
if (touchedApp == selectedApp)
executeApp();
else if (g_appData[touchedApp].IsLoaded())
{
selectedApp = touchedApp;
updCursor();
}
bFound = true;
break;
}
}
if (bFound) break;
for (int i = 0; i < g_appList.GetCount(); i ++)
{
if (!pos.InRegion(64+(i*(16+8)), 16, 16, 16))
continue;
SwitchToApp(i);
return true;
}
} while(0);
if (kDown & KEY_A)
executeApp();
#ifdef ALLOW_EXIT
if (g_appList.GetCount() == 0 && (kDown & KEY_START))
return false;
#endif
return true;
}