bool EnvironmentROBARM::InitializeEnvironment()
{
short unsigned int coord[NUMOFLINKS];
double startangles[NUMOFLINKS];
double angles[NUMOFLINKS];
int i;
short unsigned int endeffx, endeffy;
//initialize the map from Coord to StateID
EnvROBARM.HashTableSize = 32*1024; //should be power of two
EnvROBARM.Coord2StateIDHashTable = new vector<EnvROBARMHashEntry_t*>[EnvROBARM.HashTableSize];
//initialize the map from StateID to Coord
EnvROBARM.StateID2CoordTable.clear();
//initialize the angles of the start states
for(i = 0; i < NUMOFLINKS; i++)
{
startangles[i] = PI_CONST*(EnvROBARMCfg.LinkStartAngles_d[i]/180.0);
}
ComputeCoord(startangles, coord);
ComputeContAngles(coord, angles);
ComputeEndEffectorPos(angles, &endeffx, &endeffy);
//create the start state
EnvROBARM.startHashEntry = CreateNewHashEntry(coord, NUMOFLINKS, endeffx, endeffy);
//create the goal state
//initialize the coord of goal state
for(i = 0; i < NUMOFLINKS; i++)
{
coord[i] = 0;
}
EnvROBARM.goalHashEntry = CreateNewHashEntry(coord, NUMOFLINKS, EnvROBARMCfg.EndEffGoalX_c, EnvROBARMCfg.EndEffGoalY_c);
//check the validity of both goal and start configurations
//testing for EnvROBARMCfg.EndEffGoalX_c < 0 and EnvROBARMCfg.EndEffGoalY_c < 0 is useless since they are unsigned
if(!IsValidCoord(EnvROBARM.startHashEntry->coord) || EnvROBARMCfg.EndEffGoalX_c >= EnvROBARMCfg.EnvWidth_c ||
EnvROBARMCfg.EndEffGoalY_c >= EnvROBARMCfg.EnvHeight_c)
{
printf("Either start or goal configuration is invalid\n");
return false;
}
//for now heuristics are not set
EnvROBARM.Heur = NULL;
return true;
}
/**
* @function GUI_M_KeyboardCreate
* @brief open top layer & launch the keyboard
* @param coord_t x, y: desired coords for window; will be checked/modified
* @param kbd_type_e _type: keyboard type (KEYBOARD_TYPE_FR_FR, KEYBOARD_TYPE_EN_US...)
* @param void *kbdBuffer: user entry buffer; used as input/output; will be cast to uint8_t*
* @param uint16_t bufferSize: size (in bytes) of kbdBuffer
* @param signal_t _sOk: signal when <ENTER> key
* @param signal_t _sEsc: signal when <ESC> key
* @return int8_t: 0 success, -1 error; in case of error, the top layer is automatically closed
*/
int8_t GUI_M_KeyboardCreate(coord_t x, coord_t y, kbd_type_e _type, void *kbdBuffer, uint16_t bufferSize, signal_t _sOk, signal_t _sEsc) {
const key_entry_st *pEntry;
uint8_t *buff;
rect_st rec, recWnd, recEntry, recKbd, recBackground;
bool bError = false;
lut8bpp_st tmpLut;
int8_t res = -1;
/*open the top layer*/
GUI_TopLayerOpen(GUI_M_KeyboardHandle);
/*locals init*/
pKeyList = NULL;
buff = (uint8_t *) kbdBuffer;
SetMap(KBD_MAP_STD, true); /*always start in STD map*/
buff[bufferSize - 1] = 0; /*always ensure that the user buffer is \0 terminated*/
sEsc = _sEsc;
sOk = _sOk;
/*check type, then retrieve keys array address*/
if(_type < KEYBOARD_TYPE_COUNT) {
type = _type;
pKeyList = arKeyboard[type];
}
/*set the font now (needed for calculation above)*/
SetFont(arKeyboardStyle[type]->fontKbd);
/*get window, user entry & keyboard coordinates; checks also if the selected keyboard fits into the lcd*/
if(pKeyList != NULL && ComputeCoord(x, y, &recWnd, &recEntry, &recKbd) == 0) {
/*clear window content*/
P2D_SetColor(GetColor(G_COL_KBD_BACKGROUND));
P2D_SetClip(&recWnd);
//P2D_FillRect(&recWnd);
recBackground = recWnd;
recBackground.x = 20;
recBackground.y = 20;
P2D_InitLut8BPP(&tmpLut, sprite_background_lut, LUT_E_COPY);
P2D_SpriteSetLut8BPP(&tmpLut);
P2D_Sprite(&recBackground, &recWnd, sprite_background);
/*add user entry*/
pUsrEntryObj = GUI_W_UsrEntryAdd(&recEntry, buff, bufferSize, true);
if(pUsrEntryObj != NULL) {
/*key configuration*/
GUI_SetGroup(1);
GUI_KeySetConf(arKeyboardStyle[type]->fontKbd, arKeyboardStyle[type]->fontSymb, &mapId); /*key config: font key, font symbol & current map pointer*/
/*instanciate each key*/
pEntry = pKeyList;
while(pEntry != NULL && pEntry->x >= 0 && bError == false) {
rec.x = pEntry->x + recKbd.x;
rec.y = pEntry->y + recKbd.y;
rec.w = pEntry->w;
rec.h = pEntry->h;
if(GUI_AddKey(&rec, pEntry->glyph) != NULL) {
/*special key (ENTER/ESC...) trig signal when the key is released*/
if(pEntry->glyph[0] < (uint8_t) ' ') {
GUI_SetSignal(E_PUSHED_TO_RELEASED, pEntry->glyph[0]);
}
/*printable glyph? trig signal as soon as the key is pressed*/
else {
GUI_SetSignal(E_RELEASED_TO_PUSHED, pEntry->glyph[0]);
}
}
else {
bError = true; /*not enougth memory :/ */
}
pEntry++; /*next key*/
}
GUI_SetGroup(0);
if(bError == false) res = 0;
}
}
/*in case of error, kill top layer*/
if(res < 0) {
GUI_TopLayerClose();
}
return res;
}
