Diff: Code/main.cpp

Revision:

0:ac93d9db8dbd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Code/main.cpp	Thu Apr 23 20:16:51 2015 +0000
@@ -0,0 +1,130 @@
+/*
+    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;
+}