Team DIANA / comunication_1

Comunication_1

Dependents:   MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale

communication_1.cpp@3:33d025a403d5, 2016-07-02 (annotated)

Committer:
clynamen
Date:
Sat Jul 02 18:01:40 2016 +0000
Revision:
3:33d025a403d5
Parent:
2:b4d1f5424448
Child:
4:8933ad3f9bc2
add trigger method;

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