File content as of revision 2:7b3822eacad8:

/* 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 *MotorStopTimer;
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 Lspeed, int Rspeed, int ms)
{   
    *motors.Left = Lspeed;
    *motors.Right = Rspeed;
    
    if(ms > 0)
        MotorStopTimer->start(ms); 
}

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)
{
    test_dmp2();
    start_dmp2(mpu2);
    while(1)
    {
        update_dmp2();
         Thread::yield();
    }
}
void IMU_thread(void const *args)
{
     bt.baud(BT_BAUD_RATE);   //you have to do this for some reason
    test_dmp(); 
 //test_dmp2();
    bt.baud(BT_BAUD_RATE);   //you have to do this for some reason
    start_dmp(mpu);
  // start_dmp2(mpu2);
    //calibrate_1();

    while(1) {
        /*if(calibrate_flag)
        {
            calibrate_1();
            calibrate_flag = 0;
        }*/
        
        //if(!mpuInterrupt && fifoCount < packetSize);    //interrupt not ready
        //else {  //mpu interrupt is ready
        //     update_dmp2();
            update_dmp();
         
            //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();
    
    MotorStopTimer = 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(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
    
//    Thread current_sense(CURRENT_thread,NULL,osPriorityNormal,300,NULL);  //this thread for monitoring current is unnecessary. Thread use lotsa rams. Kill it.
    
    //Thread optflow_th(optflow_thread, NULL, osPriorityNormal, 500, NULL); 
    
    //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);
    
    Thread::wait(osWaitForever);
}