Numchok Pimyai / rrrrr

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.

Fork of Communication_Robot by BE@R lab

communication.cpp@1:1f6864549b92, 2015-03-01 (annotated)

Committer:
soulx
Date:
Sun Mar 01 08:48:23 2015 +0000
Revision:
1:1f6864549b92
Parent:
0:0c88691e3904
Child:
2:56c1de1612dd
create communication with protocol similar robotis protocol; -receive; -not send

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 1:1f6864549b92 4 uint8_t COMMUNICATION::receiveCommunicatePacket(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 timer.stop();
soulx 0:0c88691e3904 115
soulx 0:0c88691e3904 116 if(!isWholePacket) {
soulx 0:0c88691e3904 117 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 118 pc->printf("Error: Read response timeout\r\n");
soulx 1:1f6864549b92 119 pc->printf("Timer: %d ms\r\n", timer.read_ms());
soulx 0:0c88691e3904 120 #endif
soulx 0:0c88691e3904 121
soulx 0:0c88691e3904 122 return ANDANTE_ERRBIT_READ_TIMEOUT;
soulx 0:0c88691e3904 123 }
soulx 0:0c88691e3904 124
soulx 1:1f6864549b92 125 return ANDANTE_ERRBIT_NONE;
soulx 0:0c88691e3904 126 }
soulx 0:0c88691e3904 127
soulx 0:0c88691e3904 128 COMMUNICATION::COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff, uint16_t rx_buff )
soulx 0:0c88691e3904 129 {
soulx 0:0c88691e3904 130
soulx 0:0c88691e3904 131
soulx 0:0c88691e3904 132 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 133 pc = new Serial(USBTX, USBRX);
soulx 0:0c88691e3904 134 pc->baud(115200);
soulx 0:0c88691e3904 135 pc->printf("\033[2J");
soulx 0:0c88691e3904 136 #endif
soulx 0:0c88691e3904 137
soulx 0:0c88691e3904 138 serialCom = new iSerial(tx, rx,NULL,tx_buff,rx_buff);
soulx 0:0c88691e3904 139 serialCom->baud(baudRate);
soulx 0:0c88691e3904 140
soulx 0:0c88691e3904 141 }
soulx 0:0c88691e3904 142
soulx 0:0c88691e3904 143 COMMUNICATION::~COMMUNICATION()
soulx 0:0c88691e3904 144 {
soulx 0:0c88691e3904 145 #ifdef ANDANTE_DEBUG
soulx 0:0c88691e3904 146 if(pc != NULL)
soulx 0:0c88691e3904 147 delete pc;
soulx 0:0c88691e3904 148 #endif
soulx 0:0c88691e3904 149
soulx 0:0c88691e3904 150 if(serialCom != NULL)
soulx 0:0c88691e3904 151 delete serialCom;
soulx 0:0c88691e3904 152 }