Diff: Main.cpp

Revision:

7:95ebadc83fc7

Parent:

6:6e96e93689df

diff -r 6e96e93689df -r 95ebadc83fc7 Main.cpp
--- a/Main.cpp	Thu Apr 03 19:21:58 2014 +0000
+++ b/Main.cpp	Thu Apr 03 23:57:31 2014 +0000
@@ -1,9 +1,10 @@
 //#include "Core.h"
 
 #include "Sensors.h"
-#include "Motors.h"
+#include "PID.h"
 #include "Communication.h"
 #include "Flooding.h"
+#include "Movement.h"
 
 #include "BufferAverage.h"
 #include "ExponentialAverage.h"
@@ -20,36 +21,14 @@
     
     wait(3);
     /*
-    
-    
+
     STEVE!!!!!!
     WHENEVER YOU ARE IN A CELL AND YOU WANT TO KNOW WHAT THE NEXT BEST MOVE IS
     USE THE flood_findPath(x, y)  where x and y are your current possition
     
     the function will return a MOVE type
     and you can use something similar to this:
-    
-    Move k = flood_findPath(xPos, yPos);
-    switch(k)
-    {
-        case M_NORTH:
-            moveNORTH();
-            break;
-     
-        case M_SOUTH:
-            moveSOUTH();
-            break;
-     
-        case M_WEST:
-            moveWEST();
-            break;
-        
-        case M_EAST:
-            moveEAST();
-            break;             
-        
-    }   
-    
+
     i've written the code for the following:
     
     
@@ -66,129 +45,38 @@
     
     */
 
-    initMapping();
+    //initMapping();
     initSensors();
-
-    
     setRightSpeed(3);
     setLeftSpeed(3);
     t.start();
     
     //while (true)
     //while(SenseF.read() < 0.99)
-    while(frontExpAvg.average() < .8) // && wallRight() && wallLeft()
-    { 
-        frontExpAvg.add(SenseF.read());
-    
-        if ( wallRight() && wallLeft())
-        PIDv = PID(linearize(SenseR.read()), linearize(SenseL.read()));
-        if ( wallRight() && !wallLeft())
-        PIDv = PID(linearize(SenseR.read()),cal_R);
-        if ( !wallRight() && wallLeft())
-        PIDv = PID(cal_L, linearize(SenseL.read()));
-        
-        if(PIDv < -0.1)
-        {
-            setRightSpeed(3);
-            setLeftSpeed(4);
-            wait_ms(20); 
+    //while(true)
+        //{
+        t.reset();
+        while(frontExpAvg.average() < .8 && wallRight() && wallLeft()) 
+        { 
+            frontExpAvg.add(SenseF.read());
+            
+            pid();
+               
         }
-        else if(PIDv > .1)
-        {
-            setRightSpeed(4); //** Just flipped these values
-            setLeftSpeed(3);
-            wait_ms(20);        
-        }
-        else
-        {
-            setRightSpeed(3);
-            setLeftSpeed(3);
-        } 
-        //check_walls(); 
-        //WIRELESS.printf("ERR: %f  Front: %f\n\r", PIDv, SenseF.read());
-        //WIRELESS.printf("R: %f  L: %f\n\r", cal_R, cal_L);   
-    }
-    
-    //setRightSpeed(-2);
-    //setLeftSpeed(-2);
-    
-    //stop_mov();
-    wait_ms(20);
-    WIRELESS.printf("Time: %f\n\r", t.read());
-    handbrake();
-    wait_ms(20);  
-    printMap(WIRELESS); 
+        
+        float time = t.read();
+        handbrake();
+        wait_ms(200);
+        //updateMaze();
+        //WIRELESS.printf("Time: %f Cells: %i\n\r", time, cells_traveled);
+        //printMap(WIRELESS);
+        //if (frontExpAvg.average() < .8 && !wallRight() && wallLeft())
+            turn_right(3);
+        //if (frontExpAvg.average() < .8 && wallRight() && !wallLeft())
+        //    turn_left(3);
+       // }
+     
+     
     return 0;
 }
-void moveNORTH()
-{
-    switch(orientation)
-    {
-        case NORTH:
-            // go forward...
-            break;
-        case WEST:
-            // turn right then forward
-            break;            
-        case SOUTH:
-            // turn around then forward
-            break;            
-        case EAST:
-            // turn left then forward
-            break;            
-    }
-}
-void moveSOUTH()
-{
-    switch(orientation)
-    {
-        case NORTH:
-            // turn around then forward...
-            break;
-        case WEST:
-            // turn left then forward
-            break;            
-        case SOUTH:
-            // go forward
-            break;            
-        case EAST:
-            // turn right then forward
-            break;            
-    }
-}
-void moveEAST()
-{
-    switch(orientation)
-    {
-        case NORTH:
-            // turn right then forward...
-            break;
-        case WEST:
-            // turn around then foward
-            break;            
-        case SOUTH:
-            // turn left then forward
-            break;            
-        case EAST:
-            // go forward
-            break;            
-    }
-}
-void moveWEST()
-{
-    switch(orientation)
-    {
-        case NORTH:
-            // turn left then forward...
-            break;
-        case WEST:
-            // go forward
-            break;            
-        case SOUTH:
-            // turn left then forward
-            break;            
-        case EAST:
-            // turn around then forward
-            break;            
-    }
-}
\ No newline at end of file
+