// Teil 2 folgt, leider WartezeitCode:/* Backpropagation Netz / net optimiert für Arduino Due Version JON 0060 (C) HaWe 2015 to do: - save memory to SD file - plug real physical sensors - pre-emptive Multitasking (currently not possible for Due) */ #include <SPI.h> #include <SD.h> #include <UTFT.h> #include <ardustdio.h> #include <malloc.h> #include <DueTimer.h> //------------------------------------------------------------------------------------- // neural net size #define NMAXIN 108 // max number of inputs (sensors) #define NMAXHID 20 // max number hidden layer neurons #define NMAXOUT 20 // max number output layer neurons #define NMAXPAT 70 // <<< max number of possibly trained patterns; //------------------------------------------------------------------------------------- // neural net: neurons and patterns float WeightLIn[NMAXIN+1][NMAXHID+1]; float WeightLOut[NMAXHID+1][NMAXOUT+1]; float Input[NMAXPAT+1][NMAXIN+1]; float Target[NMAXPAT+1][NMAXOUT+1]; float Output[NMAXPAT+1][NMAXOUT+1]; //float Contxt[NMAXOUT+1]; // Jordan-net: neuron-number == output-number float currIn[NMAXIN+1], // currently polled inputs inbuf[NMAXIN+1]; // intermediate stored inputs for editing float currOut[NMAXOUT+1], // currently computed net outputs outbuf[NMAXOUT+1]; // intermediate stored outputs int16_t NumInput = NMAXIN, NumHidden = NMAXHID, NumOutput = NMAXOUT; int16_t NumPattern = 0; // <<< number of actually trained patterns; int32_t epoch; // training measures uint32_t BPTime; float Error; //------------------------------------------------------------------------------------- // TFT LCD //UTFT myGLCD(Model, SDA=MISO, SCL, CS, RESET, RS) //UTFT myGLCD(QD220A, A2, A1, A5, A4, A3); // adjust model parameter and pins ! UTFT myGLCD(QD220A, 50, 49, 52, 0, 51); // A0->Vc (LED), A4->BoardReset extern uint8_t SmallFont[]; #define LCDWhiteBlack() {myGLCD.setColor(255, 255, 255); myGLCD.setBackColor( 0, 0, 0);} #define LCDNormal() {myGLCD.setColor(255, 255, 255); myGLCD.setBackColor( 0, 0, 0);} #define LCDInvers() {myGLCD.setColor( 0, 0, 0); myGLCD.setBackColor(255, 255, 255);} #define LCDWhiteRed() {myGLCD.setColor(255, 255, 255); myGLCD.setBackColor(255, 0, 0);} #define LCDRedBlack() {myGLCD.setColor(255, 0, 0); myGLCD.setBackColor( 0, 0, 0);} #define LCDYellowBlue() {myGLCD.setColor(255, 255, 0); myGLCD.setBackColor( 64, 64, 64);} uint8_t fontwi= 8; uint8_t fonthi=10; int16_t LCDmaxX , LCDmaxY ; // display size int16_t _curx_, _cury_, // last x,y cursor pos on TFT screen _maxx_, _maxy_; // max. x,y cursor pos on TFT screen char wspace[50]; // line of white space void lcdcls() { myGLCD.clrScr(); _curx_ =0; _cury_ =0; } void curlf() { _curx_=0; if( _cury_ <=(LCDmaxY-10) ) _cury_+=10; else _cury_=0; } void lcdprintxy(int16_t x, int16_t y, char * str) { myGLCD.print(str, x, y); _curx_ = x + strlen(str)*fontwi; _cury_ = y; // check for line overflow! } void curxy(int16_t x, int16_t y) {_curx_ = x;_cury_ = y;} void lcdprint(char * str) { myGLCD.print(str, _curx_, _cury_); _curx_ = _curx_ + strlen(str)*fontwi; //_cury_ = _cury_; // check for line overflow! } //------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------- // SD Card #define SD_CSpin 53 File myFile; char fname[64]; //------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------- // misc #define TimerMS() millis() #define LRAND_MAX 32767 #define srand(seed) randomSeed(seed) #define rand() random(LRAND_MAX) #define rando() ((float)rand()/(LRAND_MAX+1)) int32_t RSeed; #define pswitchon(pin) (!digitalRead(pin)) // btn press for pinMode(pin, INPUT_PULLUP) #define pswitchoff(pin) ( digitalRead(pin)) // btn press for pinMode(pin, INPUT_PULLUP) #define pbtn(pin) (!digitalRead(pin)) // alias //------------------------------------------------------------------------------------- //------------------------------------------------------------------------------------- // user interface: button pad control pins #define PIN_ESC 13 #define PIN_UP 12 #define PIN_OK 11 #define PIN_DN 4 // instead opt.: 6 #define PIN_LE 3 // instead opt.: 5 #define PIN_RI 2 // pins 10,9,8,7 : motor // pins 22 - 37 : motor // pins 49 - 51 : LCD TFT // pin 53: SD CS // available: 5+6, 38-48 for digital touch pins //------------------------------------------------------------------------------------- int16_t btnpressed() { return ( pbtn(PIN_ESC)||pbtn(PIN_UP)||pbtn(PIN_OK)||pbtn(PIN_DN)||pbtn(PIN_LE)||pbtn(PIN_RI) ); } //------------------------------------------------------------------------------------- int16_t getbtn() { int16_t choice= -1; while (! btnpressed() ); // wait until button pad pressed if( pbtn(PIN_ESC) ) choice = PIN_ESC; if( pbtn(PIN_UP) ) choice = PIN_UP; if( pbtn(PIN_OK) ) choice = PIN_OK; if( pbtn(PIN_DN) ) choice = PIN_DN; if( pbtn(PIN_LE) ) choice = PIN_LE; if( pbtn(PIN_RI) ) choice = PIN_RI; while ( btnpressed() ); // wait until button pad released return choice; } //------------------------------------------------------------------------------------- // misc. tools int16_t toggleup(int16_t lo, int16_t hi, int16_t val ) { if ( val < hi ) val++; else val = lo; return val; } int16_t toggledn(int16_t lo, int16_t hi, int16_t val ) { if ( val > lo ) val--; else val = hi; return val; } //------------------------------------------------------------------------------------- // motor control #define MAXMOTORS 4 // max number of encoder motors at Arduino Uno=2 // Due=6 // Mega=8 // motor 0 #define pinenc0A 22 // enc0A yellow #define pinenc0B 23 // enc0B blue #define pinmot0d1 24 // dir0-1 << #define pinmot0d2 25 // dir0-2 #define pinmot0pwm 10 // pwm enable0 // motor 1 #define pinenc1A 26 // enc1A yellow #define pinenc1B 27 // enc1B blue #define pinmot1d1 28 // dir1-1 << #define pinmot1d2 29 // dir1-2 #define pinmot1pwm 9 // pwm enable1 // motor 2 #define pinenc2A 30 // enc2A yellow #define pinenc2B 31 // enc2B blue #define pinmot2d1 32 // dir2-1 << #define pinmot2d2 33 // dir2-2 #define pinmot2pwm 8 // pwm enable2 // motor 3 #define pinenc3A 34 // enc3A yellow #define pinenc3B 35 // enc3B blue #define pinmot3d1 36 // dir3-1 << #define pinmot3d2 37 // dir3-2 #define pinmot3pwm 7 // pwm enable3 //------------------------------------------------------------------------------------- // motor bit patterns const char COAST[]={0,0,0}; const char BREAK[]={0,0,1}; const char FWSLOW[]={0,1,0}; const char RVSLOW[]={0,1,1}; const char FWMED[]={1,0,0}; const char RVMED[]={1,0,1}; const char FWFAST[]={1,1,0}; const char RVFAST[]={1,1,1}; #define fwslow 20 #define rvslow -20 #define fwmed 60 #define rvslow -60 #define fwfast 100 #define rvfast -100 //===================================================================================== // SETUP () //===================================================================================== void setup() { char sbuf[128]; // output string Serial.begin(115200); pinMode(PIN_ESC,INPUT_PULLUP); pinMode(PIN_UP, INPUT_PULLUP); pinMode(PIN_OK, INPUT_PULLUP); pinMode(PIN_DN, INPUT_PULLUP); pinMode(PIN_LE, INPUT_PULLUP); pinMode(PIN_RI, INPUT_PULLUP); myGLCD.InitLCD(); LCDmaxX=myGLCD.getDisplayXSize(); LCDmaxY=myGLCD.getDisplayYSize(); myGLCD.setFont(SmallFont); _maxx_ = LCDmaxX / fontwi; _maxy_ = LCDmaxX / fonthi; memset(wspace, ' ', _maxx_); wspace[_maxx_]='\0'; lcdcls(); sprintf(sbuf, "Serial ok, GLCD=%dx%d",LCDmaxX,LCDmaxY); Serial.println(sbuf); myGLCD.print (sbuf, 0, 0); ResetNet(); RSeed = ( ((analogRead(A8)+1017)%100) * (TimerMS()% LRAND_MAX ) % LRAND_MAX ); srand(RSeed); // # DEBUG sprintf(sbuf, "Seed= %ld", RSeed); myGLCD.print (sbuf, 0,10); sprintf(sbuf, "Rand= %ld", rand() ); myGLCD.print (sbuf, 100,10); // # DEBUG memtest(20, "memtest: setup"); SetNetDefaultPatterns(); RefreshInputs(); // polls inputs and stores values to currIn[] array //*********************************** //******************************************************************************* // debug: test different input sets currIn[1]=1; //currIn[2]=0; currIn[3]=1; //currIn[4]=0; currIn[5]=0; currIn[6]=0; currIn[7]=0; //******************************************************************************* //*********************************** ComputeMatrix(); // applies inputs to net and computes outputs to currOut[] array TrainNet(); // basic training } //===================================================================================== //===================================================================================== //------------------------------------------------------------------------------------- // analog range bit patterns // returns 6 bits for ranges -32768...0...32767 or out of bounds (VOID==[32]) float Ana[64][8]={ {0,0,0,0,0,0,0,0}, // 0 {0,1,0,0,0,0,0,1}, // 0...1 {1,2,0,0,0,0,1,0}, // 1...2 {2,3,0,0,0,0,1,1}, {3,4,0,0,0,1,0,0}, {4,5,0,0,0,1,0,1}, {5,6,0,0,0,1,1,0}, {6,8,0,0,0,1,1,1}, {8,10,0,0,1,0,0,0}, {10,13,0,0,1,0,0,1}, {13,16,0,0,1,0,1,0}, {16,20,0,0,1,0,1,1}, {20,26,0,0,1,1,0,0}, {26,32,0,0,1,1,0,1}, {32,40,0,0,1,1,1,0}, {40,48,0,0,1,1,1,1}, {48,56,0,1,0,0,0,0}, {56,64,0,1,0,0,0,1}, {64,74,0,1,0,0,1,0}, {74,85,0,1,0,0,1,1}, {85,97,0,1,0,1,0,0}, {97,110,0,1,0,1,0,1}, {110,128,0,1,0,1,1,0}, {128,192,0,1,0,1,1,1}, {192,256,0,1,1,0,0,0}, {256,420,0,1,1,0,0,1}, {420,512,0,1,1,0,1,0}, {512,650,0,1,1,0,1,1}, {650,800,0,1,1,1,0,0}, {800,1024,0,1,1,1,0,1}, {1024,2048,0,1,1,1,1,0}, {2048,32767,0,1,1,1,1,1}, // <= SHORTMAX {0,0,1,0,0,0,0,0}, // #32: VOID ! {0,-1,1,0,0,0,0,1}, // 0... -1 {-1,-2,1,0,0,0,1,0}, // -1...-2 {-2,-3,1,0,0,0,1,1}, {-3,-4,1,0,0,1,0,0}, {-4,-5,1,0,0,1,0,1}, {-5,-6,1,0,0,1,1,0}, {-6,-8,1,0,0,1,1,1}, {-8,-10,1,0,1,0,0,0}, {-10,-13,1,0,1,0,0,1}, {-13,-16,1,0,1,0,1,0}, {-16,-20,1,0,1,0,1,1}, {-20,-26,1,0,1,1,0,0}, {-26,-32,1,0,1,1,0,1}, {-32,-40,1,0,1,1,1,0}, {-40,-48,1,0,1,1,1,1}, {-48,-56,1,1,0,0,0,0}, {-56,-64,1,1,0,0,0,1}, {-64,-74,1,1,0,0,1,0}, {-74,-85,1,1,0,0,1,1}, {-85,-97,1,1,0,1,0,0}, {-97,-110,1,1,0,1,0,1}, {-110,-128,1,1,0,1,1,0}, {-128,-192,1,1,0,1,1,1}, {-192,-256,1,1,1,0,0,0}, {-256,-420,1,1,1,0,0,1}, {-420,-512,1,1,1,0,1,0}, {-512,-650,1,1,1,0,1,1}, {-650,-800,1,1,1,1,0,0}, {-800,-1024,1,1,1,1,0,1}, {-1024,-2048,1,1,1,1,1,0}, {-2048,-32768,1,1,1,1,1,1} // >= -SHORTMAX }; //------------------------------------------------------------------------------------- int16_t ana2bits(int16_t aval) { // return array index int16_t i,j; if(aval == 0) return 0; else if( aval > 32767 ) return 32; // VOID else if( aval < -32768 ) return 32; // VOID else { for (i=1; i<32; ++i) { if ( (aval>Ana[0][i]) && (aval<=Ana[1][i]) ) return i; } for (i=32; i<64; ++i) { if ( (aval<Ana[0][i]) && (aval>=Ana[1][i]) ) return i; } } return 32; } //------------------------------------------------------------------------------------- // mem test extern char _end; extern "C" char *sbrk(int i); char *ramstart=(char *)0x20070000; char *ramend=(char *)0x20088000; //===================================================================================== void memtest(int ypos, char * str) { char sbuf[128]; // output string char *heapend=sbrk(0); register char * stack_ptr asm ("sp"); struct mallinfo mi=mallinfo(); sprintf(sbuf,str); Serial.println(); Serial.println(sbuf); myGLCD.print(sbuf, 0, ypos); sprintf(sbuf, "Dyn.RAM used: %-10ld ", mi.uordblks); Serial.println(sbuf); myGLCD.print(sbuf, 0, ypos+10); sprintf(sbuf, "Prg.stat.RAM used %-10ld ", & _end - ramstart); Serial.println(sbuf); myGLCD.print(sbuf, 0, ypos+20); sprintf(sbuf, "Stack RAM used %-10ld ", ramend - stack_ptr); Serial.println(sbuf); myGLCD.print(sbuf, 0, ypos+30); sprintf(sbuf, "Free mem: %-10ld ", stack_ptr - heapend + mi.fordblks); Serial.println(sbuf); myGLCD.print(sbuf, 0, ypos+40); myGLCD.print(wspace, 0, ypos+50); } //===================================================================================== // net: clear, set, reset, patch //===================================================================================== void clrnetbuf() { memset(inbuf, 0, sizeof(inbuf) ); memset(currIn, 0, sizeof(currIn) ); memset(outbuf, 0, sizeof(outbuf) ); memset(currOut, 0, sizeof(outbuf) ); } //===================================================================================== void ResetNet(){ memset(Input, 0, sizeof(Input) ); memset(Target, 0, sizeof(Target) ); //memset(Contxt, 0, sizeof(Contxt) ); clrnetbuf(); NumPattern=0; } //===================================================================================== // set input/target patterns manually //===================================================================================== int16_t setIOpattern(int16_t patt) { int16_t result; char sbuf[128]; // output string if(patt==-1) patt = CheckTestPattern(inbuf); // pattern number if known, if not: -1 if( patt > 0 ) sprintf(sbuf, " PATCH target #%-3d", patt); else sprintf(sbuf, " NEW target #%-3d", NumPattern+1); result=menu_setouttargets(sbuf); if (result == -1) return (result); // break by escape btn if (patt == -1) { // unknown => add new pattern! SetNewPattern( inbuf, outbuf ); result=NumPattern; } else { // override old pattern PatchPattern( patt, inbuf, outbuf); result=patt; } return result; // return written pattern number } //===================================================================================== //===================================================================================== // display net inputs and target outputs //===================================================================================== void DisplayNetTrainingIOs(int16_t code) { int16_t i, o, p; char sbuf[128]; // output string char msg[20]="! SUCCESS !" ; if (code==0) strcpy(msg,"calc.error"); if (code==2) strcpy(msg,"user-break"); /* print network outputs */ Serial.println(); Serial.println(); Serial.println(msg); sprintf(sbuf, "%-6ld: Err= %9.7f", epoch, Error) ; Serial.print("Epoch "); Serial.println(sbuf); myGLCD.print(sbuf, 0, 20); myGLCD.print(msg, 0, 30); sprintf(sbuf, "NET TRAINING TIME = %ld sec \n", BPTime/1000); Serial.println(sbuf); Serial.println(); sprintf(sbuf, "Patt. ") ; Serial.print(sbuf); myGLCD.print(sbuf, 0, 40); for( i = 1 ; i <= NumInput ; i++ ) { sprintf(sbuf, "Inp%-3d ", i) ; Serial.print(sbuf); } for( o = 1 ; o <= NumOutput ; o++ ) { sprintf(sbuf, "Targ%-3d Outp%-3d ", o, o); Serial.print(sbuf); } for(int p = 1 ; p <= NumPattern ; p++ ) { Serial.println(); sprintf(sbuf, "%3d ", p) ; Serial.print(sbuf); myGLCD.print(sbuf, 40, 40); for( i = 1 ; i <= NumInput ; i++ ) { sprintf(sbuf, "%5.2f ", Input[p][i]) ; Serial.print(sbuf); } for( o = 1 ; o <= NumOutput ; o++ ) { sprintf(sbuf, "%5.2f %5.2f ", Target[p][o], Output[p][o]) ; Serial.print(sbuf); } } Serial.println(); Serial.println(); sprintf(sbuf, "BPtrained, returning to main()...!") ; Serial.print(sbuf); Serial.println(); Serial.println(); } //===================================================================================== void displayMatrix(int16_t patt) { int32_t btn=-1; int16_t i, l, o, ibuf, p, lval=1, hval=NumPattern; float fbuf; char msgline[30], sbuf[30]; char valline[30]; p=patt; if(p==0) p=1; if(p>NumPattern) p=NumPattern; do { strcpy(msgline," 12345678901234567890"); sprintf(sbuf, "%-4d", p); strinsert(msgline, sbuf, 0); LCDWhiteRed(); Serial.println(msgline); lcdprintxy(0,0,msgline); curlf(); LCDNormal(); for (i=1; i<=NMAXIN; ++i) { l=(i-1)/20; if (i%20==1) { sprintf(sbuf, "%4d ", i-1); Serial.print(sbuf); lcdprint(sbuf); } ibuf = round(Input[p][i]); sprintf(sbuf, "%1d", ibuf); Serial.print(sbuf); lcdprint(sbuf); if (i%20==0) { Serial.println(); curlf(); } } Serial.println(); curlf(); strcpy(msgline,"TARG 12345678901234567890"); LCDRedBlack(); Serial.println(msgline); lcdprint(msgline); curlf(); LCDNormal(); for (i=1; i<=NMAXOUT; ++i) { l=(i-1)/20; if (i%20==1) { sprintf(sbuf, "%4d ", i-1); Serial.print(sbuf); lcdprint(sbuf); } ibuf = round(Target[p][i]); sprintf(sbuf, "%1d", ibuf); Serial.print(sbuf); lcdprint(sbuf); if (i%20==0) { Serial.println(); curlf(); } } Serial.println(); LCDYellowBlue(); sprintf(msgline, "toggle patt +-1: LEFT/RIGHT"); Serial.println(msgline); lcdprintxy(0, LCDmaxY-20, msgline); sprintf(msgline, "+-10:UP/DN edit:OK quit:ESC"); Serial.println(msgline); lcdprintxy(0, LCDmaxY-10, msgline); LCDNormal(); btn=getbtn(); if ( (btn==PIN_RI) || (btn==PIN_LE) ) { // browse displayed pattern if (btn==PIN_RI) p=toggleup(lval, hval, p); if (btn==PIN_LE) p=toggledn(lval, hval, p); } if ( (btn==PIN_UP) || (btn==PIN_DN) ) { // browse displayed pattern if (btn==PIN_UP) p=toggleup(lval, hval, p+9); if (btn==PIN_DN) p=toggledn(lval, hval, p-9); } if ( (btn==PIN_OK) ) { // change displayed pattern setIOpattern(p); } } while ( (btn!=PIN_ESC ) ); } //===================================================================================== int16_t RefreshInputs() { // polls inputs and stores values to currIn[] array // poll digital touch values and store directly (1 Dpin = 1 input) 16 DPins == 16 // poll analog sensor values and store bit pattern for ranges (1 Apin = 6 inputs) 8 A10bit == 48 // poll motor speed and store bit pattern for speed ranges (1 Apin = 6 inputs) 4 motors == 24 // Jordan/Elman neural context neurons == feedback inputs (1...NMAXCON) 20 inputs == 20 // =108 int16_t i, cx; cx = NMAXIN - NMAXOUT +1; // last NMAXCON inputs reserved for context neurons //for(i=cx; i<= NMAXIN; ++i) { currIn[i] = Contxt[i] ; } // } //===================================================================================== void ComputeMatrix() { // applies currIn[] inputs to net and computes outputs (outbuf[]) int16_t i, j, o; float SumH[NMAXHID+1]; float SumO[NMAXOUT+1]; float HidOut[NMAXHID+1]; float SumDOW[NMAXHID+1]; float lambda= 0.5; // context neuron self activation rate memset(SumH, 0, sizeof(SumH) ); memset(SumO, 0, sizeof(SumO) ); memset(HidOut, 0, sizeof(HidOut) ); memset(SumDOW, 0, sizeof(SumDOW) ); for( j = 1 ; j <= NumHidden ; j++ ) { // compute hidden unit activations SumH[j] = WeightLIn[0][j] ; // bias neuron for( i = 1 ; i <= NumInput ; i++ ) { SumH[j] += currIn[i] * WeightLIn[i][j] ; } HidOut[j] = 1.0/(1.0 + exp(-SumH[j])) ; // Sigmoidal Outputs } for( o = 1 ; o <= NumOutput ; o++ ) { // compute output unit activations and errors SumO[o] = WeightLOut[0][o] ; // bias neuron for( j = 1 ; j <= NumHidden ; j++ ) { SumO[o] += HidOut[j] * WeightLOut[j][o] ; } currOut[o] = 1.0/(1.0 + exp(-SumO[o])) ; // Sigmoidal Outputs } //for( o = 1 ; o <= NumOutput ; o++ ) { // compute context neurons // Contxt[o] = lambda*Contxt[o] + (1-lambda)*currOut[o]; // assign outputs*weight to context neurons //} } //===================================================================================== // check for known patterns //===================================================================================== int16_t CheckTestPattern(float * test) { int16_t i, p; for( p = 1; p <= NumPattern ; ++p) { for( i = 1; (i <= NumInput) && ( Input[p][i] == test[i] ); ++i); if (i > NumInput) {return p; } } return -1; } //===================================================================================== // patch old pattern //===================================================================================== void PatchPattern(int16_t patt, float * _ibuf, float * _obuf ){ int16_t i, o; if ( (NumPattern <= NMAXPAT) && ( NumPattern > 0) ) { // # DEBUG //Serial.print("NumPattern="); Serial.print(NumPattern); Serial.print(" SetNetPattern="); Serial.println(patt); for(i=1; i<=NMAXIN; ++i) { Input[patt][i] = _ibuf[i]; } for(o=1; o<=NMAXOUT; ++o) { Target[patt][o] = _obuf[o]; } } } //===================================================================================== // add new pattern //===================================================================================== void SetNewPattern(float * _ibuf, float * _obuf ){ int16_t i, o; if ( (NumPattern < NMAXPAT) && ( NumPattern >= 0) ) { NumPattern++; // # DEBUG //Serial.print("NumPattern="); Serial.print(NumPattern); Serial.print(" SetNetPattern="); Serial.println(patt); for(i=1; i<=NMAXIN; ++i) { Input[NumPattern][i] = _ibuf[i]; } for(o=1; o<=NMAXOUT; ++o) { Target[NumPattern][o] = _obuf[o]; } } }
Zitieren
Lesezeichen