communication.cpp

00001 
00002 #include "mbed.h"
00003 #include "communication.h"
00004 //#include "protocol.h"
00005 //#include "Utilities.h"
00006 //#include "iSerial.h"
00007 
00008 
00009 uint8_t COMMUNICATION::sendCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet)
00010 {
00011     uint8_t currentParameter = 0;
00012     bool isWholePacket = false;
00013     uint8_t encoderState = WAIT_ON_HEADER_0;
00014 
00015     packet->checkSum = Utilities::GetCheckSum((uint8_t*)&(packet->robotId), packet->length + 1);
00016 
00017     Timer timer;
00018     timer.start();
00019 
00020     while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) {
00021         
00022         if( serialCom->writeable()) {
00023 
00024             switch(encoderState) {
00025                 case WAIT_ON_HEADER_0: {
00026 #ifdef ANDANTE_DEBUG
00027                     pc->printf("Write: 0x%02X ", ANDANTE_PROTOCOL_HEADER_0);
00028 #endif               
00029 
00030                     serialCom->putc(ANDANTE_PROTOCOL_HEADER_0);
00031                     //   dirc_rs485=0;
00032                     //    wait_us(DELAY_DIR);
00033 
00034 
00035                     encoderState = WAIT_ON_HEADER_1;
00036 
00037                     break;
00038                 }
00039                 case WAIT_ON_HEADER_1: {
00040 #ifdef ANDANTE_DEBUG
00041                     pc->printf("0x%02X ", ANDANTE_PROTOCOL_HEADER_1);
00042 #endif
00043 
00044             
00045 
00046                     serialCom->putc(ANDANTE_PROTOCOL_HEADER_1);
00047                     //      dirc_rs485=0;
00048                     //      wait_us(DELAY_DIR);
00049 
00050                     encoderState = WAIT_ON_ROBOT_ID;
00051 
00052                     break;
00053                 }
00054                 case WAIT_ON_ROBOT_ID: {
00055 #ifdef ANDANTE_DEBUG
00056                     pc->printf("0x%02X ", packet->robotId);
00057 #endif
00058 
00059 
00060                     serialCom->putc(packet->robotId);
00061                  
00062 
00063                     encoderState = WAIT_ON_LENGTH;
00064 
00065                     break;
00066                 }
00067                 case WAIT_ON_LENGTH: {
00068 #ifdef ANDANTE_DEBUG
00069                     pc->printf("0x%02X ", packet->length);
00070 #endif
00071                 
00072                     serialCom->putc(packet->length);
00073                   
00074 
00075                     encoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
00076 
00077                     break;
00078                 }
00079                 case WAIT_ON_INSTRUCTION_ERROR_ID: {
00080 #ifdef ANDANTE_DEBUG
00081                     pc->printf("0x%02X ", packet->instructionErrorId);
00082 #endif
00083                
00084 
00085                     serialCom->putc(packet->instructionErrorId);
00086 
00087                     if(packet->length > 2)
00088                         encoderState = WAIT_ON_PARAMETER;
00089                     else
00090                         encoderState = WAIT_ON_CHECK_SUM;
00091 
00092                     break;
00093                 }
00094                 case WAIT_ON_PARAMETER: {
00095 #ifdef ANDANTE_DEBUG
00096                     pc->printf("0x%02X ", packet->parameter[currentParameter]);
00097 #endif
00098 
00099                     serialCom->putc(packet->parameter[currentParameter]);
00100 
00101                     if(++currentParameter == packet->length - 2)
00102                         encoderState = WAIT_ON_CHECK_SUM;
00103 
00104                     break;
00105                 }
00106                 case WAIT_ON_CHECK_SUM: {
00107 #ifdef ANDANTE_DEBUG
00108                     pc->printf("0x%02X\r\n", packet->checkSum);
00109 #endif
00110 
00111 
00112                     serialCom->putc(packet->checkSum);
00113 
00114 
00115                     encoderState = WAIT_ON_HEADER_0;
00116                     isWholePacket = true;
00117 
00118                     break;
00119                 }
00120             }
00121         }
00122     }
00123 
00124 
00125 #ifdef ANDANTE_DEBUG
00126     pc->printf("Timer: %d ms\r\n", timer.read_ms());
00127 #endif
00128 
00129     timer.stop();
00130 
00131 
00132 
00133     if(!isWholePacket) {
00134 #ifdef ANDANTE_DEBUG
00135         pc->printf("Error: Write response timeout.\r\n");
00136 #endif
00137 
00138         return ANDANTE_ERRBIT_WRITE_TIMEOUT;
00139     }
00140 /*
00141     if( packet->servoId == ANDANTE_PROTOCOL_BROADCAST_ID ||
00142             packet->instructionErrorId == ANDANTE_ACTION ||
00143             packet->instructionErrorId == ANDANTE_SYNC_WRITE)
00144 */
00145         return ANDANTE_ERRBIT_NONE;
00146         
00147 }
00148 
00149 uint8_t COMMUNICATION::receiveCommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet)
00150 {
00151     uint8_t currentParameter = 0;
00152     bool isWholePacket = false;
00153 
00154     currentParameter = 0;
00155     isWholePacket = false;
00156     uint8_t decoderState = WAIT_ON_HEADER_0;
00157 
00158     Timer timer;
00159     timer.start();
00160 
00161     while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) {
00162         if(serialCom->readable()) {
00163             switch(decoderState) {
00164                 case WAIT_ON_HEADER_0: {
00165 
00166                     uint8_t mx28ProtocolHeader0 = serialCom->getc();
00167 
00168 #ifdef ANDANTE_DEBUG
00169                     pc->printf("Read: 0x%02X ", mx28ProtocolHeader0);
00170 #endif
00171                     if(mx28ProtocolHeader0 == 0xFF)
00172                         decoderState = WAIT_ON_HEADER_1;
00173                     else
00174                         isWholePacket = false;
00175 
00176                     break;
00177                 }
00178                 case WAIT_ON_HEADER_1: {
00179 
00180                     uint8_t mx28ProtocolHeader1 = serialCom->getc();
00181 
00182 #ifdef ANDANTE_DEBUG
00183                     pc->printf("0x%02X ", mx28ProtocolHeader1);
00184 #endif
00185 
00186                     decoderState = WAIT_ON_ROBOT_ID;
00187 
00188                     break;
00189                 }
00190                 case WAIT_ON_ROBOT_ID: {
00191 
00192 
00193                     packet->robotId = serialCom->getc();
00194 
00195 #ifdef ANDANTE_DEBUG
00196                     pc->printf("0x%02X ", packet->robotId);
00197 #endif
00198 
00199                     decoderState = WAIT_ON_LENGTH;
00200 
00201                     break;
00202                 }
00203                 case WAIT_ON_LENGTH: {
00204 
00205                     packet->length = serialCom->getc();
00206 
00207 #ifdef ANDANTE_DEBUG
00208                     pc->printf("0x%02X ", packet->length);
00209 #endif
00210 
00211                     decoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
00212 
00213                     break;
00214                 }
00215                 case WAIT_ON_INSTRUCTION_ERROR_ID: {
00216 
00217                     packet->instructionErrorId = serialCom->getc();
00218 
00219 #ifdef ANDANTE_DEBUG
00220                     pc->printf("0x%02X ", packet->instructionErrorId);
00221 #endif
00222 
00223                     if(packet->length > 2)
00224                         decoderState = WAIT_ON_PARAMETER;
00225                     else
00226                         decoderState = WAIT_ON_CHECK_SUM;
00227 
00228                     break;
00229                 }
00230                 case WAIT_ON_PARAMETER: {
00231 
00232                     uint8_t parameter = serialCom->getc();
00233                     packet->parameter[currentParameter] = parameter;
00234 
00235 #ifdef ANDANTE_DEBUG
00236                     pc->printf("0x%02X ", parameter);
00237 #endif
00238 
00239                     if(++currentParameter == packet->length - 2)
00240                         decoderState = WAIT_ON_CHECK_SUM;
00241 
00242                     break;
00243                 }
00244                 case WAIT_ON_CHECK_SUM: {
00245 
00246                     packet->checkSum = serialCom->getc();
00247 
00248 #ifdef ANDANTE_DEBUG
00249                     pc->printf("sum =0x%02X\r\n", packet->checkSum);
00250 #endif
00251 
00252                     decoderState = WAIT_ON_HEADER_0;
00253                     isWholePacket = true;
00254 
00255                     break;
00256                 }
00257             }
00258         }
00259     }
00260 
00261     timer.stop();
00262 
00263     if(!isWholePacket) {
00264 #ifdef ANDANTE_DEBUG
00265         pc->printf("Error: Read response timeout\r\n");
00266         pc->printf("Timer: %d ms\r\n", timer.read_ms());
00267 #endif
00268 
00269         return ANDANTE_ERRBIT_READ_TIMEOUT;
00270     }
00271 
00272     return ANDANTE_ERRBIT_NONE;
00273 }
00274 
00275 COMMUNICATION::COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff, uint16_t rx_buff )
00276 {
00277 
00278 
00279 #ifdef ANDANTE_DEBUG
00280     pc = new Serial(USBTX, USBRX);
00281     pc->baud(115200);
00282     pc->printf("\033[2J");
00283 #endif
00284 
00285     serialCom = new iSerial(tx, rx,NULL,tx_buff,rx_buff);
00286     serialCom->baud(baudRate);
00287 
00288 }
00289 
00290 COMMUNICATION::~COMMUNICATION()
00291 {
00292 #ifdef ANDANTE_DEBUG
00293     if(pc != NULL)
00294         delete pc;
00295 #endif
00296 
00297     if(serialCom != NULL)
00298         delete serialCom;
00299 }