v1
Fork of Fork_Boss_Communication_Robot by
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?
User | Revision | Line number | New 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 | } |