Diff: main.cpp

Revision:

3:47461d37adfb

Parent:

2:7b3822eacad8

Child:

4:05484073a641

--- a/main.cpp	Sat Mar 21 22:44:36 2015 +0000
+++ b/main.cpp	Sun Mar 22 00:43:42 2015 +0000
@@ -5,7 +5,7 @@
 #include "robot.h"
 #include "bt_shell.h"
 
-RtosTimer *MotorStopTimer;
+RtosTimer *MotorCMDTimer;
 RtosTimer *HalfHourTimer;
 
 void HalfHourUpdate(void const *args)
@@ -31,13 +31,30 @@
 
 */
 
-void moveMotors(int Lspeed, int Rspeed, int ms)
+
+void moveMotors(int i, int j, int k)
+{   
+    //This function does nothing. LEAVE BLANK
+    //myservo = 1;
+    Thread::yield();
+}
+
+void motor_t(void const *args)
 {   
-    *motors.Left = Lspeed;
-    *motors.Right = Rspeed;
-    
-    if(ms > 0)
-        MotorStopTimer->start(ms); 
+
+    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)
@@ -66,9 +83,10 @@
         Thread::wait(3);
     }    
 }
-
+/*
 void IMU2_thread(void const *args)
 {
+    //This thread does not work
     test_dmp2();
     start_dmp2(mpu2);
     while(1)
@@ -76,32 +94,23 @@
         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();
+    //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);
-    //calibrate_1();
-
+    // start_dmp2(mpu2);
+    
     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;
-        //}
+        update_dmp();
+        //update_dmp2();
+        //mpuInterrupt = false;   //this resets the interrupt flag
+        //mpuInterrupt2 = false;
         //Thread::wait(10);
          Thread::yield();
     }
@@ -123,10 +132,10 @@
 {
     initRobot();
     
-    MotorStopTimer = new RtosTimer(&stopMotors, osTimerOnce, NULL);    
-    HalfHourTimer = new RtosTimer(&HalfHourUpdate, osTimerPeriodic, NULL);
+    //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)
+    //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)
@@ -136,11 +145,11 @@
     
     Thread IMU_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
     
-   // Thread IMU2_th(IMU_thread,NULL,osPriorityNormal, 2048,NULL);
+   // Thread IMU2_th(IMU_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 optflow_th(optflow_thread, NULL, osPriorityNormal, 500, NULL); 
+    Thread motor_th(motor_t, NULL, osPriorityNormal, 2048, NULL); 
     
     //Thread RF_mesh_th(RF_mesh_thread,NULL,osPriorityNormal,500,NULL);