Diff: communication.cpp

Revision:

3:4306d042af6f

Parent:

2:442902ec3aa1

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