File content as of revision 4:05484073a641:

/* This is the current working code for the FYDP Autowalk
-only IMU1 works. I don't know why IMU2 works. I'll fix that later
*/

#include "robot.h"
#include "bt_shell.h"

RtosTimer *MotorCMDTimer;
RtosTimer *HalfHourTimer;

void HalfHourUpdate(void const *args)
{
        long_time += t.read_us();
        t.reset();
}

/*
RtosTimer *OpticalFlowTimer;

void update_opt(void const *args)
{
    reckon.updateOpticalFlow();   
}

void RF_mesh_thread(void const *args)   //this thread handles the mesh network
{
    while(1){
        Thread::wait(10);
    }
}

*/


void moveMotors(int i, int j, int k)
{   
    //This function does nothing. LEAVE BLANK
    //myservo = 1;
    Thread::yield();
}

void motor_t(void const *args)
{   

    double angle = 1;
    while(1){
        //move the servo so it reflects the IMU ROLL
        if (((t.read()- prev)*1000) > write_rate){
            angle = imu_data.ypr[1]/90;
            myservo = angle;  
            //angle = angle - 0.01;
            //if (angle < 0)
            //    angle = 1;
            prev = t.read();
        }
        Thread::wait(10);
    }
}

void stopMotors(void const *args)
{   
    *motors.Left = 0;
    *motors.Right = 0;    
    send_flag = 0;
}

void CURRENT_thread(void const *args)
{
    float current;
    while(1){
        current = current_sensor.get_current();
        bt.lock();
        bt.printf("\n\rCurrent: %f mA",current);
        bt.unlock();
        Thread::wait(50);
    }
}

void optflow_thread(void const *args)
{    
    while(true) {
        reckon.updateOpticalFlow();                   
        Thread::wait(3);
    }    
}

void IMU2_thread(void const *args)
{
    //This thread does not work
    test_dmp2();
    start_dmp2(mpu2);
    while(1)
    {
        update_dmp2();
         Thread::yield();
    }
}
void IMU_thread(void const *args)
{
    //For some reason, using 2 IMUs causes one of them to be laggy and 
    //the connection fails after a while (buffer overflow??)
    bt.baud(BT_BAUD_RATE);   //you have to do this for some reason
    test_dmp();     //test IMU1 
    //test_dmp2();     //test IMU2
    bt.baud(BT_BAUD_RATE);   //you have to do this for some reason
    start_dmp(mpu);
    // start_dmp2(mpu2);
    
    while(1) {
        update_dmp();
        //update_dmp2();
        //mpuInterrupt = false;   //this resets the interrupt flag
        //mpuInterrupt2 = false;
        //Thread::wait(10);
         Thread::yield();
    }
}

void bt_shell(void const *args)
{
    bt_shell_init();
    bt.printf("BT shell initialized\r\n");
    
    while(true) {
        bt_shell_run();
        Thread::wait(10);
        //Thread::yield();
    }    
}

int main()
{
    initRobot();
    
    //MotorCMDTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL);    
    //HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL);
    
    //HalfHourTimer->start(1800000);  //doesn't work because the timer takes over (high priority)
    
    //OpticalFlowTimer = new RtosTimer(&update_opt, osTimerPeriodic, NULL);
    //OpticalFlowTimer->start(10);  //doesn't work because the timer takes over (high priority)
        

    Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);    //if you get a hard fault, increase memory size of this thread (second last value)
    
    Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
    
    Thread IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL); //Putting IMU2 in a separate thread does not work
    
//    Thread current_sense(CURRENT_thread,NULL,osPriorityNormal,300,NULL);  //this thread for monitoring current is unnecessary. Thread use lotsa rams. Kill it.
    
    Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); 
    
    //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);
    
    Thread::wait(osWaitForever);
}