main.cpp

00001 /* This is the current working code for the FYDP Autowalk
00002 -only IMU1 works. I don't know why IMU2 works. I'll fix that later
00003 */
00004 
00005 #include "robot.h"
00006 #include "bt_shell.h"
00007 
00008 RtosTimer *MotorStopTimer;
00009 RtosTimer *HalfHourTimer;
00010 
00011 void HalfHourUpdate(void const *args)
00012 {
00013         long_time += t.read_us();
00014         t.reset();
00015 }
00016 
00017 /*
00018 RtosTimer *OpticalFlowTimer;
00019 
00020 void update_opt(void const *args)
00021 {
00022     reckon.updateOpticalFlow();   
00023 }
00024 
00025 void RF_mesh_thread(void const *args)   //this thread handles the mesh network
00026 {
00027     while(1){
00028         Thread::wait(10);
00029     }
00030 }
00031 
00032 */
00033 
00034 void moveMotors(int Lspeed, int Rspeed, int ms)
00035 {   
00036     *motors.Left = Lspeed;
00037     *motors.Right = Rspeed;
00038     
00039     if(ms > 0)
00040         MotorStopTimer->start(ms); 
00041 }
00042 
00043 void stopMotors(void const *args)
00044 {   
00045     *motors.Left = 0;
00046     *motors.Right = 0;    
00047     send_flag = 0;
00048 }
00049 
00050 void CURRENT_thread(void const *args)
00051 {
00052     float current;
00053     while(1){
00054         current = current_sensor.get_current();
00055         bt.lock();
00056         bt.printf("\n\rCurrent: %f mA",current);
00057         bt.unlock();
00058         Thread::wait(50);
00059     }
00060 }
00061 
00062 void optflow_thread(void const *args)
00063 {    
00064     while(true) {
00065         reckon.updateOpticalFlow();                   
00066         Thread::wait(3);
00067     }    
00068 }
00069 
00070 void IMU_thread(void const *args)
00071 {
00072      bt.baud(BT_BAUD_RATE);   //you have to do this for some reason
00073     //test_dmp(); 
00074     test_dmp2();
00075     bt.baud(BT_BAUD_RATE);   //you have to do this for some reason
00076     //start_dmp(mpu);
00077     start_dmp2(mpu2);
00078     //calibrate_1();
00079 
00080     while(1) {
00081         /*if(calibrate_flag)
00082         {
00083             calibrate_1();
00084             calibrate_flag = 0;
00085         }*/
00086         
00087         //if(!mpuInterrupt && fifoCount < packetSize);    //interrupt not ready
00088         //else {  //mpu interrupt is ready
00089         //    update_dmp();
00090           update_dmp2();
00091             //mpuInterrupt = false;   //this resets the interrupt flag
00092             //mpuInterrupt2 = false;
00093         //}
00094         
00095          Thread::yield();
00096     }
00097 }
00098 
00099 void bt_shell(void const *args)
00100 {
00101     bt_shell_init();
00102     bt.printf("BT shell initialized\r\n");
00103     
00104     while(true) {
00105         bt_shell_run();
00106         Thread::yield();
00107     }    
00108 }
00109 
00110 int main()
00111 {
00112     initRobot();
00113     
00114     MotorStopTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL);    
00115     HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL);
00116     
00117     HalfHourTimer->start(1800000);  //doesn't work because the timer takes over (high priority)
00118     
00119     //OpticalFlowTimer = new RtosTimer(&update_opt, osTimerPeriodic, NULL);
00120     //OpticalFlowTimer->start(10);  //doesn't work because the timer takes over (high priority)
00121         
00122 
00123     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)
00124     
00125     Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
00126     
00127 //    Thread current_sense(CURRENT_thread,NULL,osPriorityNormal,300,NULL);  //this thread for monitoring current is unnecessary. Thread use lotsa rams. Kill it.
00128     
00129     //Thread optflow_th(optflow_thread, NULL, osPriorityNormal, 500, NULL); 
00130     
00131     //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);
00132     
00133     Thread::wait(osWaitForever);
00134 }