Revision 5:d2e955a94940, committed 2015-03-25

Comitter:

tntmarket

Date:

Wed Mar 25 09:58:50 2015 +0000

Parent:

4:05484073a641

Commit message:

only imu output

Changed in this revision

--- a/bt_shell/machine_interface/ros_machine_interface.h	Sun Mar 22 06:34:30 2015 +0000
+++ b/bt_shell/machine_interface/ros_machine_interface.h	Wed Mar 25 09:58:50 2015 +0000
@@ -168,7 +168,7 @@
                 }
                 fwd_flag = 1;
             }
-            moveMotors(arg_in[0]-m_offset,arg_in[1]-m_offset,arg_in[2]);
+//            moveMotors(arg_in[0]-m_offset,arg_in[1]-m_offset,arg_in[2]);
             bt.lock();
             bt.printf("L=%d;R=%d;t=%d",arg_in[0],arg_in[1],arg_in[2]); //don't need to respond. Maybe have a confirmation?
             bt.unlock();

--- a/bt_shell/shell/motor_fnt.h	Sun Mar 22 06:34:30 2015 +0000
+++ b/bt_shell/shell/motor_fnt.h	Wed Mar 25 09:58:50 2015 +0000
@@ -45,8 +45,6 @@
                     bt.printf("moving duration: %sms\r\n", argv[3]);
                     bt.unlock();
                 }
-
-                //moveMotors(param[0],param[1],param[2]);
             }
             else
             {

--- a/bt_shell/shell/remotecontrol_fnt.h	Sun Mar 22 06:34:30 2015 +0000
+++ b/bt_shell/shell/remotecontrol_fnt.h	Wed Mar 25 09:58:50 2015 +0000
@@ -1,7 +1,7 @@
 void remotecontrol_fnt(int argc, char **argv)
 {
     int speed = 40;
-    
+
     bt.lock();
     bt.printf("Remote Control Mode\r\n");
     bt.printf("Controls:\r\n");
@@ -9,95 +9,90 @@
     bt.printf("     r/f for speed +/-\r\n");
     bt.printf("     enter key to exit\r\n");
     bt.unlock();
-    
-    if(argc == 2){
-        speed = tinysh_atoxi(argv[1]);        
+
+    if(argc == 2) {
+        speed = tinysh_atoxi(argv[1]);
         if( speed <= 0 || speed > 100)
             speed = 100;
-    }        
-    
+    }
+
     int Lspeed = 0;
     int Rspeed = 0;
 
     char c = 0;
-    
-    while(c != '\r' && c != '\n')
-    {
+
+    while(c != '\r' && c != '\n') {
         bt.lock();
-        
-        if(bt.readable())
-        {
-        c = bt.getc();
-        bt.unlock();
-               
-        switch ( c ) {
-            case 'w':
-                Lspeed = speed;
-                Rspeed = speed;            
-            break;
-            case 'a':
-                Lspeed = -speed;
-                Rspeed = speed;
-            break;
-            case 's':
-                Lspeed = -speed;
-                Rspeed = -speed;
-            break;
-            case 'x':
-                Lspeed = -speed;
-                Rspeed = -speed;
-            break;
-            case 'd':
-                Lspeed = speed;
-                Rspeed = -speed;
-            break;
-            case 'q':
-                Lspeed = 0;
-                Rspeed = speed;
-            break;
-            case 'e':
-                Lspeed = speed;
-                Rspeed = 0;
-            break;      
-            case 'c':
-                Lspeed = -speed;
-                Rspeed = 0;
-            break;       
-            case 'z':
-                Lspeed = 0;
-                Rspeed = -speed;
-            break;        
-            case 'r':
-                speed += 10;
-                if(speed > 100)
-                    speed = 100;
-                bt.lock();
-                bt.printf("speed = %d\r\n", speed);
-                bt.unlock();
-                Lspeed = 0;
-                Rspeed = 0;
-            break;  
-            case 'f':
-                speed -= 10;
-                if(speed < 0)
-                    speed = 0;
-                bt.lock();
-                bt.printf("speed = %d\r\n", speed);      
-                bt.unlock();
-                Lspeed = 0;
-                Rspeed = 0;              
-            break;         
-            default:
-                Lspeed = 0;
-                Rspeed = 0;
-            break;
-        }
-        
-        moveMotors(Lspeed,Rspeed,100);
-        
-        }else
+
+        if(bt.readable()) {
+            c = bt.getc();
             bt.unlock();
-            
-        Thread::wait(25);   
+
+            switch ( c ) {
+                case 'w':
+                    Lspeed = speed;
+                    Rspeed = speed;
+                    break;
+                case 'a':
+                    Lspeed = -speed;
+                    Rspeed = speed;
+                    break;
+                case 's':
+                    Lspeed = -speed;
+                    Rspeed = -speed;
+                    break;
+                case 'x':
+                    Lspeed = -speed;
+                    Rspeed = -speed;
+                    break;
+                case 'd':
+                    Lspeed = speed;
+                    Rspeed = -speed;
+                    break;
+                case 'q':
+                    Lspeed = 0;
+                    Rspeed = speed;
+                    break;
+                case 'e':
+                    Lspeed = speed;
+                    Rspeed = 0;
+                    break;
+                case 'c':
+                    Lspeed = -speed;
+                    Rspeed = 0;
+                    break;
+                case 'z':
+                    Lspeed = 0;
+                    Rspeed = -speed;
+                    break;
+                case 'r':
+                    speed += 10;
+                    if(speed > 100)
+                        speed = 100;
+                    bt.lock();
+                    bt.printf("speed = %d\r\n", speed);
+                    bt.unlock();
+                    Lspeed = 0;
+                    Rspeed = 0;
+                    break;
+                case 'f':
+                    speed -= 10;
+                    if(speed < 0)
+                        speed = 0;
+                    bt.lock();
+                    bt.printf("speed = %d\r\n", speed);
+                    bt.unlock();
+                    Lspeed = 0;
+                    Rspeed = 0;
+                    break;
+                default:
+                    Lspeed = 0;
+                    Rspeed = 0;
+                    break;
+            }
+        } else
+            bt.unlock();
+
+        Thread::wait(25);
     }
 }
\ No newline at end of file

--- 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

--- a/robot/robot.h	Sun Mar 22 06:34:30 2015 +0000
+++ b/robot/robot.h	Wed Mar 25 09:58:50 2015 +0000
@@ -122,6 +122,5 @@
 float getTime();
 
 void initRobot();
-void moveMotors(int Lspeed, int Rspeed, int ms);
 
 #endif
\ No newline at end of file