Diff: Main.cpp

Revision:

1:cfe6a6ad8dca

Parent:

0:c2ec30f28676

Child:

2:997f57aee3b7

--- a/Main.cpp	Sat Mar 29 03:06:46 2014 +0000
+++ b/Main.cpp	Sat Mar 29 13:25:23 2014 +0000
@@ -3,163 +3,81 @@
 #include <iostream>
 using namespace std;
 
-
-PwmOut ledF(p23);
-PwmOut ledR(p21);
-PwmOut ledL(p22);
-AnalogIn SenseR(p15);
-AnalogIn SenseL(p16);
-AnalogIn SenseF(p17);
 IMUfilter imuFilter(0.01, 0.10f);
 
-Timer p;
-    float value = 0;
-/*
-float initRight()
-{
-    
-    int best = 0;
-    for(int i = 0; i < 1000 && !done; i++)
-    {
-        wait_us(100);
-        ledR = ((float)i)/1000.0f;
-        WIRELESS.printf("\t%i\n\r", i);
-        float tot = 0;
-        for(int j = 0; j < 10; j++)
-        {
-            tot+= SenseR.read();
-        }
-
-        if(tot / 10.0f > 0.9f)
-        {
-            tot = 0;
-            for(int k = 0; k < 1000; k++)
-            {
-                tot+= SenseR.read();
-            }
-            
-            if(tot / 1000 > 0.9f)
-            {
-                done = true;
-                best  = i;
-            }
-        }
-        
-    } 
-    
-    WIRELESS.printf("BEST: %i \n\r", best);  
-       
-    return 0.0f;
-}
-*/
-
-
-float modulate()
-{
-    float tot = 0.0f;
-    for(float i = .1; i < 1.0f; i+= 0.05f)
-    {
+Ticker ticker;
 
-        
-        tot = 0.0f;
-        wait_us(2000);
-        ledR.write(i);
-        wait_us(2000);
-        
-        for(int j = 0; j < 50; j++)
-        {
-            tot+= SenseR.read();
-        }  
-        
-        WIRELESS.printf("%f\t%f\n\r", i, (tot / 50.0f));
-        
-        if((tot / 50.0f) >= 0.75f)
-        {
-            return i;
-        }
-        
-    }
-    
-    
-    return tot;
-    
+void test()
+{
+    collectSample();
 }
 
-float oss()
-{
-    ledF.write(0.0f);
-    wait_ms(1);
-    ledF = 0.5;
-    p.start();
-
-    float time = 0.0f;
-    while(p.read_ms() < 10)
-    {
-        wait_ms(1);
-        if(time == 0.0f && SenseF.read() < .9f)
-        {
-            time = p.read_ms();
-            value = SenseF.read();
-            ledF.write(0.0f);
-        }
-    }
-    
-    p.stop();
-    p.reset();
-    
-    
-    return 1.0f - (time/10.0f);
-}
-
-
 int main()
 {
-    
     Motor.baud(115200);
-    ledF.period_us(26.3); //28.5
-    ledR.period_us(26.3); //28.5
-    ledL.period_us(26.3); //28.5
-    WIRELESS.printf("STARTING");
-    ledL.write(0.33f);
+    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.write(0.33f); 
+    ledR = 0.3;  //31 & 26.2
     wait_us(100);
-    ledF.write(0.5f);
-
-    initMapping();
-            float tot = 0;
-        float p = 0;
-        float avg;
+    ledL = 0.651;  //48 without cap
+    Motor.baud(115200);
+    //initMapping();
+    //p.start();
+    ticker.attach(&test, 0.002);
     while(1)
     {
-     /*   switch(WIRELESS.getChar())
+        /*
+        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", initRight());
+                WIRELESS.printf("%f", imuFilter.getYaw());
                 break; 
             
-        }
+        }*/
+        
+       // stop();
+        
+        /*
+        compass.read(&ax, &ay, &az, &mx, &my, &mz);
+        gyro.read(&gx, &gy, &gz);
+        
+        curr = p.read();
+        
+        imuFilter.updateFilter(gx * (PI/18000), gy * (PI/18000), (gz + 0.63f) * (PI/18000), ax, ay, az, .01);
+        imuFilter.computeEuler();
+
+        last = curr;
+        
+        WIRELESS.printf("%f %f \n\r", last-curr, 1000.0f * imuFilter.getYaw());
+        wait_ms(10);
+        
+        //forward(5);
+        //return 0;
         
         */
         
-
         
-                //WIRELESS.printf("%f::\n\r", i);
-        compass.read(&ax, &ay, &az, &mx, &my, &mz);
-        gyro.read(&gx, &gy, &gz);
-        //double Gx = gx; double Gy = gy; double Gz = gz;
-        //double Ax = ax; double Ay = ay; double Az = az;
-        imuFilter.updateFilter(gx * (PI/18000), gy * (PI/18000), (gz + 0.66f) * (PI/18000), ax, ay, az);
-        imuFilter.computeEuler();
-        
-        
-        
-        tot += gz;
-        p+= 1.0f;
-       avg = tot/p;
-        WIRELESS.printf("RAW: %f     AVG: %f      NOISE?: %f\n\r", gz, avg, 1000.0f * imuFilter.getYaw());
-        wait_ms(10);
+        //WIRELESS.printf("V: %f \tA: %f\n\r", new_value, accumulator);
            
+           
+        forward(3);
     }
     
     return 0;