terry LAI
/
ESDC2014-pwm
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: communication.cpp
- Revision:
- 3:4306d042af6f
- Parent:
- 2:442902ec3aa1
- Child:
- 4:a377ecb9364f
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;