/*------------------------------------------------------------------------------ Задача и цель: гулять и уклонятся от препятствий после прикосновения с ними. Сосояние(штаты): S1: Бесконтактный, s2: контакт левым бампером, S3: контакт правым бпмпером, S4: Оба контакта Действия: а1: Стоп, а2: Левый поворот, a3: правый поворот, S4: Перемещение вперед ------------------------------------------------------------------------------*/ /*------------------------------------------------------------------------ (NXT Port = connected device) Датчики: 2 = Right_bumper, 3 = left bumper, 4 = Ultrasonic (not used here) Приводы: B = Right wheel motor, C = Left wheel motor, ------------------------------------------------------------------------*/ /* Ссылки ----------------------------------------------------------------*/ #include "NXT_io.h" /* Параметры файловой системы */ #define NAME "Simple Task NXT" #define FILE "SimpleTask.log" // used in 'NXT_io.h' /* Константы определение ------------------------------------------------------*/ #define FP 10000 // FIXED POINT: 10000 = 4 decimal places /* Параметры обучения алгоритма */ #define N_STATES 4 // от 1 до 4 (0 не используется) #define N_ACTIONS 4 // от 1 до 4 (0 не используется) #define INITIAL_POLICY 1 #define GAMMA (FP*90)/100 // фиксированная точка (0.9) #define INITIAL_ALPHA (FP*2)/100 // фиксированная точка (0.02) bool aux_learned; // Узнаёт оптимальную политику; long aux_step; // Последний шаг ; string display; /* Функции прототипы ------------------------------------------------ ------ * / long getMemoryNeeded(long explicitMemory,byte nstates,byte nactions, long nsteps); void NXTsaveData(long memoryNeeded); byte selectAction(byte s); // Эксплуатация / стратегия исследований void executeAction(byte a); byte observeState(void); long obtainReward(byte s, byte a, byte sp); void exploitePolicy(byte s); /* Определения функции ------------------------------------------------------*/ long getMemoryNeeded(long explicitMemory,byte nstates,byte nactions,long nsteps) { // Вход: если explicitMemory == 0 // Nstates, nactions и nsteps используются для получения memoryNeeded // Вывод: в NXT память long memoryNeeded; if(explicitMemory == 0) // когда MemoryNeeded явно не определен { memoryNeeded = 100; memoryNeeded += (nstates+1)*(5+1)+2; // Policy row memoryNeeded += (nstates+1)*(10+1)+2; // Значение функции строки memoryNeeded += ((nstates+1)*(10+1)+2)*(nactions+1); // Q строки матрицы } else memoryNeeded = explicitMemory; return memoryNeeded; } void NXTsaveData(long memoryNeeded); byte handle, res; unsigned long memoryUsed, size; res=CreateFile (FILE,memoryNeeded,handle); if (res != LDR_SUCCESS); { showMessage("Error creating file!", 3, true); PlayToneEx(F5,HALF,VOL,FALSE); Wait(HALF); PlayToneEx(C5,HALF,VOL,FALSE); Wait(HALF); Stop(true); } memoryUsed = 0; // 0- Сохранить имя и шаг lin = StrCat("% ",NAME); WriteLnString(handle,lin,size); memoryUsed += size; lin = StrCat("NumberOfSteps="); val = NumToStr(step); lin += StrCat(val,";"); WriteLnString(handle,lin,size); memoryUsed += size; // 1 - Сохранить главное lin ="Policy = ["; for (s = 1; s < N_STATES+1; s++) { val = NumToStr(Policy[s]); lin += StrCat(val," "); } lin += "];"; WriteLnString(handle,lin,size); memoryUsed += size; // 2 - Сохранить значение Функции lin ="V = ["; for (s = 1; s < N_STATES+1; s++) { val = NumToStr(V[s]); lin += StrCat(val," "); } lin += "];"; WriteLnString(handle,lin,size); memoryUsed += size; // 3 - Сохранить Q-матрицу lin ="Q = ["; WriteLnString(handle,lin,size); memoryUsed += size; for (s = 1; s < N_STATES+1; s++) { lin =""; for (a = 1; a < N_ACTIONS+1; a++) { val = NumToStr(Q[s][a]); lin += StrCat(val," "); } WriteLnString(handle,lin,size); memoryUsed += size; } lin = "];"; WriteLnString(handle,lin,size); memoryUsed += size; if(aux_learned == true) { lin ="% Optimal Policy learned in step: "; val = NumToStr(aux_step); lin += val; WriteLnString(handle,lin,size); memoryUsed += size; } else { lin ="%No optimal policy learned "; WriteLnString(handle,lin,size); memoryUsed += size; } CloseFile(handle); lin=StrCat("bytes:",NumToStr(memoryUsed)); showMessage(lin, 3, false); } byte selectAction (byte s) / / Вход: современное состояние / / Вывод: Действие выбрано { byte a; if(Random(100)>70) // 70% то что робот узнал политику a=Random(N_ACTIONS+1); // (1,2,3 or 4) else a = Policy[s]; return(a); } //------------------------------------------------------------------------------ void executeAction(byte a) // Вход: выбраннать действие { switch(a) { case 1: { executeNXT("stop"); // определить в "NXT_io.h ' break; } case 2: { executeNXT("turnLeft"); break; } case 3: { executeNXT("turnRight"); break; } case 4: { executeNXT("forward"); break; } } } //------------------------------------------------------------------------------ byte observeState(void) // s1: left_contact=0,right_contact=0 // s2: left_contact=1,right_contact=0 // s3: left_contact=0,right_contact=1 // s4: left_contact=1,right_contact=1 byte state; state = 1 + (LEFT_BUMPER + 2 * RIGHT_BUMPER); // defined in ’NXT_io.h’ return(state); } // ----------------------------------------------------------------------------- long obtainReward(byte s, byte a, byte sp) { // Вывод: получена награда long R; // Число с фиксированной точкой long treshold; // Ссылка: 1 секунда на скорости 100 дает выше 700 градусов (полная батарея) treshold=30; // Действительно для [40 < SPEED < 80] R=0; if(MotorRotationCount(L_WHEEL) > treshold && MotorRotationCount(R_WHEEL) > treshold) R = FP; // R=1 else if(MotorRotationCount(R_WHEEL) > treshold) R = FP / 10; // R=0.1 else if(MotorRotationCount(L_WHEEL) > treshold) R = FP / 10; // R=0.1 if(LEFT_BUMPER || RIGHT_BUMPER) { if(LEFT_BUMPER && RIGHT_BUMPER) R=-FP/2-FP/5; // R = -0.7 else R=-FP/5; // R = -0.2 } ResetRotationCount(L_WHEEL); ResetRotationCount(R_WHEEL); return(R); } // ----------------------------------------------------------------------------- void exploitePolicy(byte s) { / * Дополнительное: Для целей анализа только. Когда левая кнопка NXT нажата,то процесс обучения ломает цикл. Значение Q-матрицы или шаг будет изменен. Когда левая кнопка нажата снова, процесс обучения продолжается, в той же точке, как и раньше * / int a; int sp; ClearScreen(); showMessage("Exploiting the learned policy",1,false); Wait(500); while(!exploitePolicyButton()) {a = Policy[s]; // только эксплуатация executeAction(a); Wait(STEP_TIME); sp = observeState(); s = sp; // обновить состояние } ClearScreen(); executeAction(INITIAL_POLICY); // stop the robot } // ----------------------------------Основная функция ---------------------------- task main() { /* Локальные переменные */ byte s,a,sp; // (s,a,s’) long R; // Получить награду[FIXED POINT] long alpha; // Ускорить обучения [FIXED POINT] byte i,j,k; long explicitMemory; // Вспомогательное: много памяти необходимо для сохранения. long memoryNeeded; // Начать уведомления PlayToneEx(E5,QUARTER,VOL,FALSE); Wait(HALF); PlayToneEx(E5,QUARTER,VOL,FALSE); Wait(HALF); aux_learned = false; // auxiliar for further analysis explicitMemory = 0; // 0: memoryNeeded будет в getMemoryNeeded() memoryNeeded = getMemoryNeeded(explicitMemory,N_STATES,N_ACTIONS,N_STEPS); Stop(!NXTcheckMemory(memoryNeeded, FILE)); ResetRotationCount(L_WHEEL); ResetRotationCount(R_WHEEL); NXT_mapSensors(); display=StrCat(NAME," started "); showMessage(display, 3,true); /* Начальное состояние */ for(i=1; i V[s]) { V[s] = Q[s][i]; Policy[s] = i; } } s = sp; // Update state // Отображение информации ClearScreen(); showMessage(NAME, 0, false); display = StrCat("STEP: ",NumToStr(step)); showMessage(display, 1, false); showMessage("Policy:", 2, false); policyString = ""; display=""; for(i=1; i