palm and chin / Communication_Robot

v1

Fork of Fork_Boss_Communication_Robot by BE@R lab

communication.cpp@0:0c88691e3904, 2015-03-01 (annotated)

Committer:
soulx
Date:
Sun Mar 01 06:28:01 2015 +0000
Revision:
0:0c88691e3904
Child:
1:1f6864549b92
create lib communication for robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 0:0c88691e3904 1 #include "mbed.h"
soulx 0:0c88691e3904 2 #include "communication.h"
soulx 0:0c88691e3904 3
soulx 0:0c88691e3904 4 uint8_t COMMUNICATION::CommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet)
soulx 0:0c88691e3904 5 {
soulx 0:0c88691e3904 6 uint8_t currentParameter = 0;
soulx 0:0c88691e3904 7 bool isWholePacket = false;
soulx 0:0c88691e3904 8
soulx 0:0c88691e3904 9 currentParameter = 0;
soulx 0:0c88691e3904 10 isWholePacket = false;
soulx 0:0c88691e3904 11 uint8_t decoderState = WAIT_ON_HEADER_0;
soulx 0:0c88691e3904 12
soulx 0:0c88691e3904 13 Timer timer;
soulx 0:0c88691e3904 14 timer.start();
soulx 0:0c88691e3904 15
soulx 0:0c88691e3904 16 while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) {
soulx 0:0c88691e3904 17 if(serialCom->readable()) {
soulx 0:0c88691e3904 18 switch(decoderState) {
soulx 0:0c88691e3904 19 case WAIT_ON_HEADER_0: {
soulx 0:0c88691e3904 20
soulx 0:0c88691e3904 21 uint8_t mx28ProtocolHeader0 = serialCom->getc();
soulx 0:0c88691e3904 22
soulx 0:0c88691e3904 23 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 24 pc->printf("Read: 0x%02X ", mx28ProtocolHeader0);
soulx 0:0c88691e3904 25 #endif
soulx 0:0c88691e3904 26
soulx 0:0c88691e3904 27 decoderState = WAIT_ON_HEADER_1;
soulx 0:0c88691e3904 28
soulx 0:0c88691e3904 29 break;
soulx 0:0c88691e3904 30 }
soulx 0:0c88691e3904 31 case WAIT_ON_HEADER_1: {
soulx 0:0c88691e3904 32
soulx 0:0c88691e3904 33 uint8_t mx28ProtocolHeader1 = serialCom->getc();
soulx 0:0c88691e3904 34
soulx 0:0c88691e3904 35 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 36 pc->printf("0x%02X ", mx28ProtocolHeader1);
soulx 0:0c88691e3904 37 #endif
soulx 0:0c88691e3904 38
soulx 0:0c88691e3904 39 decoderState = WAIT_ON_ROBOT_ID;
soulx 0:0c88691e3904 40
soulx 0:0c88691e3904 41 break;
soulx 0:0c88691e3904 42 }
soulx 0:0c88691e3904 43 case WAIT_ON_ROBOT_ID: {
soulx 0:0c88691e3904 44
soulx 0:0c88691e3904 45
soulx 0:0c88691e3904 46 packet->robotId = serialCom->getc();
soulx 0:0c88691e3904 47
soulx 0:0c88691e3904 48 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 49 pc->printf("0x%02X ", packet->servoId);
soulx 0:0c88691e3904 50 #endif
soulx 0:0c88691e3904 51
soulx 0:0c88691e3904 52 decoderState = WAIT_ON_LENGTH;
soulx 0:0c88691e3904 53
soulx 0:0c88691e3904 54 break;
soulx 0:0c88691e3904 55 }
soulx 0:0c88691e3904 56 case WAIT_ON_LENGTH: {
soulx 0:0c88691e3904 57
soulx 0:0c88691e3904 58 packet->length = serialCom->getc();
soulx 0:0c88691e3904 59
soulx 0:0c88691e3904 60 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 61 pc->printf("0x%02X ", packet->length);
soulx 0:0c88691e3904 62 #endif
soulx 0:0c88691e3904 63
soulx 0:0c88691e3904 64 decoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
soulx 0:0c88691e3904 65
soulx 0:0c88691e3904 66 break;
soulx 0:0c88691e3904 67 }
soulx 0:0c88691e3904 68 case WAIT_ON_INSTRUCTION_ERROR_ID: {
soulx 0:0c88691e3904 69
soulx 0:0c88691e3904 70 packet->instructionErrorId = serialCom->getc();
soulx 0:0c88691e3904 71
soulx 0:0c88691e3904 72 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 73 pc->printf("0x%02X ", packet->instructionErrorId);
soulx 0:0c88691e3904 74 #endif
soulx 0:0c88691e3904 75
soulx 0:0c88691e3904 76 if(packet->length > 2)
soulx 0:0c88691e3904 77 decoderState = WAIT_ON_PARAMETER;
soulx 0:0c88691e3904 78 else
soulx 0:0c88691e3904 79 decoderState = WAIT_ON_CHECK_SUM;
soulx 0:0c88691e3904 80
soulx 0:0c88691e3904 81 break;
soulx 0:0c88691e3904 82 }
soulx 0:0c88691e3904 83 case WAIT_ON_PARAMETER: {
soulx 0:0c88691e3904 84
soulx 0:0c88691e3904 85 uint8_t parameter = serialCom->getc();
soulx 0:0c88691e3904 86 packet->parameter[currentParameter] = parameter;
soulx 0:0c88691e3904 87
soulx 0:0c88691e3904 88 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 89 pc->printf("0x%02X ", parameter);
soulx 0:0c88691e3904 90 #endif
soulx 0:0c88691e3904 91
soulx 0:0c88691e3904 92 if(++currentParameter == packet->length - 2)
soulx 0:0c88691e3904 93 decoderState = WAIT_ON_CHECK_SUM;
soulx 0:0c88691e3904 94
soulx 0:0c88691e3904 95 break;
soulx 0:0c88691e3904 96 }
soulx 0:0c88691e3904 97 case WAIT_ON_CHECK_SUM: {
soulx 0:0c88691e3904 98
soulx 0:0c88691e3904 99 packet->checkSum = serialCom->getc();
soulx 0:0c88691e3904 100
soulx 0:0c88691e3904 101 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 102 pc->printf("sum =0x%02X\r\n", packet->checkSum);
soulx 0:0c88691e3904 103 #endif
soulx 0:0c88691e3904 104
soulx 0:0c88691e3904 105 decoderState = WAIT_ON_HEADER_0;
soulx 0:0c88691e3904 106 isWholePacket = true;
soulx 0:0c88691e3904 107
soulx 0:0c88691e3904 108 break;
soulx 0:0c88691e3904 109 }
soulx 0:0c88691e3904 110 }
soulx 0:0c88691e3904 111 }
soulx 0:0c88691e3904 112 }
soulx 0:0c88691e3904 113
soulx 0:0c88691e3904 114 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 115 pc->printf("Timer: %d ms\r\n", timer.read_ms());
soulx 0:0c88691e3904 116 #endif
soulx 0:0c88691e3904 117
soulx 0:0c88691e3904 118 timer.stop();
soulx 0:0c88691e3904 119
soulx 0:0c88691e3904 120 if(!isWholePacket) {
soulx 0:0c88691e3904 121 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 122 pc->printf("Error: Read response timeout\r\n");
soulx 0:0c88691e3904 123 #endif
soulx 0:0c88691e3904 124
soulx 0:0c88691e3904 125 return ANDANTE_ERRBIT_READ_TIMEOUT;
soulx 0:0c88691e3904 126 }
soulx 0:0c88691e3904 127
soulx 0:0c88691e3904 128 timer.reset();
soulx 0:0c88691e3904 129 timer.start();
soulx 0:0c88691e3904 130
soulx 0:0c88691e3904 131 }
soulx 0:0c88691e3904 132
soulx 0:0c88691e3904 133 COMMUNICATION::COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff, uint16_t rx_buff )
soulx 0:0c88691e3904 134 {
soulx 0:0c88691e3904 135
soulx 0:0c88691e3904 136
soulx 0:0c88691e3904 137 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 138 pc = new Serial(USBTX, USBRX);
soulx 0:0c88691e3904 139 pc->baud(115200);
soulx 0:0c88691e3904 140 pc->printf("\033[2J");
soulx 0:0c88691e3904 141 #endif
soulx 0:0c88691e3904 142
soulx 0:0c88691e3904 143 serialCom = new iSerial(tx, rx,NULL,tx_buff,rx_buff);
soulx 0:0c88691e3904 144 serialCom->baud(baudRate);
soulx 0:0c88691e3904 145
soulx 0:0c88691e3904 146 }
soulx 0:0c88691e3904 147
soulx 0:0c88691e3904 148 COMMUNICATION::~COMMUNICATION()
soulx 0:0c88691e3904 149 {
soulx 0:0c88691e3904 150 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 151 if(pc != NULL)
soulx 0:0c88691e3904 152 delete pc;
soulx 0:0c88691e3904 153 #endif
soulx 0:0c88691e3904 154
soulx 0:0c88691e3904 155 if(serialCom != NULL)
soulx 0:0c88691e3904 156 delete serialCom;
soulx 0:0c88691e3904 157 }