Diff: Main.cpp

Revision:

2:997f57aee3b7

Parent:

1:cfe6a6ad8dca

Child:

3:1a8a7cc709cc

--- a/Main.cpp	Sat Mar 29 13:25:23 2014 +0000
+++ b/Main.cpp	Thu Apr 03 13:15:40 2014 +0000
@@ -1,84 +1,207 @@
-#include "Core.h"
-
-#include <iostream>
-using namespace std;
-
-IMUfilter imuFilter(0.01, 0.10f);
+//#include "Core.h"
 
-Ticker ticker;
+#include "Sensors.h"
+#include "Motors.h"
+#include "Communication.h"
+#include "Flooding.h"
 
-void test()
-{
-    collectSample();
-}
+#include "BufferAverage.h"
+#include "ExponentialAverage.h"
+//#include "WeightedAverage.h"
 
 int main()
 {
     Motor.baud(115200);
     ledF.period_us(26.2); //28.5 //27.9
-    wait_us(100);
-    //ledR.period_us(26.3); //28.5 // 26
-    wait_us(100);
-    //ledL.period_us(26.3); //28.5
-    ledF = 0.5;
-    wait_us(100);
-    ledR = 0.3;  //31 & 26.2
-    wait_us(100);
-    ledL = 0.651;  //48 without cap
-    Motor.baud(115200);
-    //initMapping();
-    //p.start();
-    ticker.attach(&test, 0.002);
-    while(1)
+    wait_us(600);
+    ledF.write(.6);
+    stop_mov();
+    
+    
+    wait(5);
+    /*
+    
+    
+    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)
     {
-        /*
-        switch(WIRELESS.getChar())
-        {
-            case 'r':
-                WIRELESS.printf("Rights");
-                turn_right(3);
-                break; 
-            case 'l':
-                WIRELESS.printf("Lefts");
-                turn_left(3);
-                break; 
-            case 'f':
-                WIRELESS.printf("Front");
-                forward(3);
-                break; 
-            case 't':
-                WIRELESS.printf("%f", imuFilter.getYaw());
-                break; 
-            
-        }*/
+        case M_NORTH:
+            moveNORTH();
+            break;
+     
+        case M_SOUTH:
+            moveSOUTH();
+            break;
+     
+        case M_WEST:
+            moveWEST();
+            break;
+        
+        case M_EAST:
+            moveEAST();
+            break;             
         
-       // stop();
+    }   
+    
+    i've written the code for the following:
+    
+    
+    moveNORTH()
+    moveSOUTH()
+    moveEAST()
+    moveWEST()
+    
+    they are at the bottom of the page. take a look and read them.
+    
+    
+    REMEMBER TO MOVE THEM TO THE TOP OF THE PAGE IF YOU PLAN TO USE THEM IN THE MAIN FUNCTION, OR VICEVERSA MOVE THE MAIN FUNC.
+    TO THE BOTTOM
+    
+    */
+
+    
+    for(int i = 0; i < 100; i++)
+    {
+        float l = SenseL.read();
+        float r = SenseR.read();
+        float f = SenseF.read();
         
         /*
-        compass.read(&ax, &ay, &az, &mx, &my, &mz);
-        gyro.read(&gx, &gy, &gz);
-        
-        curr = p.read();
+        leftWeightedAvg.add(l);
+        rightWeightedAvg.add(r);
+        frontWeightedAvg.add(f);
         
-        imuFilter.updateFilter(gx * (PI/18000), gy * (PI/18000), (gz + 0.63f) * (PI/18000), ax, ay, az, .01);
-        imuFilter.computeEuler();
 
-        last = curr;
+        leftExpAvg.add(l);
+        frontExpAvg.add(f);
+        */
+        rightExpAvg.add(r);
+        frontExpAvg.add(f);
+        //leftBufferAvg.add(l);
+        //rightBufferAvg.add(r);
+        //frontBufferAvg.add(f);
         
-        WIRELESS.printf("%f %f \n\r", last-curr, 1000.0f * imuFilter.getYaw());
-        wait_ms(10);
-        
-        //forward(5);
-        //return 0;
+        WIRELESS.printf("%i || F: %f \n\r", i, frontExpAvg.average());
+    }
+    
+    float avgRight = rightExpAvg.average();
+    
+    
+    setRightSpeed(3);
+    setLeftSpeed(3);
+    
+    
+    while (true)
+    //while(SenseL.read() < 0.99)
+    { 
+//        frontExpAvg.add(SenseF.read());
+    
+        //WIRELESS.printf("R: %f  L: %f  F: %f\n\r", SenseR.read(), SenseL.read(), SenseF.read());
         
-        */
-        
+        float PIDv = PID(SenseR.read(), SenseL.read());
+        //WIRELESS.printf("ERR: %f  Front: %f\n\r", PIDv, SenseF.read());
         
-        //WIRELESS.printf("V: %f \tA: %f\n\r", new_value, accumulator);
-           
-           
-        forward(3);
+        if(PIDv < -0.1)
+        {
+            setRightSpeed(3);
+            setLeftSpeed(4);  
+        }
+        else if(PIDv > .1)
+        {
+            setRightSpeed(4);
+            setLeftSpeed(3);        
+        }
+        else
+        {
+            setRightSpeed(3);
+            setLeftSpeed(3);
+        }     
     }
     
+    //setRightSpeed(-2);
+    //setLeftSpeed(-2);
+    
+    //stop_mov();
+    
+    stop_mov();   
     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