Comunication_1
Dependents: MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale
Diff: communication_1.cpp
- Revision:
- 3:33d025a403d5
- Parent:
- 2:b4d1f5424448
- Child:
- 4:8933ad3f9bc2
--- a/communication_1.cpp Sun Jun 26 21:14:44 2016 +0000 +++ b/communication_1.cpp Sat Jul 02 18:01:40 2016 +0000 @@ -2,148 +2,205 @@ #include "mbed.h" #define READ_DEBUG 1 +#define TRIGGER_DEBUG 0 communication_1::communication_1(PinName tx, PinName rx, int baud_rate) -: _SerialHalfDuplex(tx,rx) { - - _SerialHalfDuplex.baud(baud_rate); -} + : _SerialHalfDuplex(tx,rx) { + + _SerialHalfDuplex.baud(baud_rate); + } int communication_1::read(int ID, int start, int bytes, char* data) { - char PacketLength = 0x4; - char TxBuf[16]; - char sum = 0; - char Status[16]; + char PacketLength = 0x4; + char TxBuf[16]; + char sum = 0; + char Status[16]; - Status[4] = 0xFE; // return code + Status[4] = 0xFE; // return code + + TxBuf[0] = 0xff; + TxBuf[1] = 0xff; - TxBuf[0] = 0xff; - TxBuf[1] = 0xff; + // ID + TxBuf[2] = ID; + sum += TxBuf[2]; - // ID - TxBuf[2] = ID; - sum += TxBuf[2]; + // Packet Length + TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) + sum += TxBuf[3]; // Accululate the packet sum - // Packet Length - TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) - sum += TxBuf[3]; // Accululate the packet sum + // Instruction - Read + TxBuf[4] = 0x2; + sum += TxBuf[4]; - // Instruction - Read - TxBuf[4] = 0x2; - sum += TxBuf[4]; + // Start Address + TxBuf[5] = start; + sum += TxBuf[5]; + + // Bytes to read + TxBuf[6] = bytes; + sum += TxBuf[6]; + + // Checksum + TxBuf[7] = 0xFF - sum; - // Start Address - TxBuf[5] = start; - sum += TxBuf[5]; - - // Bytes to read - TxBuf[6] = bytes; - sum += TxBuf[6]; - - // Checksum - TxBuf[7] = 0xFF - sum; + // Transmit the packet in one burst with no pausing + for (int i = 0; i<8 ; i++) { + // printf("Inizio scrittura numero %d\n",i); + _SerialHalfDuplex.putc(TxBuf[i]); + //printf("Fine scrittura numero %d\n",i); + } - // Transmit the packet in one burst with no pausing - for (int i = 0; i<8 ; i++) { - // printf("Inizio scrittura numero %d\n",i); - _SerialHalfDuplex.putc(TxBuf[i]); - //printf("Fine scrittura numero %d\n",i); + // Wait for the bytes to be transmitted + wait (0.00002); + + // Skip if the read was to the broadcast address + if (ID != 0xFE) { + // Receive the Status packet 6+ number of bytes read + for (int i=0; i<(6+bytes) ; i++) { + Status[i] = _SerialHalfDuplex.getc(); } - // Wait for the bytes to be transmitted - wait (0.00002); - - // Skip if the read was to the broadcast address - if (ID != 0xFE) { - // Receive the Status packet 6+ number of bytes read - for (int i=0; i<(6+bytes) ; i++) { - Status[i] = _SerialHalfDuplex.getc(); - } + // Copy the data from Status into data for return + for (int i=0; i < Status[3]-2 ; i++) { + data[i] = Status[5+i]; + } - // Copy the data from Status into data for return - for (int i=0; i < Status[3]-2 ; i++) { - data[i] = Status[5+i]; - } - - if (READ_DEBUG) { - printf("\nStatus Packet\n"); - printf(" Header : 0x%x\n",Status[0]); - printf(" Header : 0x%x\n",Status[1]); - printf(" ID : 0x%x\n",Status[2]); - printf(" Length : 0x%x\n",Status[3]); - printf(" Error Code : 0x%x\n",Status[4]); - - for (int i=0; i < Status[3]-2 ; i++) { - printf(" Data : 0x%x\n",Status[5+i]); - } - printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]); - } - } // if (ID!=0xFE) - - return(Status[4]); + if (READ_DEBUG) { + printf("\nStatus Packet\n"); + printf(" Header : 0x%x\n",Status[0]); + printf(" Header : 0x%x\n",Status[1]); + printf(" ID : 0x%x\n",Status[2]); + printf(" Length : 0x%x\n",Status[3]); + printf(" Error Code : 0x%x\n",Status[4]); + + for (int i=0; i < Status[3]-2 ; i++) { + printf(" Data : 0x%x\n",Status[5+i]); + } + printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]); + } + } // if (ID!=0xFE) + + return(Status[4]); } int communication_1::write(int ID, int start, int bytes, char* data, int flag) { -// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum + // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum + + char TxBuf[16]; + char sum = 0; + char Status[6]; + + TxBuf[0] = 0xff; + TxBuf[1] = 0xff; + + // ID + TxBuf[2] = ID; + sum += TxBuf[2]; - char TxBuf[16]; - char sum = 0; - char Status[6]; + // packet Length + TxBuf[3] = 3+bytes; + sum += TxBuf[3]; - TxBuf[0] = 0xff; - TxBuf[1] = 0xff; + // Instruction + if (flag == 1) { + TxBuf[4]=0x04; + sum += TxBuf[4]; + } else { + TxBuf[4]=0x03; + sum += TxBuf[4]; + } - // ID - TxBuf[2] = ID; - sum += TxBuf[2]; + // Start Address + TxBuf[5] = start; + sum += TxBuf[5]; - // packet Length - TxBuf[3] = 3+bytes; - sum += TxBuf[3]; + // data + for (int i=0; i<bytes ; i++) { + TxBuf[6+i] = data[i]; + sum += TxBuf[6+i]; + } + + // checksum + TxBuf[6+bytes] = 0xFF - sum; - // Instruction - if (flag == 1) { - TxBuf[4]=0x04; - sum += TxBuf[4]; - } else { - TxBuf[4]=0x03; - sum += TxBuf[4]; + // Transmit the packet in one burst with no pausing + for (int i = 0; i < (7 + bytes) ; i++) { + _SerialHalfDuplex.putc(TxBuf[i]); + } + + // Wait for data to transmit + wait (0.00002); + + // make sure we have a valid return + Status[4]=0x00; + + // we'll only get a reply if it was not broadcast + if (ID!=0xFE) { + // response is always 6 bytes + // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum + for (int i=0; i < 6 ; i++) { + Status[i] = _SerialHalfDuplex.getc(); } - // Start Address - TxBuf[5] = start; - sum += TxBuf[5]; - - // data - for (int i=0; i<bytes ; i++) { - TxBuf[6+i] = data[i]; - sum += TxBuf[6+i]; - } + return(Status[4]); // return error code + + } +} + +void communication_1::trigger(void) { + + char TxBuf[16]; + char sum = 0; + + if (TRIGGER_DEBUG) { + printf("\nTriggered\n"); + } - // checksum - TxBuf[6+bytes] = 0xFF - sum; + // Build the TxPacket first in RAM, then we'll send in one go + if (TRIGGER_DEBUG) { + printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); + } + + TxBuf[0] = 0xFF; + TxBuf[1] = 0xFF; - // Transmit the packet in one burst with no pausing - for (int i = 0; i < (7 + bytes) ; i++) { - _SerialHalfDuplex.putc(TxBuf[i]); - } + // ID - Broadcast + TxBuf[2] = 0xFE; + sum += TxBuf[2]; + + if (TRIGGER_DEBUG) { + printf(" ID : %d\n",TxBuf[2]); + } - // Wait for data to transmit - wait (0.00002); + // Length + TxBuf[3] = 0x02; + sum += TxBuf[3]; + if (TRIGGER_DEBUG) { + printf(" Length %d\n",TxBuf[3]); + } - // make sure we have a valid return - Status[4]=0x00; + // Instruction - ACTION + TxBuf[4] = 0x04; + sum += TxBuf[4]; + if (TRIGGER_DEBUG) { + printf(" Instruction 0x%X\n",TxBuf[5]); + } - // we'll only get a reply if it was not broadcast - if (ID!=0xFE) { - // response is always 6 bytes - // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum - for (int i=0; i < 6 ; i++) { - Status[i] = _SerialHalfDuplex.getc(); - } - - return(Status[4]); // return error code + // Checksum + TxBuf[5] = 0xFF - sum; + if (TRIGGER_DEBUG) { + printf(" Checksum 0x%X\n",TxBuf[5]); + } -}} \ No newline at end of file + // Transmit the packet in one burst with no pausing + for (int i = 0; i < 6 ; i++) { + _SerialHalfDuplex.putc(TxBuf[i]); + } + + // This is a broadcast packet, so there will be no reply + + return; +}