main.cpp

00001 #include "robot.h"
00002 #include "bt_shell.h"
00003 
00004 
00005 
00006 void motor_t(void const *args)
00007 {   
00008     double angle = 1;
00009     while(1){
00010         //move the servo so it reflects the IMU ROLL
00011         if (((t.read()- prev)*1000) > write_rate){
00012             angle = imu_data.ypr[1]/90;
00013             myservo = angle;  
00014             //angle = angle - 0.01;
00015             //if (angle < 0)
00016             //    angle = 1;
00017             prev = t.read();
00018         }
00019         Thread::wait(10);
00020     }
00021 }
00022 
00023 void IMU2_thread(void const *args)
00024 {
00025     test_dmp2();
00026     start_dmp2(mpu2);
00027     while(1)
00028     {
00029         update_dmp2();
00030          Thread::yield();
00031     }
00032 }
00033 void IMU_thread(void const *args)
00034 {
00035     bt.baud(BT_BAUD_RATE);
00036     test_dmp();
00037     bt.baud(BT_BAUD_RATE);
00038     start_dmp(mpu);
00039     
00040     while(1) {
00041         update_dmp();
00042          Thread::yield();
00043     }
00044 }
00045 
00046 void bt_shell(void const *args)
00047 {
00048     bt_shell_init();
00049     bt.printf("BT shell initialized\r\n");
00050     
00051     while(true) {
00052         bt_shell_run();
00053         Thread::wait(10);
00054     }    
00055 }
00056 
00057 int main()
00058 {
00059     initRobot();
00060 
00061     Thread bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);
00062     
00063     Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
00064     
00065     Thread IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL);
00066     Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); 
00067     
00068     Thread::wait(osWaitForever);
00069 }