File content as of revision 0:c2ec30f28676:

#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;
}