Diff: main.cpp

Revision:

5:d2e955a94940

Parent:

4:05484073a641

--- a/main.cpp	Sun Mar 22 06:34:30 2015 +0000
+++ b/main.cpp	Wed Mar 25 09:58:50 2015 +0000
@@ -1,47 +1,10 @@
-/* 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
@@ -57,36 +20,8 @@
     }
 }
 
-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)
@@ -97,21 +32,13 @@
 }
 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
+    bt.baud(BT_BAUD_RATE);
+    test_dmp();
+    bt.baud(BT_BAUD_RATE);
     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();
     }
 }
@@ -124,34 +51,19 @@
     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 bt_shell_th(bt_shell, NULL, osPriorityNormal, 2048, NULL);
     
     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 IMU2_th(IMU2_thread,NULL,osPriorityNormal, 2048,NULL);
     Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); 
     
-    //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);
-    
     Thread::wait(osWaitForever);
 }
\ No newline at end of file