Comunication_1
Dependents: MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale
communication_1.cpp@2:b4d1f5424448, 2016-06-26 (annotated)
- Committer:
- mattiasub
- Date:
- Sun Jun 26 21:14:44 2016 +0000
- Revision:
- 2:b4d1f5424448
- Child:
- 3:33d025a403d5
Risolti errori su Communication_1 . Da risolvere ancora MX106;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mattiasub | 2:b4d1f5424448 | 1 | #include "communication_1.h" |
mattiasub | 2:b4d1f5424448 | 2 | #include "mbed.h" |
mattiasub | 2:b4d1f5424448 | 3 | |
mattiasub | 2:b4d1f5424448 | 4 | #define READ_DEBUG 1 |
mattiasub | 2:b4d1f5424448 | 5 | |
mattiasub | 2:b4d1f5424448 | 6 | communication_1::communication_1(PinName tx, PinName rx, int baud_rate) |
mattiasub | 2:b4d1f5424448 | 7 | : _SerialHalfDuplex(tx,rx) { |
mattiasub | 2:b4d1f5424448 | 8 | |
mattiasub | 2:b4d1f5424448 | 9 | _SerialHalfDuplex.baud(baud_rate); |
mattiasub | 2:b4d1f5424448 | 10 | } |
mattiasub | 2:b4d1f5424448 | 11 | |
mattiasub | 2:b4d1f5424448 | 12 | int communication_1::read(int ID, int start, int bytes, char* data) { |
mattiasub | 2:b4d1f5424448 | 13 | |
mattiasub | 2:b4d1f5424448 | 14 | char PacketLength = 0x4; |
mattiasub | 2:b4d1f5424448 | 15 | char TxBuf[16]; |
mattiasub | 2:b4d1f5424448 | 16 | char sum = 0; |
mattiasub | 2:b4d1f5424448 | 17 | char Status[16]; |
mattiasub | 2:b4d1f5424448 | 18 | |
mattiasub | 2:b4d1f5424448 | 19 | Status[4] = 0xFE; // return code |
mattiasub | 2:b4d1f5424448 | 20 | |
mattiasub | 2:b4d1f5424448 | 21 | TxBuf[0] = 0xff; |
mattiasub | 2:b4d1f5424448 | 22 | TxBuf[1] = 0xff; |
mattiasub | 2:b4d1f5424448 | 23 | |
mattiasub | 2:b4d1f5424448 | 24 | // ID |
mattiasub | 2:b4d1f5424448 | 25 | TxBuf[2] = ID; |
mattiasub | 2:b4d1f5424448 | 26 | sum += TxBuf[2]; |
mattiasub | 2:b4d1f5424448 | 27 | |
mattiasub | 2:b4d1f5424448 | 28 | // Packet Length |
mattiasub | 2:b4d1f5424448 | 29 | TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) |
mattiasub | 2:b4d1f5424448 | 30 | sum += TxBuf[3]; // Accululate the packet sum |
mattiasub | 2:b4d1f5424448 | 31 | |
mattiasub | 2:b4d1f5424448 | 32 | // Instruction - Read |
mattiasub | 2:b4d1f5424448 | 33 | TxBuf[4] = 0x2; |
mattiasub | 2:b4d1f5424448 | 34 | sum += TxBuf[4]; |
mattiasub | 2:b4d1f5424448 | 35 | |
mattiasub | 2:b4d1f5424448 | 36 | // Start Address |
mattiasub | 2:b4d1f5424448 | 37 | TxBuf[5] = start; |
mattiasub | 2:b4d1f5424448 | 38 | sum += TxBuf[5]; |
mattiasub | 2:b4d1f5424448 | 39 | |
mattiasub | 2:b4d1f5424448 | 40 | // Bytes to read |
mattiasub | 2:b4d1f5424448 | 41 | TxBuf[6] = bytes; |
mattiasub | 2:b4d1f5424448 | 42 | sum += TxBuf[6]; |
mattiasub | 2:b4d1f5424448 | 43 | |
mattiasub | 2:b4d1f5424448 | 44 | // Checksum |
mattiasub | 2:b4d1f5424448 | 45 | TxBuf[7] = 0xFF - sum; |
mattiasub | 2:b4d1f5424448 | 46 | |
mattiasub | 2:b4d1f5424448 | 47 | // Transmit the packet in one burst with no pausing |
mattiasub | 2:b4d1f5424448 | 48 | for (int i = 0; i<8 ; i++) { |
mattiasub | 2:b4d1f5424448 | 49 | // printf("Inizio scrittura numero %d\n",i); |
mattiasub | 2:b4d1f5424448 | 50 | _SerialHalfDuplex.putc(TxBuf[i]); |
mattiasub | 2:b4d1f5424448 | 51 | //printf("Fine scrittura numero %d\n",i); |
mattiasub | 2:b4d1f5424448 | 52 | } |
mattiasub | 2:b4d1f5424448 | 53 | |
mattiasub | 2:b4d1f5424448 | 54 | // Wait for the bytes to be transmitted |
mattiasub | 2:b4d1f5424448 | 55 | wait (0.00002); |
mattiasub | 2:b4d1f5424448 | 56 | |
mattiasub | 2:b4d1f5424448 | 57 | // Skip if the read was to the broadcast address |
mattiasub | 2:b4d1f5424448 | 58 | if (ID != 0xFE) { |
mattiasub | 2:b4d1f5424448 | 59 | // Receive the Status packet 6+ number of bytes read |
mattiasub | 2:b4d1f5424448 | 60 | for (int i=0; i<(6+bytes) ; i++) { |
mattiasub | 2:b4d1f5424448 | 61 | Status[i] = _SerialHalfDuplex.getc(); |
mattiasub | 2:b4d1f5424448 | 62 | } |
mattiasub | 2:b4d1f5424448 | 63 | |
mattiasub | 2:b4d1f5424448 | 64 | // Copy the data from Status into data for return |
mattiasub | 2:b4d1f5424448 | 65 | for (int i=0; i < Status[3]-2 ; i++) { |
mattiasub | 2:b4d1f5424448 | 66 | data[i] = Status[5+i]; |
mattiasub | 2:b4d1f5424448 | 67 | } |
mattiasub | 2:b4d1f5424448 | 68 | |
mattiasub | 2:b4d1f5424448 | 69 | if (READ_DEBUG) { |
mattiasub | 2:b4d1f5424448 | 70 | printf("\nStatus Packet\n"); |
mattiasub | 2:b4d1f5424448 | 71 | printf(" Header : 0x%x\n",Status[0]); |
mattiasub | 2:b4d1f5424448 | 72 | printf(" Header : 0x%x\n",Status[1]); |
mattiasub | 2:b4d1f5424448 | 73 | printf(" ID : 0x%x\n",Status[2]); |
mattiasub | 2:b4d1f5424448 | 74 | printf(" Length : 0x%x\n",Status[3]); |
mattiasub | 2:b4d1f5424448 | 75 | printf(" Error Code : 0x%x\n",Status[4]); |
mattiasub | 2:b4d1f5424448 | 76 | |
mattiasub | 2:b4d1f5424448 | 77 | for (int i=0; i < Status[3]-2 ; i++) { |
mattiasub | 2:b4d1f5424448 | 78 | printf(" Data : 0x%x\n",Status[5+i]); |
mattiasub | 2:b4d1f5424448 | 79 | } |
mattiasub | 2:b4d1f5424448 | 80 | printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]); |
mattiasub | 2:b4d1f5424448 | 81 | } |
mattiasub | 2:b4d1f5424448 | 82 | } // if (ID!=0xFE) |
mattiasub | 2:b4d1f5424448 | 83 | |
mattiasub | 2:b4d1f5424448 | 84 | return(Status[4]); |
mattiasub | 2:b4d1f5424448 | 85 | } |
mattiasub | 2:b4d1f5424448 | 86 | |
mattiasub | 2:b4d1f5424448 | 87 | |
mattiasub | 2:b4d1f5424448 | 88 | int communication_1::write(int ID, int start, int bytes, char* data, int flag) { |
mattiasub | 2:b4d1f5424448 | 89 | // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum |
mattiasub | 2:b4d1f5424448 | 90 | |
mattiasub | 2:b4d1f5424448 | 91 | char TxBuf[16]; |
mattiasub | 2:b4d1f5424448 | 92 | char sum = 0; |
mattiasub | 2:b4d1f5424448 | 93 | char Status[6]; |
mattiasub | 2:b4d1f5424448 | 94 | |
mattiasub | 2:b4d1f5424448 | 95 | TxBuf[0] = 0xff; |
mattiasub | 2:b4d1f5424448 | 96 | TxBuf[1] = 0xff; |
mattiasub | 2:b4d1f5424448 | 97 | |
mattiasub | 2:b4d1f5424448 | 98 | // ID |
mattiasub | 2:b4d1f5424448 | 99 | TxBuf[2] = ID; |
mattiasub | 2:b4d1f5424448 | 100 | sum += TxBuf[2]; |
mattiasub | 2:b4d1f5424448 | 101 | |
mattiasub | 2:b4d1f5424448 | 102 | // packet Length |
mattiasub | 2:b4d1f5424448 | 103 | TxBuf[3] = 3+bytes; |
mattiasub | 2:b4d1f5424448 | 104 | sum += TxBuf[3]; |
mattiasub | 2:b4d1f5424448 | 105 | |
mattiasub | 2:b4d1f5424448 | 106 | // Instruction |
mattiasub | 2:b4d1f5424448 | 107 | if (flag == 1) { |
mattiasub | 2:b4d1f5424448 | 108 | TxBuf[4]=0x04; |
mattiasub | 2:b4d1f5424448 | 109 | sum += TxBuf[4]; |
mattiasub | 2:b4d1f5424448 | 110 | } else { |
mattiasub | 2:b4d1f5424448 | 111 | TxBuf[4]=0x03; |
mattiasub | 2:b4d1f5424448 | 112 | sum += TxBuf[4]; |
mattiasub | 2:b4d1f5424448 | 113 | } |
mattiasub | 2:b4d1f5424448 | 114 | |
mattiasub | 2:b4d1f5424448 | 115 | // Start Address |
mattiasub | 2:b4d1f5424448 | 116 | TxBuf[5] = start; |
mattiasub | 2:b4d1f5424448 | 117 | sum += TxBuf[5]; |
mattiasub | 2:b4d1f5424448 | 118 | |
mattiasub | 2:b4d1f5424448 | 119 | // data |
mattiasub | 2:b4d1f5424448 | 120 | for (int i=0; i<bytes ; i++) { |
mattiasub | 2:b4d1f5424448 | 121 | TxBuf[6+i] = data[i]; |
mattiasub | 2:b4d1f5424448 | 122 | sum += TxBuf[6+i]; |
mattiasub | 2:b4d1f5424448 | 123 | } |
mattiasub | 2:b4d1f5424448 | 124 | |
mattiasub | 2:b4d1f5424448 | 125 | // checksum |
mattiasub | 2:b4d1f5424448 | 126 | TxBuf[6+bytes] = 0xFF - sum; |
mattiasub | 2:b4d1f5424448 | 127 | |
mattiasub | 2:b4d1f5424448 | 128 | // Transmit the packet in one burst with no pausing |
mattiasub | 2:b4d1f5424448 | 129 | for (int i = 0; i < (7 + bytes) ; i++) { |
mattiasub | 2:b4d1f5424448 | 130 | _SerialHalfDuplex.putc(TxBuf[i]); |
mattiasub | 2:b4d1f5424448 | 131 | } |
mattiasub | 2:b4d1f5424448 | 132 | |
mattiasub | 2:b4d1f5424448 | 133 | // Wait for data to transmit |
mattiasub | 2:b4d1f5424448 | 134 | wait (0.00002); |
mattiasub | 2:b4d1f5424448 | 135 | |
mattiasub | 2:b4d1f5424448 | 136 | // make sure we have a valid return |
mattiasub | 2:b4d1f5424448 | 137 | Status[4]=0x00; |
mattiasub | 2:b4d1f5424448 | 138 | |
mattiasub | 2:b4d1f5424448 | 139 | // we'll only get a reply if it was not broadcast |
mattiasub | 2:b4d1f5424448 | 140 | if (ID!=0xFE) { |
mattiasub | 2:b4d1f5424448 | 141 | // response is always 6 bytes |
mattiasub | 2:b4d1f5424448 | 142 | // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum |
mattiasub | 2:b4d1f5424448 | 143 | for (int i=0; i < 6 ; i++) { |
mattiasub | 2:b4d1f5424448 | 144 | Status[i] = _SerialHalfDuplex.getc(); |
mattiasub | 2:b4d1f5424448 | 145 | } |
mattiasub | 2:b4d1f5424448 | 146 | |
mattiasub | 2:b4d1f5424448 | 147 | return(Status[4]); // return error code |
mattiasub | 2:b4d1f5424448 | 148 | |
mattiasub | 2:b4d1f5424448 | 149 | }} |