Team DIANA / comunication_1

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?

UserRevisionLine numberNew 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 }}