Team DIANA / comunication_1

Comunication_1

Dependents:   MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale

communication_1.cpp@6:eb570f9f48ad, 2019-04-16 (annotated)

Committer:
stebonicelli
Date:
Tue Apr 16 08:58:52 2019 +0000
Revision:
6:eb570f9f48ad
Parent:
5:a9648ebd6ab7 
using RawSerial for mbed5 os compatibility

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
clynamen 4:8933ad3f9bc2 5 #define WRITE_DEBUG 1
clynamen 3:33d025a403d5 6 #define TRIGGER_DEBUG 0
mattiasub 2:b4d1f5424448 7
mattiasub 2:b4d1f5424448 8 communication_1::communication_1(PinName tx, PinName rx, int baud_rate)
clynamen 3:33d025a403d5 9 : _SerialHalfDuplex(tx,rx) {
clynamen 3:33d025a403d5 10
clynamen 3:33d025a403d5 11 _SerialHalfDuplex.baud(baud_rate);
clynamen 3:33d025a403d5 12 }
mattiasub 2:b4d1f5424448 13
mattiasub 2:b4d1f5424448 14 int communication_1::read(int ID, int start, int bytes, char* data) {
mattiasub 2:b4d1f5424448 15
clynamen 3:33d025a403d5 16 char PacketLength = 0x4;
clynamen 3:33d025a403d5 17 char TxBuf[16];
clynamen 3:33d025a403d5 18 char sum = 0;
clynamen 3:33d025a403d5 19 char Status[16];
mattiasub 2:b4d1f5424448 20
clynamen 3:33d025a403d5 21 Status[4] = 0xFE; // return code
clynamen 3:33d025a403d5 22
clynamen 3:33d025a403d5 23 TxBuf[0] = 0xff;
clynamen 3:33d025a403d5 24 TxBuf[1] = 0xff;
mattiasub 2:b4d1f5424448 25
clynamen 3:33d025a403d5 26 // ID
clynamen 3:33d025a403d5 27 TxBuf[2] = ID;
clynamen 3:33d025a403d5 28 sum += TxBuf[2];
mattiasub 2:b4d1f5424448 29
clynamen 3:33d025a403d5 30 // Packet Length
clynamen 3:33d025a403d5 31 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
clynamen 3:33d025a403d5 32 sum += TxBuf[3]; // Accululate the packet sum
mattiasub 2:b4d1f5424448 33
clynamen 3:33d025a403d5 34 // Instruction - Read
clynamen 3:33d025a403d5 35 TxBuf[4] = 0x2;
clynamen 3:33d025a403d5 36 sum += TxBuf[4];
mattiasub 2:b4d1f5424448 37
clynamen 3:33d025a403d5 38 // Start Address
clynamen 3:33d025a403d5 39 TxBuf[5] = start;
clynamen 3:33d025a403d5 40 sum += TxBuf[5];
clynamen 3:33d025a403d5 41
clynamen 3:33d025a403d5 42 // Bytes to read
clynamen 3:33d025a403d5 43 TxBuf[6] = bytes;
clynamen 3:33d025a403d5 44 sum += TxBuf[6];
clynamen 3:33d025a403d5 45
clynamen 3:33d025a403d5 46 // Checksum
clynamen 3:33d025a403d5 47 TxBuf[7] = 0xFF - sum;
mattiasub 2:b4d1f5424448 48
clynamen 3:33d025a403d5 49 // Transmit the packet in one burst with no pausing
clynamen 3:33d025a403d5 50 for (int i = 0; i<8 ; i++) {
clynamen 3:33d025a403d5 51 // printf("Inizio scrittura numero %d\n",i);
clynamen 3:33d025a403d5 52 _SerialHalfDuplex.putc(TxBuf[i]);
clynamen 3:33d025a403d5 53 //printf("Fine scrittura numero %d\n",i);
clynamen 3:33d025a403d5 54 }
mattiasub 2:b4d1f5424448 55
clynamen 3:33d025a403d5 56 // Wait for the bytes to be transmitted
clynamen 3:33d025a403d5 57 wait (0.00002);
clynamen 3:33d025a403d5 58
clynamen 3:33d025a403d5 59 // Skip if the read was to the broadcast address
clynamen 3:33d025a403d5 60 if (ID != 0xFE) {
clynamen 3:33d025a403d5 61 // Receive the Status packet 6+ number of bytes read
clynamen 3:33d025a403d5 62 for (int i=0; i<(6+bytes) ; i++) {
clynamen 3:33d025a403d5 63 Status[i] = _SerialHalfDuplex.getc();
mattiasub 2:b4d1f5424448 64 }
mattiasub 2:b4d1f5424448 65
clynamen 3:33d025a403d5 66 // Copy the data from Status into data for return
clynamen 3:33d025a403d5 67 for (int i=0; i < Status[3]-2 ; i++) {
clynamen 3:33d025a403d5 68 data[i] = Status[5+i];
clynamen 3:33d025a403d5 69 }
mattiasub 2:b4d1f5424448 70
clynamen 3:33d025a403d5 71 if (READ_DEBUG) {
clynamen 3:33d025a403d5 72 printf("\nStatus Packet\n");
clynamen 3:33d025a403d5 73 printf(" Header : 0x%x\n",Status[0]);
clynamen 3:33d025a403d5 74 printf(" Header : 0x%x\n",Status[1]);
clynamen 3:33d025a403d5 75 printf(" ID : 0x%x\n",Status[2]);
clynamen 3:33d025a403d5 76 printf(" Length : 0x%x\n",Status[3]);
clynamen 3:33d025a403d5 77 printf(" Error Code : 0x%x\n",Status[4]);
clynamen 3:33d025a403d5 78
clynamen 3:33d025a403d5 79 for (int i=0; i < Status[3]-2 ; i++) {
clynamen 3:33d025a403d5 80 printf(" Data : 0x%x\n",Status[5+i]);
clynamen 3:33d025a403d5 81 }
clynamen 3:33d025a403d5 82 printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
clynamen 3:33d025a403d5 83 }
clynamen 3:33d025a403d5 84 } // if (ID!=0xFE)
clynamen 3:33d025a403d5 85
clynamen 3:33d025a403d5 86 return(Status[4]);
mattiasub 2:b4d1f5424448 87 }
mattiasub 2:b4d1f5424448 88
mattiasub 2:b4d1f5424448 89
mattiasub 2:b4d1f5424448 90 int communication_1::write(int ID, int start, int bytes, char* data, int flag) {
clynamen 3:33d025a403d5 91 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
clynamen 3:33d025a403d5 92
clynamen 3:33d025a403d5 93 char TxBuf[16];
clynamen 3:33d025a403d5 94 char sum = 0;
clynamen 3:33d025a403d5 95 char Status[6];
clynamen 3:33d025a403d5 96
clynamen 3:33d025a403d5 97 TxBuf[0] = 0xff;
clynamen 3:33d025a403d5 98 TxBuf[1] = 0xff;
clynamen 3:33d025a403d5 99
clynamen 3:33d025a403d5 100 // ID
clynamen 3:33d025a403d5 101 TxBuf[2] = ID;
clynamen 3:33d025a403d5 102 sum += TxBuf[2];
mattiasub 2:b4d1f5424448 103
clynamen 3:33d025a403d5 104 // packet Length
clynamen 3:33d025a403d5 105 TxBuf[3] = 3+bytes;
clynamen 3:33d025a403d5 106 sum += TxBuf[3];
mattiasub 2:b4d1f5424448 107
clynamen 3:33d025a403d5 108 // Instruction
clynamen 3:33d025a403d5 109 if (flag == 1) {
clynamen 3:33d025a403d5 110 TxBuf[4]=0x04;
clynamen 3:33d025a403d5 111 sum += TxBuf[4];
clynamen 3:33d025a403d5 112 } else {
clynamen 3:33d025a403d5 113 TxBuf[4]=0x03;
clynamen 3:33d025a403d5 114 sum += TxBuf[4];
clynamen 3:33d025a403d5 115 }
mattiasub 2:b4d1f5424448 116
clynamen 3:33d025a403d5 117 // Start Address
clynamen 3:33d025a403d5 118 TxBuf[5] = start;
clynamen 3:33d025a403d5 119 sum += TxBuf[5];
mattiasub 2:b4d1f5424448 120
clynamen 3:33d025a403d5 121 // data
clynamen 3:33d025a403d5 122 for (int i=0; i<bytes ; i++) {
clynamen 3:33d025a403d5 123 TxBuf[6+i] = data[i];
clynamen 3:33d025a403d5 124 sum += TxBuf[6+i];
clynamen 3:33d025a403d5 125 }
clynamen 3:33d025a403d5 126
clynamen 3:33d025a403d5 127 // checksum
clynamen 3:33d025a403d5 128 TxBuf[6+bytes] = 0xFF - sum;
mattiasub 2:b4d1f5424448 129
clynamen 3:33d025a403d5 130 // Transmit the packet in one burst with no pausing
clynamen 3:33d025a403d5 131 for (int i = 0; i < (7 + bytes) ; i++) {
clynamen 3:33d025a403d5 132 _SerialHalfDuplex.putc(TxBuf[i]);
clynamen 3:33d025a403d5 133 }
clynamen 3:33d025a403d5 134
clynamen 3:33d025a403d5 135 // Wait for data to transmit
clynamen 3:33d025a403d5 136 wait (0.00002);
clynamen 3:33d025a403d5 137
clynamen 3:33d025a403d5 138 // make sure we have a valid return
clynamen 3:33d025a403d5 139 Status[4]=0x00;
gidiana 5:a9648ebd6ab7 140 return(Status[4]);
clynamen 3:33d025a403d5 141
clynamen 3:33d025a403d5 142 // we'll only get a reply if it was not broadcast
gidiana 5:a9648ebd6ab7 143 //if (ID!=0xFE) {
clynamen 3:33d025a403d5 144 // response is always 6 bytes
clynamen 3:33d025a403d5 145 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
gidiana 5:a9648ebd6ab7 146 //for (int i=0; i < 6 ; i++) {
gidiana 5:a9648ebd6ab7 147 //Status[i] = _SerialHalfDuplex.getc();
gidiana 5:a9648ebd6ab7 148 //}
mattiasub 2:b4d1f5424448 149
gidiana 5:a9648ebd6ab7 150 //return(Status[4]); // return error code
clynamen 3:33d025a403d5 151
gidiana 5:a9648ebd6ab7 152 //}
clynamen 3:33d025a403d5 153 }
clynamen 3:33d025a403d5 154
clynamen 3:33d025a403d5 155 void communication_1::trigger(void) {
clynamen 3:33d025a403d5 156
clynamen 3:33d025a403d5 157 char TxBuf[16];
clynamen 3:33d025a403d5 158 char sum = 0;
clynamen 3:33d025a403d5 159
clynamen 3:33d025a403d5 160 if (TRIGGER_DEBUG) {
clynamen 3:33d025a403d5 161 printf("\nTriggered\n");
clynamen 3:33d025a403d5 162 }
mattiasub 2:b4d1f5424448 163
clynamen 3:33d025a403d5 164 // Build the TxPacket first in RAM, then we'll send in one go
clynamen 3:33d025a403d5 165 if (TRIGGER_DEBUG) {
clynamen 3:33d025a403d5 166 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
clynamen 3:33d025a403d5 167 }
clynamen 3:33d025a403d5 168
clynamen 3:33d025a403d5 169 TxBuf[0] = 0xFF;
clynamen 3:33d025a403d5 170 TxBuf[1] = 0xFF;
mattiasub 2:b4d1f5424448 171
clynamen 3:33d025a403d5 172 // ID - Broadcast
clynamen 3:33d025a403d5 173 TxBuf[2] = 0xFE;
clynamen 3:33d025a403d5 174 sum += TxBuf[2];
clynamen 3:33d025a403d5 175
clynamen 3:33d025a403d5 176 if (TRIGGER_DEBUG) {
clynamen 3:33d025a403d5 177 printf(" ID : %d\n",TxBuf[2]);
clynamen 3:33d025a403d5 178 }
mattiasub 2:b4d1f5424448 179
clynamen 3:33d025a403d5 180 // Length
clynamen 3:33d025a403d5 181 TxBuf[3] = 0x02;
clynamen 3:33d025a403d5 182 sum += TxBuf[3];
clynamen 3:33d025a403d5 183 if (TRIGGER_DEBUG) {
clynamen 3:33d025a403d5 184 printf(" Length %d\n",TxBuf[3]);
clynamen 3:33d025a403d5 185 }
mattiasub 2:b4d1f5424448 186
clynamen 3:33d025a403d5 187 // Instruction - ACTION
clynamen 3:33d025a403d5 188 TxBuf[4] = 0x04;
clynamen 3:33d025a403d5 189 sum += TxBuf[4];
clynamen 3:33d025a403d5 190 if (TRIGGER_DEBUG) {
clynamen 3:33d025a403d5 191 printf(" Instruction 0x%X\n",TxBuf[5]);
clynamen 3:33d025a403d5 192 }
mattiasub 2:b4d1f5424448 193
clynamen 3:33d025a403d5 194 // Checksum
clynamen 3:33d025a403d5 195 TxBuf[5] = 0xFF - sum;
clynamen 3:33d025a403d5 196 if (TRIGGER_DEBUG) {
clynamen 3:33d025a403d5 197 printf(" Checksum 0x%X\n",TxBuf[5]);
clynamen 3:33d025a403d5 198 }
mattiasub 2:b4d1f5424448 199
clynamen 3:33d025a403d5 200 // Transmit the packet in one burst with no pausing
clynamen 3:33d025a403d5 201 for (int i = 0; i < 6 ; i++) {
clynamen 3:33d025a403d5 202 _SerialHalfDuplex.putc(TxBuf[i]);
clynamen 3:33d025a403d5 203 }
clynamen 3:33d025a403d5 204
clynamen 3:33d025a403d5 205 // This is a broadcast packet, so there will be no reply
clynamen 3:33d025a403d5 206
clynamen 3:33d025a403d5 207 return;
clynamen 3:33d025a403d5 208 }