File content as of revision 0:ac93d9db8dbd:

/*
    This file is part of mbeddedNets.

    mbeddedNets is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    mbeddedNets is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.
    
    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
//
//  Description: For use with the m3pi in conjunction with Ping))) sensors.
//               Creates user defined neural nets, iterating through each to
//               complete a task. Reward is calculated using reflactance sensors
//               on underside of m3pi chassis.

#include <vector>
#include <math.h>
#include <cstdlib>
#include <ctime>

#include "mbed.h"
#include "m3pimaze.h"
#include "Ping.h"
#include "CNeuralNet.h"
#include "CGenAlg.h"
#include "utils.h"

const unsigned int NBots = 6; //set number of networks each bot has
const unsigned int Gens = 48; //number of generations to run
const float MAX = 0.5;
const unsigned int CSz = 90; //half of container size in cm, limiting squashing function

 m3pi m3pi(p23,p9,p10);
 Ping Pinger1(p15);
 Ping Pinger2(p16);
 LocalFileSystem local("local");//output storage

int main()
{
     float pr1; //right
     float pr2; //left
     Pinger1.Send();//send ping
     Pinger2.Send();    
     wait_ms(30);
     pr1 = Pinger1.Read_cm();
     pr2 = Pinger2.Read_cm();
     pr1 = pr1 * 100;//for seeding random # generator
     pr2 = pr2 * 100;
     
     srand(int(pr2*pr1));
     FILE *fp = fopen("/local/out.txt", "a");
     fprintf(fp, "Bot tick fit rIn lIn out1 out2");
     fprintf(fp, "\n");
     fclose(fp);
     
     CNeuralNet myNets[NBots]; //networks
     vector<SGenome> botGenes; //genes
     CGenAlg* m_pGA; //genetic algorithm
     CGenAlg pGA(NBots, 0.1, 0.7, myNets[0].GetNumberOfWeights()); //nets,mutrate,crossrate,weightsinnn
     m_pGA = &pGA;
     botGenes = m_pGA->GetChromos();
     
     for (int i=0; i<NBots; i++){
         myNets[i].PutWeights(botGenes[i].vecWeights);
     }
    
    for(int j=0; j<Gens; j++){  
        for(int i=0; i<NBots; i++){
            //int fit = 0;
            float fit = 0;
            int ticks = 128;
            int sensors[5];
            m3pi.locate(0,1);
            m3pi.sensor_auto_calibrate();//calibrate reflectance sensors
            
            //check if goal found
            while(ticks >= 0){       
                Pinger1.Send();    
                Pinger2.Send();
                wait_ms(30);
                pr1 = Pinger1.Read_cm();
                pr2 = Pinger2.Read_cm();
                if(pr1 < 10 || pr2 < 10){
                    break;  
                }
                Squash(pr1,CSz,-1,1);
                Squash(pr2,CSz,-1,1);
                vector<float> inputs; //inputs to network
                inputs.push_back(pr1);
                inputs.push_back(pr2);
                vector<float> output = myNets[i].Update(inputs);//get outputs from network
                m3pi.right_motor(output[0] * MAX);
                m3pi.left_motor(output[1] * MAX);
                m3pi.readsensor(sensors);
                if(sensors[0] > 600 && sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600 && sensors[4] > 600){
                    fit=384.0;
                    FILE *fp = fopen("/local/out.txt", "a");
                    fprintf(fp, "%d, %d, %f, %f, %f, %f, %f", i, ticks, fit, pr1, pr2, output[0], output[1]);
                    fprintf(fp, "\n");
                    fclose(fp);
                    break;
                }
                FILE *fp = fopen("/local/out.txt", "a");
                fprintf(fp, "%d, %d, %f, %f, %f, %f, %f", i, ticks, fit, pr1, pr2, output[0], output[1]);
                fprintf(fp, "\n");
                fclose(fp);
                ticks--;
                //fit++;
                fit += (output[0] + output[1]);
                wait(0.1);
            }
            m3pi.right_motor(0);//stop bot
            m3pi.left_motor(0);
            botGenes[i].dFitness = fit;
            wait(6); //reset robot position
        }
        botGenes = m_pGA->Epoch(botGenes);
        for (int i=0; i<NBots; ++i){
            myNets[i].PutWeights(botGenes[i].vecWeights);
        }     
    }      
    return 0;
}