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