Diff: Main.cpp

Revision:

0:c2ec30f28676

Child:

1:cfe6a6ad8dca

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Main.cpp	Sat Mar 29 03:06:46 2014 +0000
@@ -0,0 +1,166 @@
+#include "Core.h"
+
+#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)
+    {
+
+        
+        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;
+    
+}
+
+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);
+    wait_us(100);
+    ledR.write(0.33f); 
+    wait_us(100);
+    ledF.write(0.5f);
+
+    initMapping();
+            float tot = 0;
+        float p = 0;
+        float avg;
+    while(1)
+    {
+     /*   switch(WIRELESS.getChar())
+        {
+            case 't':
+                WIRELESS.printf("%f", initRight());
+                break; 
+            
+        }
+        
+        */
+        
+
+        
+                //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);
+           
+    }
+    
+    return 0;
+}
\ No newline at end of file