File content as of revision 1:cfe6a6ad8dca:

#include "Core.h"

#include <iostream>
using namespace std;

IMUfilter imuFilter(0.01, 0.10f);

Ticker ticker;

void test()
{
    collectSample();
}

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)
    {
        /*
        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; 
            
        }*/
        
       // 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("V: %f \tA: %f\n\r", new_value, accumulator);
           
           
        forward(3);
    }
    
    return 0;
}