Revision 3:4306d042af6f, committed 2014-07-03

Comitter:

TonyYI

Date:

Thu Jul 03 14:52:44 2014 +0000

Parent:

2:442902ec3aa1

Commit message:

123123

Changed in this revision

diff -r 442902ec3aa1 -r 4306d042af6f communication.cpp
--- a/communication.cpp	Thu Jul 03 10:59:37 2014 +0000
+++ b/communication.cpp	Thu Jul 03 14:52:44 2014 +0000
@@ -27,12 +27,12 @@
 **********************************************************/
 #include <communication.h>
 
-Communication::Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino)
+Communication::Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino, COMPASS* _compass)
 {
     this->_DEBUG = _DEBUG;
     this->_IntelToMbed = _IntelToMbed;
     this->_MbedToArduino = _MbedToArduino;
-    this->compass=new COMPASS(COMPASS_TX,COMPASS_RX);
+    this->_compass = _compass;
     init();
 }
 
@@ -40,15 +40,20 @@
 {
     delete[] buffer_IntelToMbed;
     delete[] buffer_MbedToArduino;
+    delete[] forward_msg_buffer;
     delete _DEBUG;
     delete _IntelToMbed;
     delete _MbedToArduino;
+    delete _compass;
 }
 
 void Communication::init()
 {
     buffer_IntelToMbed = new uint8_t[BUFFER_SIZE];
     buffer_MbedToArduino = new uint8_t[BUFFER_SIZE];
+    
+    forward_msg_buffer = new uint8_t[9]; //the message struct is 9 byte
+    
     in_IntelToMbed = 0;
     out_IntelToMbed = 0;
     in_MbedToArduino = 0;
@@ -120,7 +125,7 @@
         buffer_IntelToMbed[in_IntelToMbed++] = _x;
         if(in_IntelToMbed == BUFFER_SIZE)
         {
-            in_IntelToMbed &= 0x00;
+            in_IntelToMbed &= 0x0000;
         }
     }
     else if(communication_type == 1)
@@ -128,7 +133,7 @@
         buffer_MbedToArduino[in_MbedToArduino++] = _x;
         if(in_MbedToArduino == BUFFER_SIZE)
         {
-            in_MbedToArduino &= 0x00;
+            in_MbedToArduino &= 0x0000;
         }
     }
 }
@@ -151,6 +156,7 @@
                 if(_x == STARTER || _x == COMPASS_STARTER)
                 {
                     state_IntelToMbed++;
+                    forward_msg_buffer[0] = _x;
                 }
                 else
                 {
@@ -174,6 +180,7 @@
                 if(action_type == 0 || action_type == 1 || action_type == 2 || action_type == 3 || action_type == 4)
                 {
                     state_IntelToMbed++;
+                    forward_msg_buffer[1] = _x;
                 }
                 else
                 {
@@ -195,6 +202,7 @@
                 check_sum += _x;
                 move_dis = _x << 8;
                 state_IntelToMbed++;
+                forward_msg_buffer[2] = _x;
                 break;
             }
 
@@ -207,6 +215,7 @@
                 check_sum += _x;
                 move_dis |=  _x;
                 state_IntelToMbed++;
+                forward_msg_buffer[3] = _x;
                 break;
             }
 
@@ -221,6 +230,7 @@
                 if((action_type == 0 && (move_dir == 0 || move_dir == 1 || move_dir == 2 || move_dir == 3)) || (action_type == 1 && (move_dir == 0 || move_dir == 2)) || action_type == 2 || action_type == 3 || action_type == 4)
                 {
                     state_IntelToMbed++;
+                    forward_msg_buffer[4] = _x;
                 }
                 else
                 {
@@ -242,6 +252,7 @@
                 check_sum += _x;
                 rotate_dis = _x << 8;
                 state_IntelToMbed++;
+                forward_msg_buffer[5] = _x;
                 break;
             }
 
@@ -254,6 +265,7 @@
                 check_sum += _x;
                 rotate_dis |= _x;
                 state_IntelToMbed++;
+                forward_msg_buffer[6] = _x;
                 break;
             }
 
@@ -268,6 +280,7 @@
                 if(action_type == 3 || action_type == 4 || (action_type == 1 && ((rotate_dir >> 6) == 0)) || ((action_type == 0 || action_type == 2) && ((rotate_dir >> 6) == 3)))
                 {
                     state_IntelToMbed++;
+                    forward_msg_buffer[7] = _x;
                 }
                 else
                 {
@@ -288,6 +301,7 @@
                 }
                 if(check_sum == _x)
                 {
+                    forward_msg_buffer[8] = _x;
                     switch(action_type)
                     {
                         case 0: //car movement
@@ -338,22 +352,27 @@
 void Communication::forwardMessage()
 {
     //message structure is defined in source/motion_platform/intel_board/lib/message.h
-    uint8_t i = out_IntelToMbed - 9; //message size is 9 bytes
-    putByte(buffer_IntelToMbed[i++], 2); //starter, 2 means MbedToArduino
-    putByte(buffer_IntelToMbed[i++], 2); //action_type
-    putByte(buffer_IntelToMbed[i++], 2); //move_dis
-    putByte(buffer_IntelToMbed[i++], 2);
-    putByte(buffer_IntelToMbed[i++], 2); //move_dir
-    putByte(buffer_IntelToMbed[i++], 2); //rotate_dis
-    putByte(buffer_IntelToMbed[i++], 2);
-    putByte(buffer_IntelToMbed[i++], 2); //rotate_dir
-    putByte(buffer_IntelToMbed[i++], 2); //checksum
+    putByte(forward_msg_buffer[0], 2); //starter, 2 means MbedToArduino
+    putByte(forward_msg_buffer[1], 2); //action_type
+    putByte(forward_msg_buffer[2], 2); //move_dis
+    putByte(forward_msg_buffer[3], 2);
+    putByte(forward_msg_buffer[4], 2); //move_dir
+    putByte(forward_msg_buffer[5], 2); //rotate_dis
+    putByte(forward_msg_buffer[6], 2);
+    putByte(forward_msg_buffer[7], 2); //rotate_dir
+    putByte(forward_msg_buffer[8], 2); //checksum
 }
 
 void Communication::ACK(Lifter* lifter, Camera_platform* camera_platform)
 {
     if(action_type == 0) //car movement
     {
+        for(int i = 0; i < 9; i++)
+        {
+            uint8_t _y = getByte(1);
+            printf("Communication::ACK(). Get byte: %x\r\n", _y);
+        }
+        
         while(info_ok_MbedToArduino != 1)
         {
             if(in_MbedToArduino != out_MbedToArduino)
@@ -363,13 +382,21 @@
                 {
                     case 0: //checking starter
                     {
-                        //putByte('0', 1);
+                        if(DEBUG_ON)
+                        {
+                            _DEBUG->printf("Communication::ACK(). Checking SARTER...\r\n");
+                        }
+                        
                         if(_x == STARTER)
                         {
                             state_MbedToArduino++;
                         }
                         else
                         {
+                            if(DEBUG_ON)
+                            {
+                                _DEBUG->printf("Communication::ACK(). ERROR when checking SARTER: %x\r\n", _x);
+                            }
                             state_MbedToArduino = 0;
                         }
                         break;
@@ -377,13 +404,21 @@
 
                     case 1: //checking 'O'
                     {
-                        //putByte('1', 1);
+                        if(DEBUG_ON)
+                        {
+                            _DEBUG->printf("Communication::ACK(). Checking O...\r\n");
+                        }
+                        
                         if(_x == 0x4f) //O
                         {
                             state_MbedToArduino++;
                         }
                         else
                         {
+                            if(DEBUG_ON)
+                            {
+                                _DEBUG->printf("Communication::ACK(). ERROR when checking O: %x\r\n", _x);
+                            }
                             state_MbedToArduino = 0;
                         }
                         break;
@@ -391,13 +426,21 @@
 
                     case 2: //checking 'K'
                     {
-                        //putByte('2', 1);
+                        if(DEBUG_ON)
+                        {
+                            _DEBUG->printf("Communication::ACK(). Checking K...\r\n");
+                        }
+                        
                         if(_x == 0x4b) //K
                         {
                             state_MbedToArduino++;
                         }
                         else
                         {
+                            if(DEBUG_ON)
+                            {
+                                _DEBUG->printf("Communication::ACK(). ERROR when checking K: %x\r\n", _x);
+                            }
                             state_MbedToArduino = 0;
                         }
                         break;
@@ -405,11 +448,22 @@
 
                     case 3: //checking check_sum_MbedToArduino
                     {
-                        //putByte('3', 1);
+                        if(DEBUG_ON)
+                        {
+                            _DEBUG->printf("Communication::ACK(). Checking CHECK_SUM...\r\n");
+                        }
+                        
                         if(_x == 0x9a) //checksum
                         {
                             info_ok_MbedToArduino = 1;
                         }
+                        else
+                        {
+                            if(DEBUG_ON)
+                            {
+                                _DEBUG->printf("Communication::ACK(). ERROR when checking CHECK_SUM: %x\r\n", _x);
+                            }
+                        }
                         
                         state_MbedToArduino = 0;
                         break;

diff -r 442902ec3aa1 -r 4306d042af6f communication.h
--- a/communication.h	Thu Jul 03 10:59:37 2014 +0000
+++ b/communication.h	Thu Jul 03 14:52:44 2014 +0000
@@ -48,7 +48,7 @@
 class Communication
 {
 public:
-    Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino);
+    Communication(MySerial* _DEBUG, MySerial *_IntelToMbed, MySerial *_MbedToArduino, COMPASS *_compass);
     ~Communication();
 
     void putToBuffer(uint8_t _x, uint8_t communication_type); //0 is IntelToMbed, 1 is MbedTOArduino
@@ -74,6 +74,7 @@
     void init();
     uint8_t* buffer_IntelToMbed;
     uint8_t* buffer_MbedToArduino;
+    uint8_t* forward_msg_buffer; //for forwarding message to the car
     uint16_t in_IntelToMbed;
     uint16_t out_IntelToMbed;
     uint16_t in_MbedToArduino;
@@ -93,6 +94,7 @@
     MySerial *_DEBUG;
     MySerial *_IntelToMbed;
     MySerial *_MbedToArduino;
+    COMPASS *_compass;
 };
 
 #endif
\ No newline at end of file

diff -r 442902ec3aa1 -r 4306d042af6f compass.cpp
--- a/compass.cpp	Thu Jul 03 10:59:37 2014 +0000
+++ b/compass.cpp	Thu Jul 03 14:52:44 2014 +0000
@@ -1,28 +1,126 @@
 #include "compass.h"
+/*
+Serial pc(USBTX, USBRX);
+
 
-COMPASS::COMPASS(PinName tx, PinName rx)
+*/
+
+COMPASS::COMPASS(MySerial* serial)
 {
-   this->serial= new MySerial(tx,rx);
+   this->_serial = serial;
+   
    serial->baud(56000);
    serial->format(8,SerialBase::None, 1);
+   
    init();
 }
 
-
 uint16_t COMPASS::read()
 {
+    printf("before resume  \r\n");
     resume();
-    buffer[0]=serial->getc();
-    buffer[1]=serial->getc();
-    stop();
-    degree=(uint8_t)(buffer[0])*256+(uint8_t)(buffer[1]);
-    return degree;
+    printf("after resume \r\n");
+
+    wait(0.5);
+         printf("before run \r\n");
+
+    run();
+             printf("after run \r\n");
+
+    printf("enter while buffer_count: %d \r\n",buffer_count);
+
+    
+    
+    printf("enter while 1 \r\n");
+    while(1)
+    {
+          if(buffer_count<=2)
+         {
+             buffer_count=0;
+             break;
+         }
+         
+         buffer_count--;
+         uint8_t tempc1=buffer[buffer_count];
+         buffer_count--;
+         uint8_t tempc2=buffer[buffer_count];
+         
+                 temp[0]= tempc1;
+                 temp[1]= tempc2;
+                 
+                 if( 0xa0==( (temp[0]) & 0xe0))
+                 { 
+                    flag=1;
+                    printf("match !!!!!\r\n");
+                 }
+             
+            
+            
+        
+        
+        if(flag==1)
+        {
+                      twobytes=(temp[0])*256+(uint8_t)(temp[1]);
+            
+                     digits= temp[1] & 0x0f;
+            
+                      tens= (temp[1]>>4) & 0x0f;
+            
+                      hundreds= temp[0] & 0x07;
+        
+                     _degree=100*hundreds+10*tens+digits;
+                      printf("buffer[0]: %x\r\n",temp[0]);
+                      printf("buffer[1]: %x\r\n",temp[1]);
+                     temp[0]=0;
+                     temp[1]=0;
+                     flag=0;
+           
+            buffer_count=0;
+            
+             
+             // printf("hundreds: %d\r\n",hundreds);
+//              printf("tens: %d\r\n",tens);
+//              printf("digits: %d\r\n",digits);
+          
+               resume();
+               
+               
+               _degree+=90;
+               
+               if(_degree>=360)
+               _degree-=360;
+              
+                   printf("degree: %d\r\n",_degree);
+              printf("----------------------\r\n");
+               return _degree;
+//            break;
+        }
+   
+    }
+     // printf("leave while 1 \r\n");
+//       wait(0.5);
+//       
+//     printf("before resume2 \r\n");
+//
+//     
+//           printf("after resume2 \r\n");
+
+           
+         
+    return 0;
 }
 
 void COMPASS::init() 
 {
-    run();
-    stop();
+    buffer[0]=0;
+    buffer[1]=0;
+    count=0;
+    flag=0;
+    buffer_count=0;
+    for(int i=0;i<_BUFFER_SIZE;i++)
+    {
+        buffer[i]=0;
+    }
 }
 
 void COMPASS::run() 
@@ -30,19 +128,16 @@
     write2Bytes(RUN_MSB,RUN_LSB);
 }
 
-
 void COMPASS::stop() 
 {
     write2Bytes(STOP_MSB,STOP_LSB);
 }
 
-
 void COMPASS::resume() 
 {
     write2Bytes(RESUME_MSB,RESUME_LSB);
 }
 
-
 void COMPASS::reset() 
 {
     write2Bytes(RST_MSB,RST_LSB);
@@ -50,9 +145,42 @@
 
 void COMPASS::write2Bytes(char msb, char lsb) 
 {
-    serial->putc(msb);
-    serial->putc(lsb);
-   
+    _serial->putc(lsb);
+    _serial->putc(msb);
+    
 }
 
- 
+void COMPASS::putToBuffer(uint8_t data) 
+{
+    if(buffer_count<_BUFFER_SIZE-10)
+    buffer_count++;
+    else
+     {
+     printf("Error full buffer \r\n");
+     buffer_count=0;
+     }
+     
+    buffer[buffer_count]=data;
+    
+}
+//
+//void COMPASS::check_time_out()
+//{
+//    if(flag == 1)
+//    {
+//        ON();
+//        wait(5);
+//        mbed_reset();
+//    }
+//    else
+//    {
+//        OFF();
+//    }
+//}
+//
+//void COMPASS::time_out_init()
+//{
+//    setFlag();
+//    time_out.detach();
+//    time_out.attach(this, &Buzzer::check_time_out, TIME_OUT);
+//}

diff -r 442902ec3aa1 -r 4306d042af6f compass.h
--- a/compass.h	Thu Jul 03 10:59:37 2014 +0000
+++ b/compass.h	Thu Jul 03 14:52:44 2014 +0000
@@ -22,7 +22,7 @@
 #define COMPASS_TX      p9
 #define COMPASS_RX      p10
 
-
+#define _BUFFER_SIZE 512
 
 #define DECLINATIONANGLE  -0.0457
 #define OFFSET 0
@@ -31,28 +31,35 @@
 
 class COMPASS 
 {
-
 public:
+    COMPASS(MySerial* serial);
+    uint16_t read();
+    void putToBuffer(uint8_t data);
 
-   COMPASS(PinName TX, PinName RX);
-    uint16_t read();
-    
     
 private:
-     
-    MySerial *serial;
-    char buffer[2];
-    uint16_t degree;
+    MySerial* _serial;
+    uint16_t _degree;
+    uint8_t flag;
+    uint8_t count;
+    float declinationAngle;
+    int offset;
+    uint16_t buffer_count;
+    char temp[2];
+    char buffer[_BUFFER_SIZE];
+    uint16_t twobytes;
+    uint8_t hundreds,tens,digits;
     
     void init();
+    void write2Bytes(char msb, char lsb);
+    
     void run();
     void stop();
     void resume();
     void reset();
-    void write2Bytes(char msb, char lsb);
     
-    float declinationAngle;
-    int offset;
+  //  void check_time_out();
+//    void time_out_init();
 };
 
 #endif /* COMPASS_H */
\ No newline at end of file

diff -r 442902ec3aa1 -r 4306d042af6f main.cpp
--- a/main.cpp	Thu Jul 03 10:59:37 2014 +0000
+++ b/main.cpp	Thu Jul 03 14:52:44 2014 +0000
@@ -40,34 +40,42 @@
         buzzer.time_out_init();
         if(com.getInfoOK(0) == 1) //car
         {
+            printf("main(). Car action starting...\r\n");
             com.forwardMessage();
             com.ACK(&lifter, &camera_platform);
             com.resetInfoOK(0);
             com.resetInfoOK(1);
+            printf("main(). Car action ended...\r\n");
         }
         else if(com.getInfoOK(0) == 2) //lifter
         {
+            printf("main(). Lifter action starting...\r\n");
             lifter.lifterMove(com.getMoveDis(), com.getMoveDir(), com.getRotateDis(), com.getRotateDir());
             com.ACK(&lifter, &camera_platform);
             com.resetInfoOK(0);
             com.resetInfoOK(1);
+            printf("main(). Lifter action ended...\r\n");
         }
         else if(com.getInfoOK(0) == 3) //camera_platform
         {
+            printf("main(). camera_platform action starting...\r\n");
             camera_platform.cameraPlatformMove(com.getMoveDis(), com.getMoveDir(), com.getRotateDis(), com.getRotateDir());
             com.ACK(&lifter, &camera_platform);
             com.resetInfoOK(0);
             com.resetInfoOK(1);
+            printf("main(). Camera_platform action ended...\r\n");
         }
         else if(com.getInfoOK(0) == 4) //compass
         {
+            printf("main(). Compass action starting...\r\n");
             com.ACK(&lifter, &camera_platform);
             com.resetInfoOK(0);
             com.resetInfoOK(1);
+            printf("main(). Compass action ended...\r\n");
         }
-        
-         else if(com.getInfoOK(0) == 5) //compass
+        else if(com.getInfoOK(0) == 5) //buzzer
         {
+            printf("main(). Buzzer action starting...\r\n");
             buzzer.ON();
             wait(0.1);
             buzzer.OFF();
@@ -78,6 +86,7 @@
             com.ACK(&lifter, &camera_platform);
             com.resetInfoOK(0);
             com.resetInfoOK(1);
+            printf("main(). Buzzer action ended...\r\n");
         }
         
         buzzer.cleanFlag();

diff -r 442902ec3aa1 -r 4306d042af6f port.h
--- a/port.h	Thu Jul 03 10:59:37 2014 +0000
+++ b/port.h	Thu Jul 03 14:52:44 2014 +0000
@@ -34,6 +34,7 @@
 MyDigitalOut IntelToMbed_LED(LED1); //uart port LED between Intel Board and Mbed
 MyDigitalOut MbedToArduino_LED(LED2); //uart port LED between Mbed and Arduino
 MySerial DEBUG(USBTX, USBRX); //usb serial port between computer and Mbed
+MySerial CompassData(p9, p10); ////uart port between compass and Mbed
 MySerial IntelToMbed(p13, p14); //uart port between Intel Board and Mbed
 MySerial MbedToArduino(p28, p27); //uart port between Mbed and Arduino
 MyPwmOut lifter_pwmUp(p21);
@@ -48,8 +49,9 @@
 
 MyDigitalOut buzzer_pin(p29);
 Buzzer buzzer(&buzzer_pin);
+COMPASS compass(&CompassData);
 
-Communication com(&DEBUG, &IntelToMbed, &MbedToArduino);
+Communication com(&DEBUG, &IntelToMbed, &MbedToArduino, &compass);
 Lifter lifter(&lifter_enable, &lifter_pwmUp, &lifter_pwmDown, &lifter_encoder_A, &lifter_encoder_B);
 Camera_platform camera_platform(&camera_platform_pwmRoll, &camera_platform_pwmPitch, &camera_platform_pwmYaw);
 
@@ -67,6 +69,13 @@
     com.putToBuffer(_x, 1); //function inside Communication::
     //__enable_irq();
 }
+void compassHandler()
+{
+    //__disable_irq();//disable interupt when receiving data from XBEE_UART
+    uint8_t _x = CompassData.getc();
+    compass.putToBuffer(_x); //function inside Communication::
+    //__enable_irq();
+}
 
 void LifterPulseHandler()
 {
@@ -100,6 +109,9 @@
     MbedToArduino.baud(9600);
     MbedToArduino.attach(&MbedToArduinoRxHandler); //serial interrupt function
     
+    CompassData.baud(56000);
+    CompassData.attach(&compassHandler);
+    
     lifter_encoder_A.fall(&LifterPulseHandler); //interrupt
     
     camera_platform_pwmRoll.period_ms(20); //20ms periodic, 1000us to 2000us