Rhino Reimburse / Mbed 2 deprecated Dynamixel_XL320

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of XL320_Test by Rhino Reimburse

XL320_MFA/XL320_MFA.cpp@1:acad5f85d4f5, 2016-02-24 (annotated)

Committer:
mfajaradityo
Date:
Wed Feb 24 10:37:29 2016 +0000
Revision:
1:acad5f85d4f5
Parent:
0:2071f8e463dd
Child:
2:0ec2dce7c5f8
ternyata jadi bener semua fungsi Get nya kalo line: pc.printf("has wait Return Packet for :'%d' us\n\r", i); di fungsi getRPacket() ga di-comment

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfajaradityo 0:2071f8e463dd 1 #include "XL320_MFA.h"
mfajaradityo 0:2071f8e463dd 2
mfajaradityo 0:2071f8e463dd 3 XL320::XL320(uint32_t baudrate, PinName tx, PinName rx, PinName dirpin): sXL320(tx, rx), dPin(dirpin)
mfajaradityo 0:2071f8e463dd 4 {
mfajaradityo 0:2071f8e463dd 5 dPin=0;
mfajaradityo 0:2071f8e463dd 6 sXL320.baud(baudrate);
mfajaradityo 0:2071f8e463dd 7 pc.baud(115200);
mfajaradityo 0:2071f8e463dd 8
mfajaradityo 0:2071f8e463dd 9 //Initialize header
mfajaradityo 0:2071f8e463dd 10 iPacket[0] = 0xff; //header
mfajaradityo 0:2071f8e463dd 11 iPacket[1] = 0xff; //header
mfajaradityo 0:2071f8e463dd 12 iPacket[2] = 0xfd; //header
mfajaradityo 0:2071f8e463dd 13 iPacket[3] = 0x00; //reserved
mfajaradityo 0:2071f8e463dd 14 }
mfajaradityo 0:2071f8e463dd 15
mfajaradityo 0:2071f8e463dd 16 ////// EEPROM ACCESS METHODS //////
mfajaradityo 0:2071f8e463dd 17
mfajaradityo 0:2071f8e463dd 18 /***** XL320 Network Parameter *****/
mfajaradityo 0:2071f8e463dd 19 void XL320::SetID(uint8_t id,uint8_t idnew)
mfajaradityo 0:2071f8e463dd 20 {
mfajaradityo 0:2071f8e463dd 21 pc.printf("Set ID\n\r");
mfajaradityo 0:2071f8e463dd 22 targetID = id;
mfajaradityo 0:2071f8e463dd 23 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 24 parameter[0] = idnew;
mfajaradityo 0:2071f8e463dd 25
mfajaradityo 0:2071f8e463dd 26 iWrite(1, XL_ID, parameter);
mfajaradityo 0:2071f8e463dd 27 sendIPacket();
mfajaradityo 0:2071f8e463dd 28
mfajaradityo 0:2071f8e463dd 29 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 30 getRPacket();
mfajaradityo 0:2071f8e463dd 31
mfajaradityo 0:2071f8e463dd 32 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 33 }
mfajaradityo 0:2071f8e463dd 34
mfajaradityo 0:2071f8e463dd 35 uint8_t XL320::GetID(uint8_t id)
mfajaradityo 0:2071f8e463dd 36 {
mfajaradityo 0:2071f8e463dd 37 pc.printf("Get ID\n\r");
mfajaradityo 0:2071f8e463dd 38 targetID = id;
mfajaradityo 0:2071f8e463dd 39 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 40
mfajaradityo 0:2071f8e463dd 41 iRead(1, XL_ID);
mfajaradityo 0:2071f8e463dd 42 sendIPacket();
mfajaradityo 0:2071f8e463dd 43
mfajaradityo 0:2071f8e463dd 44 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 45 getRPacket();
mfajaradityo 0:2071f8e463dd 46
mfajaradityo 0:2071f8e463dd 47 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 48
mfajaradityo 0:2071f8e463dd 49 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 50 {
mfajaradityo 0:2071f8e463dd 51 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 52 return rPacket[9]; //return tergeted servo ID
mfajaradityo 0:2071f8e463dd 53
mfajaradityo 0:2071f8e463dd 54 default : //error occurred
mfajaradityo 0:2071f8e463dd 55 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 56 }
mfajaradityo 0:2071f8e463dd 57 }
mfajaradityo 0:2071f8e463dd 58
mfajaradityo 0:2071f8e463dd 59 void XL320::SetBaudRate(uint8_t id,uint8_t baudrate)
mfajaradityo 0:2071f8e463dd 60 {
mfajaradityo 0:2071f8e463dd 61 pc.printf("Set BaudRate\n\r");
mfajaradityo 0:2071f8e463dd 62 targetID = id;
mfajaradityo 0:2071f8e463dd 63 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 64 parameter[0] = baudrate;
mfajaradityo 0:2071f8e463dd 65
mfajaradityo 0:2071f8e463dd 66 iWrite(1, BAUD_RATE, parameter);
mfajaradityo 0:2071f8e463dd 67 sendIPacket();
mfajaradityo 0:2071f8e463dd 68
mfajaradityo 0:2071f8e463dd 69 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 70 getRPacket();
mfajaradityo 0:2071f8e463dd 71 }
mfajaradityo 0:2071f8e463dd 72
mfajaradityo 0:2071f8e463dd 73 uint8_t XL320::GetBaudRate(uint8_t id)
mfajaradityo 0:2071f8e463dd 74 {
mfajaradityo 0:2071f8e463dd 75 pc.printf("Get BaudRate\n\r");
mfajaradityo 0:2071f8e463dd 76 targetID = id;
mfajaradityo 0:2071f8e463dd 77 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 78
mfajaradityo 0:2071f8e463dd 79 iRead(1, BAUD_RATE);
mfajaradityo 0:2071f8e463dd 80 sendIPacket();
mfajaradityo 0:2071f8e463dd 81
mfajaradityo 0:2071f8e463dd 82 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 83 getRPacket();
mfajaradityo 0:2071f8e463dd 84
mfajaradityo 0:2071f8e463dd 85 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 86
mfajaradityo 0:2071f8e463dd 87 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 88 {
mfajaradityo 0:2071f8e463dd 89 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 90 return rPacket[9]; //return tergeted servo Baud Rate
mfajaradityo 0:2071f8e463dd 91
mfajaradityo 0:2071f8e463dd 92 default : //error occurred
mfajaradityo 0:2071f8e463dd 93 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 94 }
mfajaradityo 0:2071f8e463dd 95 }
mfajaradityo 0:2071f8e463dd 96
mfajaradityo 0:2071f8e463dd 97 void XL320::SetRetDelTime(uint8_t id,uint8_t time)
mfajaradityo 0:2071f8e463dd 98 {
mfajaradityo 0:2071f8e463dd 99 pc.printf("Set Return Delay Time\n\r");
mfajaradityo 0:2071f8e463dd 100 targetID = id;
mfajaradityo 0:2071f8e463dd 101 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 102 parameter[0] = time;
mfajaradityo 0:2071f8e463dd 103
mfajaradityo 0:2071f8e463dd 104 iWrite(1, RETURN_DELAY_TIME, parameter);
mfajaradityo 0:2071f8e463dd 105 sendIPacket();
mfajaradityo 0:2071f8e463dd 106
mfajaradityo 0:2071f8e463dd 107 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 108 getRPacket();
mfajaradityo 0:2071f8e463dd 109 }
mfajaradityo 0:2071f8e463dd 110
mfajaradityo 0:2071f8e463dd 111 uint8_t XL320::GetRetDelTime(uint8_t id)
mfajaradityo 0:2071f8e463dd 112 {
mfajaradityo 0:2071f8e463dd 113 pc.printf("Get Return Delay Time\n\r");
mfajaradityo 0:2071f8e463dd 114 targetID = id;
mfajaradityo 0:2071f8e463dd 115 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 116
mfajaradityo 0:2071f8e463dd 117 iRead(1, RETURN_DELAY_TIME);
mfajaradityo 0:2071f8e463dd 118 sendIPacket();
mfajaradityo 0:2071f8e463dd 119
mfajaradityo 0:2071f8e463dd 120 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 121 getRPacket();
mfajaradityo 0:2071f8e463dd 122
mfajaradityo 0:2071f8e463dd 123 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 124
mfajaradityo 0:2071f8e463dd 125 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 126 {
mfajaradityo 0:2071f8e463dd 127 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 128 return rPacket[9]; //return tergeted servo Baud Rate
mfajaradityo 0:2071f8e463dd 129
mfajaradityo 0:2071f8e463dd 130 default : //error occurred
mfajaradityo 0:2071f8e463dd 131 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 132 }
mfajaradityo 0:2071f8e463dd 133 }
mfajaradityo 0:2071f8e463dd 134
mfajaradityo 0:2071f8e463dd 135 /***** XL320 Motor Setting *****/
mfajaradityo 0:2071f8e463dd 136 void XL320::SetCWAngLim(uint8_t id,uint16_t angle)
mfajaradityo 0:2071f8e463dd 137 {
mfajaradityo 0:2071f8e463dd 138 pc.printf("Set CW Angle Limit\n\r");
mfajaradityo 0:2071f8e463dd 139 targetID = id;
mfajaradityo 0:2071f8e463dd 140 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 141 parameter[0] = SHIFT_TO_LSB(angle);
mfajaradityo 0:2071f8e463dd 142 parameter[1] = SHIFT_TO_MSB(angle);
mfajaradityo 0:2071f8e463dd 143
mfajaradityo 0:2071f8e463dd 144 iWrite(2, CW_ANGLE_LIMIT, parameter);
mfajaradityo 0:2071f8e463dd 145 sendIPacket();
mfajaradityo 0:2071f8e463dd 146
mfajaradityo 0:2071f8e463dd 147 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 148 getRPacket();
mfajaradityo 0:2071f8e463dd 149 }
mfajaradityo 0:2071f8e463dd 150
mfajaradityo 0:2071f8e463dd 151 uint16_t XL320::GetCWAngLim(uint8_t id)
mfajaradityo 0:2071f8e463dd 152 {
mfajaradityo 0:2071f8e463dd 153 pc.printf("Get CW Angle Limit\n\r");
mfajaradityo 0:2071f8e463dd 154 targetID = id;
mfajaradityo 0:2071f8e463dd 155 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 156
mfajaradityo 0:2071f8e463dd 157 iRead(2, CW_ANGLE_LIMIT);
mfajaradityo 0:2071f8e463dd 158 sendIPacket();
mfajaradityo 0:2071f8e463dd 159
mfajaradityo 0:2071f8e463dd 160 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 161 getRPacket();
mfajaradityo 0:2071f8e463dd 162
mfajaradityo 0:2071f8e463dd 163 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 164
mfajaradityo 0:2071f8e463dd 165 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 166 {
mfajaradityo 0:2071f8e463dd 167 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 168 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 169
mfajaradityo 0:2071f8e463dd 170 default : //error occurred
mfajaradityo 0:2071f8e463dd 171 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 172 }
mfajaradityo 0:2071f8e463dd 173 }
mfajaradityo 0:2071f8e463dd 174
mfajaradityo 0:2071f8e463dd 175 void XL320::SetCCWAngLim(uint8_t id,uint16_t angle)
mfajaradityo 0:2071f8e463dd 176 {
mfajaradityo 0:2071f8e463dd 177 pc.printf("Set CCW Angle Limit\n\r");
mfajaradityo 0:2071f8e463dd 178 targetID = id;
mfajaradityo 0:2071f8e463dd 179 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 180 parameter[0] = SHIFT_TO_LSB(angle);
mfajaradityo 0:2071f8e463dd 181 parameter[1] = SHIFT_TO_MSB(angle);
mfajaradityo 0:2071f8e463dd 182
mfajaradityo 0:2071f8e463dd 183 iWrite(2, CCW_ANGLE_LIMIT, parameter);
mfajaradityo 0:2071f8e463dd 184 sendIPacket();
mfajaradityo 0:2071f8e463dd 185
mfajaradityo 0:2071f8e463dd 186 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 187 getRPacket();
mfajaradityo 0:2071f8e463dd 188 }
mfajaradityo 0:2071f8e463dd 189
mfajaradityo 0:2071f8e463dd 190 uint16_t XL320::GetCCWAngLim(uint8_t id)
mfajaradityo 0:2071f8e463dd 191 {
mfajaradityo 0:2071f8e463dd 192 pc.printf("Get CCW Angle Limit\n\r");
mfajaradityo 0:2071f8e463dd 193 targetID = id;
mfajaradityo 0:2071f8e463dd 194 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 195
mfajaradityo 0:2071f8e463dd 196 iRead(2, CW_ANGLE_LIMIT);
mfajaradityo 0:2071f8e463dd 197 sendIPacket();
mfajaradityo 0:2071f8e463dd 198
mfajaradityo 0:2071f8e463dd 199 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 200 getRPacket();
mfajaradityo 0:2071f8e463dd 201
mfajaradityo 0:2071f8e463dd 202 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 203
mfajaradityo 0:2071f8e463dd 204 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 205 {
mfajaradityo 0:2071f8e463dd 206 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 207 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 208
mfajaradityo 0:2071f8e463dd 209 default : //error occurred
mfajaradityo 0:2071f8e463dd 210 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 211 }
mfajaradityo 0:2071f8e463dd 212 }
mfajaradityo 0:2071f8e463dd 213
mfajaradityo 0:2071f8e463dd 214 void XL320::SetContMode(uint8_t id,uint8_t mode)
mfajaradityo 0:2071f8e463dd 215 {
mfajaradityo 0:2071f8e463dd 216 pc.printf("Set Control Mode \n\r");
mfajaradityo 0:2071f8e463dd 217 targetID = id;
mfajaradityo 0:2071f8e463dd 218 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 219 parameter[0] = mode;
mfajaradityo 0:2071f8e463dd 220
mfajaradityo 0:2071f8e463dd 221 iWrite(1, CONTROL_MODE, parameter);
mfajaradityo 0:2071f8e463dd 222 sendIPacket();
mfajaradityo 0:2071f8e463dd 223
mfajaradityo 0:2071f8e463dd 224 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 225 getRPacket();
mfajaradityo 0:2071f8e463dd 226 }
mfajaradityo 0:2071f8e463dd 227
mfajaradityo 0:2071f8e463dd 228 uint8_t XL320::GetContMode(uint8_t id)
mfajaradityo 0:2071f8e463dd 229 {
mfajaradityo 0:2071f8e463dd 230 pc.printf("Get Control Mode\n\r");
mfajaradityo 0:2071f8e463dd 231 targetID = id;
mfajaradityo 0:2071f8e463dd 232 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 233
mfajaradityo 0:2071f8e463dd 234 iRead(1, CONTROL_MODE);
mfajaradityo 0:2071f8e463dd 235 sendIPacket();
mfajaradityo 0:2071f8e463dd 236
mfajaradityo 0:2071f8e463dd 237 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 238 getRPacket();
mfajaradityo 0:2071f8e463dd 239
mfajaradityo 0:2071f8e463dd 240 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 241
mfajaradityo 0:2071f8e463dd 242 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 243 {
mfajaradityo 0:2071f8e463dd 244 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 245 return rPacket[9];
mfajaradityo 0:2071f8e463dd 246
mfajaradityo 0:2071f8e463dd 247 default : //error occurred
mfajaradityo 0:2071f8e463dd 248 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 249 }
mfajaradityo 0:2071f8e463dd 250 }
mfajaradityo 0:2071f8e463dd 251
mfajaradityo 0:2071f8e463dd 252 void XL320::SetTempLim(uint8_t id,uint8_t temp)
mfajaradityo 0:2071f8e463dd 253 {
mfajaradityo 0:2071f8e463dd 254 pc.printf("Set Temperature Limit\n\r");
mfajaradityo 0:2071f8e463dd 255 targetID = id;
mfajaradityo 0:2071f8e463dd 256 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 257 parameter[0] = temp;
mfajaradityo 0:2071f8e463dd 258
mfajaradityo 0:2071f8e463dd 259 iWrite(1, LIMIT_TEMPERATURE, parameter);
mfajaradityo 0:2071f8e463dd 260 sendIPacket();
mfajaradityo 0:2071f8e463dd 261
mfajaradityo 0:2071f8e463dd 262 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 263 getRPacket();
mfajaradityo 0:2071f8e463dd 264 }
mfajaradityo 0:2071f8e463dd 265
mfajaradityo 0:2071f8e463dd 266 uint8_t XL320::GetTempLim(uint8_t id)
mfajaradityo 0:2071f8e463dd 267 {
mfajaradityo 0:2071f8e463dd 268 pc.printf("Get Temperature Limit\n\r");
mfajaradityo 0:2071f8e463dd 269 targetID = id;
mfajaradityo 0:2071f8e463dd 270 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 271
mfajaradityo 0:2071f8e463dd 272 iRead(1, LIMIT_TEMPERATURE);
mfajaradityo 0:2071f8e463dd 273 sendIPacket();
mfajaradityo 0:2071f8e463dd 274
mfajaradityo 0:2071f8e463dd 275 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 276 getRPacket();
mfajaradityo 0:2071f8e463dd 277
mfajaradityo 0:2071f8e463dd 278 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 279
mfajaradityo 0:2071f8e463dd 280 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 281 {
mfajaradityo 0:2071f8e463dd 282 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 283 return rPacket[9];
mfajaradityo 0:2071f8e463dd 284
mfajaradityo 0:2071f8e463dd 285 default : //error occurred
mfajaradityo 0:2071f8e463dd 286 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 287 }
mfajaradityo 0:2071f8e463dd 288 }
mfajaradityo 0:2071f8e463dd 289
mfajaradityo 0:2071f8e463dd 290 void XL320::SetLowVoltLim(uint8_t id,uint8_t volt)
mfajaradityo 0:2071f8e463dd 291 {
mfajaradityo 0:2071f8e463dd 292 pc.printf("Set Lower Voltage Limit\n\r");
mfajaradityo 0:2071f8e463dd 293 targetID = id;
mfajaradityo 0:2071f8e463dd 294 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 295 parameter[0] = volt;
mfajaradityo 0:2071f8e463dd 296
mfajaradityo 0:2071f8e463dd 297 iWrite(1, LOWER_LIMIT_VOLTAGE, parameter);
mfajaradityo 0:2071f8e463dd 298 sendIPacket();
mfajaradityo 0:2071f8e463dd 299
mfajaradityo 0:2071f8e463dd 300 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 301 getRPacket();
mfajaradityo 0:2071f8e463dd 302 }
mfajaradityo 0:2071f8e463dd 303
mfajaradityo 0:2071f8e463dd 304 uint8_t XL320::GetLowVoltLim(uint8_t id)
mfajaradityo 0:2071f8e463dd 305 {
mfajaradityo 0:2071f8e463dd 306 pc.printf("Get Lower Voltage Limit\n\r");
mfajaradityo 0:2071f8e463dd 307 targetID = id;
mfajaradityo 0:2071f8e463dd 308 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 309
mfajaradityo 0:2071f8e463dd 310 iRead(1, LOWER_LIMIT_VOLTAGE);
mfajaradityo 0:2071f8e463dd 311 sendIPacket();
mfajaradityo 0:2071f8e463dd 312
mfajaradityo 0:2071f8e463dd 313 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 314 getRPacket();
mfajaradityo 0:2071f8e463dd 315
mfajaradityo 0:2071f8e463dd 316 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 317
mfajaradityo 0:2071f8e463dd 318 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 319 {
mfajaradityo 0:2071f8e463dd 320 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 321 return rPacket[9];
mfajaradityo 0:2071f8e463dd 322
mfajaradityo 0:2071f8e463dd 323 default : //error occurred
mfajaradityo 0:2071f8e463dd 324 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 325 }
mfajaradityo 0:2071f8e463dd 326 }
mfajaradityo 0:2071f8e463dd 327
mfajaradityo 0:2071f8e463dd 328 void XL320::SetUpVoltLim(uint8_t id,uint8_t volt)
mfajaradityo 0:2071f8e463dd 329 {
mfajaradityo 0:2071f8e463dd 330 pc.printf("Set Upper Voltage Limit\n\r");
mfajaradityo 0:2071f8e463dd 331 targetID = id;
mfajaradityo 0:2071f8e463dd 332 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 333 parameter[0] = volt;
mfajaradityo 0:2071f8e463dd 334
mfajaradityo 0:2071f8e463dd 335 iWrite(1, UPPER_LIMIT_VOLTAGE, parameter);
mfajaradityo 0:2071f8e463dd 336 sendIPacket();
mfajaradityo 0:2071f8e463dd 337
mfajaradityo 0:2071f8e463dd 338 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 339 getRPacket();
mfajaradityo 0:2071f8e463dd 340 }
mfajaradityo 0:2071f8e463dd 341
mfajaradityo 0:2071f8e463dd 342 uint8_t XL320::GetUpVoltLim(uint8_t id)
mfajaradityo 0:2071f8e463dd 343 {
mfajaradityo 0:2071f8e463dd 344 pc.printf("Get Upper Voltage Limit\n\r");
mfajaradityo 0:2071f8e463dd 345 targetID = id;
mfajaradityo 0:2071f8e463dd 346 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 347
mfajaradityo 0:2071f8e463dd 348 iRead(1, UPPER_LIMIT_VOLTAGE);
mfajaradityo 0:2071f8e463dd 349 sendIPacket();
mfajaradityo 0:2071f8e463dd 350
mfajaradityo 0:2071f8e463dd 351 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 352 getRPacket();
mfajaradityo 0:2071f8e463dd 353
mfajaradityo 0:2071f8e463dd 354 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 355
mfajaradityo 0:2071f8e463dd 356 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 357 {
mfajaradityo 0:2071f8e463dd 358 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 359 return rPacket[9];
mfajaradityo 0:2071f8e463dd 360
mfajaradityo 0:2071f8e463dd 361 default : //error occurred
mfajaradityo 0:2071f8e463dd 362 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 363 }
mfajaradityo 0:2071f8e463dd 364 }
mfajaradityo 0:2071f8e463dd 365
mfajaradityo 0:2071f8e463dd 366
mfajaradityo 0:2071f8e463dd 367 void XL320::SetMaxTorq(uint8_t id,uint16_t torque)
mfajaradityo 0:2071f8e463dd 368 {
mfajaradityo 0:2071f8e463dd 369 pc.printf("Set Maximum Torque\n\r");
mfajaradityo 0:2071f8e463dd 370 targetID = id;
mfajaradityo 0:2071f8e463dd 371 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 372 parameter[0] = SHIFT_TO_LSB(torque);
mfajaradityo 0:2071f8e463dd 373 parameter[1] = SHIFT_TO_MSB(torque);
mfajaradityo 0:2071f8e463dd 374
mfajaradityo 0:2071f8e463dd 375 iWrite(2, MAX_TORQUE, parameter);
mfajaradityo 0:2071f8e463dd 376 sendIPacket();
mfajaradityo 0:2071f8e463dd 377
mfajaradityo 0:2071f8e463dd 378 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 379 getRPacket();
mfajaradityo 0:2071f8e463dd 380 }
mfajaradityo 0:2071f8e463dd 381
mfajaradityo 0:2071f8e463dd 382 uint16_t XL320::GetMaxTorq(uint8_t id)
mfajaradityo 0:2071f8e463dd 383 {
mfajaradityo 0:2071f8e463dd 384 pc.printf("Get Maximum Torque\n\r");
mfajaradityo 0:2071f8e463dd 385 targetID = id;
mfajaradityo 0:2071f8e463dd 386 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 387
mfajaradityo 0:2071f8e463dd 388 iRead(2, MAX_TORQUE);
mfajaradityo 0:2071f8e463dd 389 sendIPacket();
mfajaradityo 0:2071f8e463dd 390
mfajaradityo 0:2071f8e463dd 391 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 392 getRPacket();
mfajaradityo 0:2071f8e463dd 393
mfajaradityo 0:2071f8e463dd 394 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 395
mfajaradityo 0:2071f8e463dd 396 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 397 {
mfajaradityo 0:2071f8e463dd 398 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 399 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 400
mfajaradityo 0:2071f8e463dd 401 default : //error occurred
mfajaradityo 0:2071f8e463dd 402 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 403 }
mfajaradityo 0:2071f8e463dd 404 }
mfajaradityo 0:2071f8e463dd 405
mfajaradityo 0:2071f8e463dd 406 void XL320::SetRetLev(uint8_t id,uint8_t level)
mfajaradityo 0:2071f8e463dd 407 {
mfajaradityo 0:2071f8e463dd 408 pc.printf("Set Return Level\n\r");
mfajaradityo 0:2071f8e463dd 409 targetID = id;
mfajaradityo 0:2071f8e463dd 410 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 411 parameter[0] = level;
mfajaradityo 0:2071f8e463dd 412
mfajaradityo 0:2071f8e463dd 413 iWrite(1, RETURN_LEVEL, parameter);
mfajaradityo 0:2071f8e463dd 414 sendIPacket();
mfajaradityo 0:2071f8e463dd 415
mfajaradityo 0:2071f8e463dd 416 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 417 getRPacket();
mfajaradityo 0:2071f8e463dd 418 }
mfajaradityo 0:2071f8e463dd 419
mfajaradityo 0:2071f8e463dd 420 uint8_t XL320::GetRetLev(uint8_t id)
mfajaradityo 0:2071f8e463dd 421 {
mfajaradityo 0:2071f8e463dd 422 pc.printf("Get Return Level\n\r");
mfajaradityo 0:2071f8e463dd 423 targetID = id;
mfajaradityo 0:2071f8e463dd 424 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 425
mfajaradityo 0:2071f8e463dd 426 iRead(1, RETURN_LEVEL);
mfajaradityo 0:2071f8e463dd 427 sendIPacket();
mfajaradityo 0:2071f8e463dd 428
mfajaradityo 0:2071f8e463dd 429 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 430 getRPacket();
mfajaradityo 0:2071f8e463dd 431
mfajaradityo 0:2071f8e463dd 432 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 433
mfajaradityo 0:2071f8e463dd 434 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 435 {
mfajaradityo 0:2071f8e463dd 436 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 437 return rPacket[9];
mfajaradityo 0:2071f8e463dd 438
mfajaradityo 0:2071f8e463dd 439 default : //error occurred
mfajaradityo 0:2071f8e463dd 440 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 441 }
mfajaradityo 0:2071f8e463dd 442 }
mfajaradityo 0:2071f8e463dd 443
mfajaradityo 0:2071f8e463dd 444 void XL320::SetAlarmShut(uint8_t id,uint8_t alarm)
mfajaradityo 0:2071f8e463dd 445 {
mfajaradityo 0:2071f8e463dd 446 pc.printf("Set Alarm Shutdown\n\r");
mfajaradityo 0:2071f8e463dd 447 targetID = id;
mfajaradityo 0:2071f8e463dd 448 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 449 parameter[0] = alarm;
mfajaradityo 0:2071f8e463dd 450
mfajaradityo 0:2071f8e463dd 451 iWrite(1, ALARM_SHUTDOWN, parameter);
mfajaradityo 0:2071f8e463dd 452 sendIPacket();
mfajaradityo 0:2071f8e463dd 453
mfajaradityo 0:2071f8e463dd 454 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 455 getRPacket();
mfajaradityo 0:2071f8e463dd 456 }
mfajaradityo 0:2071f8e463dd 457
mfajaradityo 0:2071f8e463dd 458 uint8_t XL320::GetAlarmShut(uint8_t id)
mfajaradityo 0:2071f8e463dd 459 {
mfajaradityo 0:2071f8e463dd 460 pc.printf("Get Alarm Shutdown\n\r");
mfajaradityo 0:2071f8e463dd 461 targetID = id;
mfajaradityo 0:2071f8e463dd 462 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 463
mfajaradityo 0:2071f8e463dd 464 iRead(1, ALARM_SHUTDOWN);
mfajaradityo 0:2071f8e463dd 465 sendIPacket();
mfajaradityo 0:2071f8e463dd 466
mfajaradityo 0:2071f8e463dd 467 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 468 getRPacket();
mfajaradityo 0:2071f8e463dd 469
mfajaradityo 0:2071f8e463dd 470 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 471
mfajaradityo 0:2071f8e463dd 472 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 473 {
mfajaradityo 0:2071f8e463dd 474 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 475 return rPacket[9];
mfajaradityo 0:2071f8e463dd 476
mfajaradityo 0:2071f8e463dd 477 default : //error occurred
mfajaradityo 0:2071f8e463dd 478 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 479 }
mfajaradityo 0:2071f8e463dd 480 }
mfajaradityo 0:2071f8e463dd 481
mfajaradityo 0:2071f8e463dd 482 ////// RAM ACCESS METHODS //////
mfajaradityo 0:2071f8e463dd 483 /***** XL320 On/Off *****/
mfajaradityo 0:2071f8e463dd 484 void XL320::SetTorqueEn(uint8_t id,uint8_t enable)
mfajaradityo 0:2071f8e463dd 485 {
mfajaradityo 0:2071f8e463dd 486 pc.printf("Set Torque Enable\n\r");
mfajaradityo 0:2071f8e463dd 487 targetID = id;
mfajaradityo 0:2071f8e463dd 488 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 489 parameter[0] = enable;
mfajaradityo 0:2071f8e463dd 490
mfajaradityo 0:2071f8e463dd 491 iWrite(1, TORQUE_ENABLE, parameter);
mfajaradityo 0:2071f8e463dd 492 sendIPacket();
mfajaradityo 0:2071f8e463dd 493
mfajaradityo 0:2071f8e463dd 494 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 495 getRPacket();
mfajaradityo 0:2071f8e463dd 496 }
mfajaradityo 0:2071f8e463dd 497
mfajaradityo 0:2071f8e463dd 498 uint8_t XL320::GetTorqueEn(uint8_t id)
mfajaradityo 0:2071f8e463dd 499 {
mfajaradityo 0:2071f8e463dd 500 pc.printf("Get Torque Enable\n\r");
mfajaradityo 0:2071f8e463dd 501 targetID = id;
mfajaradityo 0:2071f8e463dd 502 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 503
mfajaradityo 0:2071f8e463dd 504 iRead(1, TORQUE_ENABLE);
mfajaradityo 0:2071f8e463dd 505 sendIPacket();
mfajaradityo 0:2071f8e463dd 506
mfajaradityo 0:2071f8e463dd 507 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 508 getRPacket();
mfajaradityo 0:2071f8e463dd 509
mfajaradityo 0:2071f8e463dd 510 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 511
mfajaradityo 0:2071f8e463dd 512 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 513 {
mfajaradityo 0:2071f8e463dd 514 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 515 return rPacket[9];
mfajaradityo 0:2071f8e463dd 516
mfajaradityo 0:2071f8e463dd 517 default : //error occurred
mfajaradityo 0:2071f8e463dd 518 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 519 }
mfajaradityo 0:2071f8e463dd 520 }
mfajaradityo 0:2071f8e463dd 521
mfajaradityo 0:2071f8e463dd 522 void XL320::TurnOnLED(uint8_t id,uint8_t led)
mfajaradityo 0:2071f8e463dd 523 {
mfajaradityo 0:2071f8e463dd 524 pc.printf("Turn On LED\n\r");
mfajaradityo 0:2071f8e463dd 525 targetID = id; //XL320 ID
mfajaradityo 0:2071f8e463dd 526 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 527 parameter[0] = led; //led to turn on
mfajaradityo 0:2071f8e463dd 528
mfajaradityo 0:2071f8e463dd 529 iWrite(1, LED, parameter);
mfajaradityo 0:2071f8e463dd 530 sendIPacket();
mfajaradityo 0:2071f8e463dd 531
mfajaradityo 0:2071f8e463dd 532 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 533 getRPacket();
mfajaradityo 0:2071f8e463dd 534
mfajaradityo 0:2071f8e463dd 535 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 536 }
mfajaradityo 0:2071f8e463dd 537
mfajaradityo 0:2071f8e463dd 538 uint8_t XL320::GetStatusLED(uint8_t id)
mfajaradityo 0:2071f8e463dd 539 {
mfajaradityo 0:2071f8e463dd 540 pc.printf("Get Status LED\n\r");
mfajaradityo 0:2071f8e463dd 541 targetID = id;
mfajaradityo 0:2071f8e463dd 542 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 543
mfajaradityo 0:2071f8e463dd 544 iRead(1, LED);
mfajaradityo 0:2071f8e463dd 545 sendIPacket();
mfajaradityo 0:2071f8e463dd 546
mfajaradityo 0:2071f8e463dd 547 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 548 getRPacket();
mfajaradityo 0:2071f8e463dd 549
mfajaradityo 0:2071f8e463dd 550 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 551
mfajaradityo 0:2071f8e463dd 552 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 553 {
mfajaradityo 0:2071f8e463dd 554 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 555 return rPacket[9];
mfajaradityo 0:2071f8e463dd 556
mfajaradityo 0:2071f8e463dd 557 default : //error occurred
mfajaradityo 0:2071f8e463dd 558 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 559 }
mfajaradityo 0:2071f8e463dd 560 }
mfajaradityo 0:2071f8e463dd 561
mfajaradityo 0:2071f8e463dd 562 /***** XL320 Motor Control *****/
mfajaradityo 0:2071f8e463dd 563 void XL320::SetDGain(uint8_t id,uint8_t d_cons)
mfajaradityo 0:2071f8e463dd 564 {
mfajaradityo 0:2071f8e463dd 565 pc.printf("Set D-Gain\n\r");
mfajaradityo 0:2071f8e463dd 566 targetID = id;
mfajaradityo 0:2071f8e463dd 567 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 568 parameter[0] = d_cons;
mfajaradityo 0:2071f8e463dd 569
mfajaradityo 0:2071f8e463dd 570 iWrite(1, D_GAIN, parameter);
mfajaradityo 0:2071f8e463dd 571 sendIPacket();
mfajaradityo 0:2071f8e463dd 572
mfajaradityo 0:2071f8e463dd 573 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 574 getRPacket();
mfajaradityo 0:2071f8e463dd 575
mfajaradityo 0:2071f8e463dd 576 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 577 }
mfajaradityo 0:2071f8e463dd 578
mfajaradityo 0:2071f8e463dd 579 uint8_t XL320::GetDGain(uint8_t id)
mfajaradityo 0:2071f8e463dd 580 {
mfajaradityo 0:2071f8e463dd 581 pc.printf("Get D-Gain\n\r");
mfajaradityo 0:2071f8e463dd 582 targetID = id;
mfajaradityo 0:2071f8e463dd 583 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 584
mfajaradityo 0:2071f8e463dd 585 iRead(1, D_GAIN);
mfajaradityo 0:2071f8e463dd 586 sendIPacket();
mfajaradityo 0:2071f8e463dd 587
mfajaradityo 0:2071f8e463dd 588 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 589 getRPacket();
mfajaradityo 0:2071f8e463dd 590
mfajaradityo 0:2071f8e463dd 591 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 592
mfajaradityo 0:2071f8e463dd 593 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 594 {
mfajaradityo 0:2071f8e463dd 595 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 596 return rPacket[9];
mfajaradityo 0:2071f8e463dd 597
mfajaradityo 0:2071f8e463dd 598 default : //error occurred
mfajaradityo 0:2071f8e463dd 599 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 600 }
mfajaradityo 0:2071f8e463dd 601 }
mfajaradityo 0:2071f8e463dd 602
mfajaradityo 0:2071f8e463dd 603 void XL320::SetIGain(uint8_t id,uint8_t i_cons)
mfajaradityo 0:2071f8e463dd 604 {
mfajaradityo 0:2071f8e463dd 605 pc.printf("Set I-Gain\n\r");
mfajaradityo 0:2071f8e463dd 606 targetID = id;
mfajaradityo 0:2071f8e463dd 607 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 608 parameter[0] = i_cons;
mfajaradityo 0:2071f8e463dd 609
mfajaradityo 0:2071f8e463dd 610 iWrite(1, I_GAIN, parameter);
mfajaradityo 0:2071f8e463dd 611 sendIPacket();
mfajaradityo 0:2071f8e463dd 612
mfajaradityo 0:2071f8e463dd 613 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 614 getRPacket();
mfajaradityo 0:2071f8e463dd 615
mfajaradityo 0:2071f8e463dd 616 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 617 }
mfajaradityo 0:2071f8e463dd 618
mfajaradityo 0:2071f8e463dd 619 uint8_t XL320::GetIGain(uint8_t id)
mfajaradityo 0:2071f8e463dd 620 {
mfajaradityo 0:2071f8e463dd 621 pc.printf("Get I-Gain\n\r");
mfajaradityo 0:2071f8e463dd 622 targetID = id;
mfajaradityo 0:2071f8e463dd 623 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 624
mfajaradityo 0:2071f8e463dd 625 iRead(1, I_GAIN);
mfajaradityo 0:2071f8e463dd 626 sendIPacket();
mfajaradityo 0:2071f8e463dd 627
mfajaradityo 0:2071f8e463dd 628 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 629 getRPacket();
mfajaradityo 0:2071f8e463dd 630
mfajaradityo 0:2071f8e463dd 631 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 632
mfajaradityo 0:2071f8e463dd 633 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 634 {
mfajaradityo 0:2071f8e463dd 635 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 636 return rPacket[9];
mfajaradityo 0:2071f8e463dd 637
mfajaradityo 0:2071f8e463dd 638 default : //error occurred
mfajaradityo 0:2071f8e463dd 639 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 640 }
mfajaradityo 0:2071f8e463dd 641 }
mfajaradityo 0:2071f8e463dd 642
mfajaradityo 0:2071f8e463dd 643 void XL320::SetPGain(uint8_t id,uint8_t p_cons)
mfajaradityo 0:2071f8e463dd 644 {
mfajaradityo 0:2071f8e463dd 645 pc.printf("Set P-Gain\n\r");
mfajaradityo 0:2071f8e463dd 646 targetID = id;
mfajaradityo 0:2071f8e463dd 647 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 648 parameter[0] = p_cons;
mfajaradityo 0:2071f8e463dd 649
mfajaradityo 0:2071f8e463dd 650 iWrite(1, P_GAIN, parameter);
mfajaradityo 0:2071f8e463dd 651 sendIPacket();
mfajaradityo 0:2071f8e463dd 652
mfajaradityo 0:2071f8e463dd 653 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 654 getRPacket();
mfajaradityo 0:2071f8e463dd 655
mfajaradityo 0:2071f8e463dd 656 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 657 }
mfajaradityo 0:2071f8e463dd 658
mfajaradityo 0:2071f8e463dd 659 uint8_t XL320::GetPGain(uint8_t id)
mfajaradityo 0:2071f8e463dd 660 {
mfajaradityo 0:2071f8e463dd 661 pc.printf("Get P-Gain\n\r");
mfajaradityo 0:2071f8e463dd 662 targetID = id;
mfajaradityo 0:2071f8e463dd 663 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 664
mfajaradityo 0:2071f8e463dd 665 iRead(1, P_GAIN);
mfajaradityo 0:2071f8e463dd 666 sendIPacket();
mfajaradityo 0:2071f8e463dd 667
mfajaradityo 0:2071f8e463dd 668 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 669 getRPacket();
mfajaradityo 0:2071f8e463dd 670
mfajaradityo 0:2071f8e463dd 671 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 672
mfajaradityo 0:2071f8e463dd 673 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 674 {
mfajaradityo 0:2071f8e463dd 675 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 676 return rPacket[9];
mfajaradityo 0:2071f8e463dd 677
mfajaradityo 0:2071f8e463dd 678 default : //error occurred
mfajaradityo 0:2071f8e463dd 679 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 680 }
mfajaradityo 0:2071f8e463dd 681 }
mfajaradityo 0:2071f8e463dd 682
mfajaradityo 0:2071f8e463dd 683 void XL320::SetGoalPos(uint8_t id,uint16_t position)
mfajaradityo 0:2071f8e463dd 684 {
mfajaradityo 0:2071f8e463dd 685 pc.printf("Set Goal Position\n\r");
mfajaradityo 0:2071f8e463dd 686 targetID = id; //XL320 ID
mfajaradityo 0:2071f8e463dd 687 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 688 parameter[0] = SHIFT_TO_LSB(position); //position goal LSB
mfajaradityo 0:2071f8e463dd 689 parameter[1] = SHIFT_TO_MSB(position); //position goal MSB
mfajaradityo 0:2071f8e463dd 690
mfajaradityo 0:2071f8e463dd 691 iWrite(2, GOAL_POSITION, parameter);
mfajaradityo 0:2071f8e463dd 692 sendIPacket();
mfajaradityo 0:2071f8e463dd 693
mfajaradityo 0:2071f8e463dd 694 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 695 getRPacket();
mfajaradityo 0:2071f8e463dd 696
mfajaradityo 0:2071f8e463dd 697 wait_ms(restTime); //gives some time to the targeted servo before another Instruction Packet
mfajaradityo 0:2071f8e463dd 698 }
mfajaradityo 0:2071f8e463dd 699
mfajaradityo 0:2071f8e463dd 700 uint16_t XL320::GetGoalPos(uint8_t id)
mfajaradityo 0:2071f8e463dd 701 {
mfajaradityo 0:2071f8e463dd 702 pc.printf("Get Goal Position\n\r");
mfajaradityo 0:2071f8e463dd 703 targetID = id;
mfajaradityo 0:2071f8e463dd 704 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 705
mfajaradityo 0:2071f8e463dd 706 iRead(2, GOAL_POSITION);
mfajaradityo 0:2071f8e463dd 707 sendIPacket();
mfajaradityo 0:2071f8e463dd 708
mfajaradityo 0:2071f8e463dd 709 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 710 getRPacket();
mfajaradityo 0:2071f8e463dd 711
mfajaradityo 0:2071f8e463dd 712 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 713
mfajaradityo 0:2071f8e463dd 714 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 715 {
mfajaradityo 0:2071f8e463dd 716 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 717 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 718
mfajaradityo 0:2071f8e463dd 719 default : //error occurred
mfajaradityo 0:2071f8e463dd 720 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 721 }
mfajaradityo 0:2071f8e463dd 722 }
mfajaradityo 0:2071f8e463dd 723
mfajaradityo 0:2071f8e463dd 724 void XL320::SetGoalVel(uint8_t id,uint16_t velocity)
mfajaradityo 0:2071f8e463dd 725 {
mfajaradityo 0:2071f8e463dd 726 pc.printf("Set Goal Velocity\n\r");
mfajaradityo 0:2071f8e463dd 727 targetID = id; //XL320 ID
mfajaradityo 0:2071f8e463dd 728 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 729 parameter[0] = SHIFT_TO_LSB(velocity);
mfajaradityo 0:2071f8e463dd 730 parameter[1] = SHIFT_TO_MSB(velocity);
mfajaradityo 0:2071f8e463dd 731
mfajaradityo 0:2071f8e463dd 732 iWrite(2, GOAL_SPEED, parameter);
mfajaradityo 0:2071f8e463dd 733 sendIPacket();
mfajaradityo 0:2071f8e463dd 734
mfajaradityo 0:2071f8e463dd 735 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 736 getRPacket();
mfajaradityo 0:2071f8e463dd 737
mfajaradityo 0:2071f8e463dd 738 wait_ms(restTime); //gives some time to the targeted servo before another Instruction Packet
mfajaradityo 0:2071f8e463dd 739 }
mfajaradityo 0:2071f8e463dd 740
mfajaradityo 0:2071f8e463dd 741 uint16_t XL320::GetGoalVel(uint8_t id)
mfajaradityo 0:2071f8e463dd 742 {
mfajaradityo 0:2071f8e463dd 743 pc.printf("Get Goal Velocity\n\r");
mfajaradityo 0:2071f8e463dd 744 targetID = id;
mfajaradityo 0:2071f8e463dd 745 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 746
mfajaradityo 0:2071f8e463dd 747 iRead(2, GOAL_SPEED);
mfajaradityo 0:2071f8e463dd 748 sendIPacket();
mfajaradityo 0:2071f8e463dd 749
mfajaradityo 0:2071f8e463dd 750 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 751 getRPacket();
mfajaradityo 0:2071f8e463dd 752
mfajaradityo 0:2071f8e463dd 753 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 754
mfajaradityo 0:2071f8e463dd 755 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 756 {
mfajaradityo 0:2071f8e463dd 757 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 758 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 759
mfajaradityo 0:2071f8e463dd 760 default : //error occurred
mfajaradityo 0:2071f8e463dd 761 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 762 }
mfajaradityo 0:2071f8e463dd 763 }
mfajaradityo 0:2071f8e463dd 764
mfajaradityo 0:2071f8e463dd 765 void XL320::SetGoalTorq(uint8_t id,uint16_t torque)
mfajaradityo 0:2071f8e463dd 766 {
mfajaradityo 0:2071f8e463dd 767 pc.printf("Set Goal Torque\n\r");
mfajaradityo 0:2071f8e463dd 768 targetID = id; //XL320 ID
mfajaradityo 0:2071f8e463dd 769 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 770 parameter[0] = SHIFT_TO_LSB(torque);
mfajaradityo 0:2071f8e463dd 771 parameter[1] = SHIFT_TO_MSB(torque);
mfajaradityo 0:2071f8e463dd 772
mfajaradityo 0:2071f8e463dd 773 iWrite(2, GOAL_TORQUE, parameter);
mfajaradityo 0:2071f8e463dd 774 sendIPacket();
mfajaradityo 0:2071f8e463dd 775
mfajaradityo 0:2071f8e463dd 776 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 777 getRPacket();
mfajaradityo 0:2071f8e463dd 778
mfajaradityo 0:2071f8e463dd 779 wait_ms(restTime); //gives some time to the targeted servo before another Instruction Packet
mfajaradityo 0:2071f8e463dd 780 }
mfajaradityo 0:2071f8e463dd 781
mfajaradityo 0:2071f8e463dd 782 uint16_t XL320::GetGoalTorq(uint8_t id)
mfajaradityo 0:2071f8e463dd 783 {
mfajaradityo 0:2071f8e463dd 784 pc.printf("Get Goal Torque\n\r");
mfajaradityo 0:2071f8e463dd 785 targetID = id;
mfajaradityo 0:2071f8e463dd 786 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 787
mfajaradityo 0:2071f8e463dd 788 iRead(2, GOAL_TORQUE);
mfajaradityo 0:2071f8e463dd 789 sendIPacket();
mfajaradityo 0:2071f8e463dd 790
mfajaradityo 0:2071f8e463dd 791 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 792 getRPacket();
mfajaradityo 0:2071f8e463dd 793
mfajaradityo 0:2071f8e463dd 794 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 795
mfajaradityo 0:2071f8e463dd 796 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 797 {
mfajaradityo 0:2071f8e463dd 798 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 799 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 800
mfajaradityo 0:2071f8e463dd 801 default : //error occurred
mfajaradityo 0:2071f8e463dd 802 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 803 }
mfajaradityo 0:2071f8e463dd 804 }
mfajaradityo 0:2071f8e463dd 805
mfajaradityo 0:2071f8e463dd 806 /***** XL320 Feedback *****/
mfajaradityo 0:2071f8e463dd 807 uint16_t XL320::GetPresentPos(uint8_t id)
mfajaradityo 0:2071f8e463dd 808 {
mfajaradityo 0:2071f8e463dd 809 pc.printf("Get Present Position\n\r");
mfajaradityo 0:2071f8e463dd 810 targetID = id;
mfajaradityo 0:2071f8e463dd 811 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 812
mfajaradityo 0:2071f8e463dd 813 iRead(2, PRESENT_POSITION);
mfajaradityo 0:2071f8e463dd 814 sendIPacket();
mfajaradityo 0:2071f8e463dd 815
mfajaradityo 0:2071f8e463dd 816 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 817 getRPacket();
mfajaradityo 0:2071f8e463dd 818
mfajaradityo 0:2071f8e463dd 819 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 820
mfajaradityo 0:2071f8e463dd 821 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 822 {
mfajaradityo 0:2071f8e463dd 823 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 824 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 825
mfajaradityo 0:2071f8e463dd 826 default : //error occurred
mfajaradityo 0:2071f8e463dd 827 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 828 }
mfajaradityo 0:2071f8e463dd 829 }
mfajaradityo 0:2071f8e463dd 830
mfajaradityo 0:2071f8e463dd 831 uint16_t XL320::GetPresentSpeed(uint8_t id)
mfajaradityo 0:2071f8e463dd 832 {
mfajaradityo 0:2071f8e463dd 833 pc.printf("Get Present Speed\n\r");
mfajaradityo 0:2071f8e463dd 834 targetID = id;
mfajaradityo 0:2071f8e463dd 835 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 836
mfajaradityo 0:2071f8e463dd 837 iRead(2, PRESENT_SPEED);
mfajaradityo 0:2071f8e463dd 838 sendIPacket();
mfajaradityo 0:2071f8e463dd 839
mfajaradityo 0:2071f8e463dd 840 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 841 getRPacket();
mfajaradityo 0:2071f8e463dd 842
mfajaradityo 0:2071f8e463dd 843 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 844
mfajaradityo 0:2071f8e463dd 845 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 846 {
mfajaradityo 0:2071f8e463dd 847 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 848 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 849
mfajaradityo 0:2071f8e463dd 850 default : //error occurred
mfajaradityo 0:2071f8e463dd 851 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 852 }
mfajaradityo 0:2071f8e463dd 853 }
mfajaradityo 0:2071f8e463dd 854
mfajaradityo 0:2071f8e463dd 855 uint16_t XL320::GetPresentLoad(uint8_t id)
mfajaradityo 0:2071f8e463dd 856 {
mfajaradityo 0:2071f8e463dd 857 pc.printf("Get Present Load\n\r");
mfajaradityo 0:2071f8e463dd 858 targetID = id;
mfajaradityo 0:2071f8e463dd 859 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 860
mfajaradityo 0:2071f8e463dd 861 iRead(2, PRESENT_LOAD);
mfajaradityo 0:2071f8e463dd 862 sendIPacket();
mfajaradityo 0:2071f8e463dd 863
mfajaradityo 0:2071f8e463dd 864 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 865 getRPacket();
mfajaradityo 0:2071f8e463dd 866
mfajaradityo 0:2071f8e463dd 867 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 868
mfajaradityo 0:2071f8e463dd 869 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 870 {
mfajaradityo 0:2071f8e463dd 871 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 872 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 873
mfajaradityo 0:2071f8e463dd 874 default : //error occurred
mfajaradityo 0:2071f8e463dd 875 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 876 }
mfajaradityo 0:2071f8e463dd 877 }
mfajaradityo 0:2071f8e463dd 878
mfajaradityo 0:2071f8e463dd 879 uint8_t XL320::GetPresentVolt(uint8_t id)
mfajaradityo 0:2071f8e463dd 880 {
mfajaradityo 0:2071f8e463dd 881 pc.printf("Get Present Volt\n\r");
mfajaradityo 0:2071f8e463dd 882 targetID = id;
mfajaradityo 0:2071f8e463dd 883 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 884
mfajaradityo 0:2071f8e463dd 885 iRead(1, PRESENT_VOLTAGE);
mfajaradityo 0:2071f8e463dd 886 sendIPacket();
mfajaradityo 0:2071f8e463dd 887
mfajaradityo 0:2071f8e463dd 888 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 889 getRPacket();
mfajaradityo 0:2071f8e463dd 890
mfajaradityo 0:2071f8e463dd 891 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 892
mfajaradityo 0:2071f8e463dd 893 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 894 {
mfajaradityo 0:2071f8e463dd 895 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 896 return rPacket[9];
mfajaradityo 0:2071f8e463dd 897
mfajaradityo 0:2071f8e463dd 898 default : //error occurred
mfajaradityo 0:2071f8e463dd 899 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 900 }
mfajaradityo 0:2071f8e463dd 901 }
mfajaradityo 0:2071f8e463dd 902
mfajaradityo 0:2071f8e463dd 903 uint8_t XL320::GetPresentTemp(uint8_t id)
mfajaradityo 0:2071f8e463dd 904 {
mfajaradityo 0:2071f8e463dd 905 pc.printf("Get Present Temperature\n\r");
mfajaradityo 0:2071f8e463dd 906 targetID = id;
mfajaradityo 0:2071f8e463dd 907 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 908
mfajaradityo 0:2071f8e463dd 909 iRead(1, PRESENT_TEMPERATURE);
mfajaradityo 0:2071f8e463dd 910 sendIPacket();
mfajaradityo 0:2071f8e463dd 911
mfajaradityo 0:2071f8e463dd 912 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 913 getRPacket();
mfajaradityo 0:2071f8e463dd 914
mfajaradityo 0:2071f8e463dd 915 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 916
mfajaradityo 0:2071f8e463dd 917 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 918 {
mfajaradityo 0:2071f8e463dd 919 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 920 return rPacket[9];
mfajaradityo 0:2071f8e463dd 921
mfajaradityo 0:2071f8e463dd 922 default : //error occurred
mfajaradityo 0:2071f8e463dd 923 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 924 }
mfajaradityo 0:2071f8e463dd 925 }
mfajaradityo 0:2071f8e463dd 926
mfajaradityo 0:2071f8e463dd 927 /***** XL320 Status *****/
mfajaradityo 0:2071f8e463dd 928 uint8_t XL320::GetRegInst(uint8_t id)
mfajaradityo 0:2071f8e463dd 929 {
mfajaradityo 0:2071f8e463dd 930 pc.printf("Get Registered Instruction\n\r");
mfajaradityo 0:2071f8e463dd 931 targetID = id;
mfajaradityo 0:2071f8e463dd 932 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 933
mfajaradityo 0:2071f8e463dd 934 iRead(1, REGISTERED_INSTRUCTION);
mfajaradityo 0:2071f8e463dd 935 sendIPacket();
mfajaradityo 0:2071f8e463dd 936
mfajaradityo 0:2071f8e463dd 937 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 938 getRPacket();
mfajaradityo 0:2071f8e463dd 939
mfajaradityo 0:2071f8e463dd 940 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 941
mfajaradityo 0:2071f8e463dd 942 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 943 {
mfajaradityo 0:2071f8e463dd 944 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 945 return rPacket[9];
mfajaradityo 0:2071f8e463dd 946
mfajaradityo 0:2071f8e463dd 947 default : //error occurred
mfajaradityo 0:2071f8e463dd 948 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 949 }
mfajaradityo 0:2071f8e463dd 950 }
mfajaradityo 0:2071f8e463dd 951
mfajaradityo 0:2071f8e463dd 952 uint8_t XL320::GetMoving(uint8_t id)
mfajaradityo 0:2071f8e463dd 953 {
mfajaradityo 0:2071f8e463dd 954 pc.printf("Get Moving\n\r");
mfajaradityo 0:2071f8e463dd 955 targetID = id;
mfajaradityo 0:2071f8e463dd 956 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 957
mfajaradityo 0:2071f8e463dd 958 iRead(1, MOVING);
mfajaradityo 0:2071f8e463dd 959 sendIPacket();
mfajaradityo 0:2071f8e463dd 960
mfajaradityo 0:2071f8e463dd 961 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 962 getRPacket();
mfajaradityo 0:2071f8e463dd 963
mfajaradityo 0:2071f8e463dd 964 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 965
mfajaradityo 0:2071f8e463dd 966 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 967 {
mfajaradityo 0:2071f8e463dd 968 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 969 return rPacket[9];
mfajaradityo 0:2071f8e463dd 970
mfajaradityo 0:2071f8e463dd 971 default : //error occurred
mfajaradityo 0:2071f8e463dd 972 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 973 }
mfajaradityo 0:2071f8e463dd 974 }
mfajaradityo 0:2071f8e463dd 975
mfajaradityo 0:2071f8e463dd 976 uint8_t XL320::GetHWErr(uint8_t id)
mfajaradityo 0:2071f8e463dd 977 {
mfajaradityo 0:2071f8e463dd 978 pc.printf("Get Hardware Error Status\n\r");
mfajaradityo 0:2071f8e463dd 979 targetID = id;
mfajaradityo 0:2071f8e463dd 980 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 981
mfajaradityo 0:2071f8e463dd 982 iRead(1, HARDWARE_ERROR_STATUS);
mfajaradityo 0:2071f8e463dd 983 sendIPacket();
mfajaradityo 0:2071f8e463dd 984
mfajaradityo 0:2071f8e463dd 985 rPacketLength = 12;
mfajaradityo 0:2071f8e463dd 986 getRPacket();
mfajaradityo 0:2071f8e463dd 987
mfajaradityo 0:2071f8e463dd 988 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 989
mfajaradityo 0:2071f8e463dd 990 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 991 {
mfajaradityo 0:2071f8e463dd 992 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 993 return rPacket[9];
mfajaradityo 0:2071f8e463dd 994
mfajaradityo 0:2071f8e463dd 995 default : //error occurred
mfajaradityo 0:2071f8e463dd 996 return rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 997 }
mfajaradityo 0:2071f8e463dd 998 }
mfajaradityo 0:2071f8e463dd 999
mfajaradityo 0:2071f8e463dd 1000 void XL320::SetPunch(uint8_t id,uint16_t punch)
mfajaradityo 0:2071f8e463dd 1001 {
mfajaradityo 0:2071f8e463dd 1002 pc.printf("Set Punch\n\r");
mfajaradityo 0:2071f8e463dd 1003 targetID = id; //XL320 ID
mfajaradityo 0:2071f8e463dd 1004 targetInst = WRITE_DATA;
mfajaradityo 0:2071f8e463dd 1005 parameter[0] = SHIFT_TO_LSB(punch);
mfajaradityo 0:2071f8e463dd 1006 parameter[1] = SHIFT_TO_MSB(punch);
mfajaradityo 0:2071f8e463dd 1007
mfajaradityo 0:2071f8e463dd 1008 iWrite(2, PUNCH, parameter);
mfajaradityo 0:2071f8e463dd 1009 sendIPacket();
mfajaradityo 0:2071f8e463dd 1010
mfajaradityo 0:2071f8e463dd 1011 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 1012 getRPacket();
mfajaradityo 0:2071f8e463dd 1013
mfajaradityo 0:2071f8e463dd 1014 wait_ms(restTime); //gives some time to the targeted servo before another Instruction Packet
mfajaradityo 0:2071f8e463dd 1015 }
mfajaradityo 0:2071f8e463dd 1016
mfajaradityo 0:2071f8e463dd 1017 uint16_t XL320::GetPunch(uint8_t id)
mfajaradityo 0:2071f8e463dd 1018 {
mfajaradityo 0:2071f8e463dd 1019 pc.printf("Get Punch\n\r");
mfajaradityo 0:2071f8e463dd 1020 targetID = id;
mfajaradityo 0:2071f8e463dd 1021 targetInst = READ_DATA;
mfajaradityo 0:2071f8e463dd 1022
mfajaradityo 0:2071f8e463dd 1023 iRead(2, PUNCH);
mfajaradityo 0:2071f8e463dd 1024 sendIPacket();
mfajaradityo 0:2071f8e463dd 1025
mfajaradityo 0:2071f8e463dd 1026 rPacketLength = 13;
mfajaradityo 0:2071f8e463dd 1027 getRPacket();
mfajaradityo 0:2071f8e463dd 1028
mfajaradityo 0:2071f8e463dd 1029 wait_ms(restTime);
mfajaradityo 0:2071f8e463dd 1030
mfajaradityo 0:2071f8e463dd 1031 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 1032 {
mfajaradityo 0:2071f8e463dd 1033 case 0x00 : //no error
mfajaradityo 0:2071f8e463dd 1034 return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00);
mfajaradityo 0:2071f8e463dd 1035
mfajaradityo 0:2071f8e463dd 1036 default : //error occurred
mfajaradityo 0:2071f8e463dd 1037 return (uint16_t)rPacket[8]; //return error status
mfajaradityo 0:2071f8e463dd 1038 }
mfajaradityo 0:2071f8e463dd 1039 }
mfajaradityo 0:2071f8e463dd 1040
mfajaradityo 0:2071f8e463dd 1041
mfajaradityo 0:2071f8e463dd 1042 /***** XL320 Instruction Method *****/
mfajaradityo 0:2071f8e463dd 1043 void XL320::iFactoryReset(uint8_t id,uint8_t option)
mfajaradityo 0:2071f8e463dd 1044 {
mfajaradityo 0:2071f8e463dd 1045 pc.printf("Factory Reset!\n\r");
mfajaradityo 0:2071f8e463dd 1046 targetID = id;
mfajaradityo 0:2071f8e463dd 1047 targetInst = FACTORY_RESET;
mfajaradityo 0:2071f8e463dd 1048
mfajaradityo 0:2071f8e463dd 1049 iPacket[4] = targetID;
mfajaradityo 0:2071f8e463dd 1050 iPacket[5] = 0x04; //[intruction][reset_option][CRC_LSB][CRC_MSB]
mfajaradityo 0:2071f8e463dd 1051 iPacket[6] = 0x00;
mfajaradityo 0:2071f8e463dd 1052 iPacket[7] = targetInst; //instruction
mfajaradityo 0:2071f8e463dd 1053 iPacket[8] = option; //Intruction Packet's parameter field 0 (reset option)
mfajaradityo 0:2071f8e463dd 1054
mfajaradityo 0:2071f8e463dd 1055 CRC = update_crc(0, iPacket, 9); //All Instruction Packet minus CRC
mfajaradityo 0:2071f8e463dd 1056 iPacket[9] = SHIFT_TO_LSB(CRC);
mfajaradityo 0:2071f8e463dd 1057 iPacket[10] = SHIFT_TO_MSB(CRC);
mfajaradityo 0:2071f8e463dd 1058
mfajaradityo 0:2071f8e463dd 1059 iPacketLength = 11;
mfajaradityo 0:2071f8e463dd 1060 sendIPacket();
mfajaradityo 0:2071f8e463dd 1061
mfajaradityo 0:2071f8e463dd 1062 rPacketLength = 11;
mfajaradityo 0:2071f8e463dd 1063 getRPacket();
mfajaradityo 0:2071f8e463dd 1064 }
mfajaradityo 0:2071f8e463dd 1065
mfajaradityo 0:2071f8e463dd 1066 void XL320::iPing(uint8_t id)
mfajaradityo 0:2071f8e463dd 1067 {
mfajaradityo 0:2071f8e463dd 1068 pc.printf("PING!\n\r");
mfajaradityo 0:2071f8e463dd 1069 targetID = id;
mfajaradityo 0:2071f8e463dd 1070 targetInst = PING;
mfajaradityo 0:2071f8e463dd 1071
mfajaradityo 0:2071f8e463dd 1072 iPacket[4] = targetID;
mfajaradityo 0:2071f8e463dd 1073 iPacket[5] = 0x03; //[instruction][CRC_LSB][CRC_MSB]
mfajaradityo 0:2071f8e463dd 1074 iPacket[6] = 0x00;
mfajaradityo 0:2071f8e463dd 1075 iPacket[7] = targetInst; //instruction
mfajaradityo 0:2071f8e463dd 1076
mfajaradityo 0:2071f8e463dd 1077 CRC = update_crc(0, iPacket, 9); //All Instruction Packet minus CRC
mfajaradityo 0:2071f8e463dd 1078 iPacket[8] = SHIFT_TO_LSB(CRC);
mfajaradityo 0:2071f8e463dd 1079 iPacket[9] = SHIFT_TO_MSB(CRC);
mfajaradityo 0:2071f8e463dd 1080
mfajaradityo 0:2071f8e463dd 1081 iPacketLength = 10;
mfajaradityo 0:2071f8e463dd 1082 sendIPacket();
mfajaradityo 0:2071f8e463dd 1083
mfajaradityo 0:2071f8e463dd 1084 rPacketLength = 14;
mfajaradityo 0:2071f8e463dd 1085 getRPacket();
mfajaradityo 0:2071f8e463dd 1086 }
mfajaradityo 0:2071f8e463dd 1087
mfajaradityo 0:2071f8e463dd 1088 /***** Debugging *****/
mfajaradityo 0:2071f8e463dd 1089 void XL320::printLastPackets()
mfajaradityo 0:2071f8e463dd 1090 {
mfajaradityo 0:2071f8e463dd 1091 int i;
mfajaradityo 0:2071f8e463dd 1092
mfajaradityo 0:2071f8e463dd 1093 //print to screen
mfajaradityo 0:2071f8e463dd 1094 pc.printf("Instruction Packet : [");
mfajaradityo 0:2071f8e463dd 1095 for(i = 0; i < iPacketLength; i++)
mfajaradityo 0:2071f8e463dd 1096 {
mfajaradityo 0:2071f8e463dd 1097 pc.printf("'%x' |", iPacket[i]);
mfajaradityo 0:2071f8e463dd 1098 }
mfajaradityo 0:2071f8e463dd 1099 pc.printf("]\n");
mfajaradityo 0:2071f8e463dd 1100 pc.printf("\r");
mfajaradityo 0:2071f8e463dd 1101
mfajaradityo 0:2071f8e463dd 1102 pc.printf("Return Packet : [");
mfajaradityo 0:2071f8e463dd 1103 for(i = 0; i < rPacketLength; i++)
mfajaradityo 0:2071f8e463dd 1104 {
mfajaradityo 0:2071f8e463dd 1105 pc.printf("'%x' |",rPacket[i]);
mfajaradityo 0:2071f8e463dd 1106 }
mfajaradityo 0:2071f8e463dd 1107 pc.printf("]\n");
mfajaradityo 0:2071f8e463dd 1108 pc.printf("\r");
mfajaradityo 0:2071f8e463dd 1109 }
mfajaradityo 0:2071f8e463dd 1110
mfajaradityo 0:2071f8e463dd 1111 void XL320::translateLastRPacket()
mfajaradityo 0:2071f8e463dd 1112 {
mfajaradityo 0:2071f8e463dd 1113 //check Return Packet from Write instruction
mfajaradityo 0:2071f8e463dd 1114 if(rPacket[8] != 0x00) //rPacket[8] is Error Byte
mfajaradityo 0:2071f8e463dd 1115 {
mfajaradityo 0:2071f8e463dd 1116 switch(rPacket[8])
mfajaradityo 0:2071f8e463dd 1117 {
mfajaradityo 0:2071f8e463dd 1118 case RESULT_FAIL :
mfajaradityo 0:2071f8e463dd 1119 pc.printf("Writing Intruction Failed! :(\n\r");
mfajaradityo 0:2071f8e463dd 1120 break;
mfajaradityo 0:2071f8e463dd 1121
mfajaradityo 0:2071f8e463dd 1122 case INS_ERR :
mfajaradityo 0:2071f8e463dd 1123 pc.printf("Undefined Instruction ~.~\n\r");
mfajaradityo 0:2071f8e463dd 1124 break;
mfajaradityo 0:2071f8e463dd 1125
mfajaradityo 0:2071f8e463dd 1126 case CRC_ERR :
mfajaradityo 0:2071f8e463dd 1127 pc.printf("CRC Value Error ~.~\n\r");
mfajaradityo 0:2071f8e463dd 1128 break;
mfajaradityo 0:2071f8e463dd 1129
mfajaradityo 0:2071f8e463dd 1130 case DATA_RANGE_ERR :
mfajaradityo 0:2071f8e463dd 1131 pc.printf("Data Range Error ~.~\n\r");
mfajaradityo 0:2071f8e463dd 1132 break;
mfajaradityo 0:2071f8e463dd 1133
mfajaradityo 0:2071f8e463dd 1134 case DATA_LENGTH_ERR :
mfajaradityo 0:2071f8e463dd 1135 pc.printf("Data too short! -__-\n\r");
mfajaradityo 0:2071f8e463dd 1136 break;
mfajaradityo 0:2071f8e463dd 1137
mfajaradityo 0:2071f8e463dd 1138 case DATA_LIMIT_ERR :
mfajaradityo 0:2071f8e463dd 1139 pc.printf("Data too long! -__-\n\r");
mfajaradityo 0:2071f8e463dd 1140 break;
mfajaradityo 0:2071f8e463dd 1141
mfajaradityo 0:2071f8e463dd 1142 case ACCESS_ERR :
mfajaradityo 0:2071f8e463dd 1143 pc.printf("Not Allowed! >:(\n\r");
mfajaradityo 0:2071f8e463dd 1144 break;
mfajaradityo 0:2071f8e463dd 1145
mfajaradityo 0:2071f8e463dd 1146 default :
mfajaradityo 0:2071f8e463dd 1147 pc.printf("Something is wrong, don't know what ;(\n\r");
mfajaradityo 0:2071f8e463dd 1148 break;
mfajaradityo 0:2071f8e463dd 1149 }
mfajaradityo 0:2071f8e463dd 1150 }
mfajaradityo 0:2071f8e463dd 1151 else
mfajaradityo 0:2071f8e463dd 1152 {
mfajaradityo 0:2071f8e463dd 1153 if(targetInst == WRITE_DATA)
mfajaradityo 0:2071f8e463dd 1154 {
mfajaradityo 0:2071f8e463dd 1155 pc.printf("write Instruction Success!\n\r");
mfajaradityo 0:2071f8e463dd 1156 }
mfajaradityo 0:2071f8e463dd 1157 else
mfajaradityo 0:2071f8e463dd 1158 {
mfajaradityo 0:2071f8e463dd 1159 if(targetInst == READ_DATA)
mfajaradityo 0:2071f8e463dd 1160 {
mfajaradityo 0:2071f8e463dd 1161 switch(targetAddr)
mfajaradityo 0:2071f8e463dd 1162 {
mfajaradityo 0:2071f8e463dd 1163 case BAUD_RATE:
mfajaradityo 0:2071f8e463dd 1164 switch(rPacket[9])
mfajaradityo 0:2071f8e463dd 1165 {
mfajaradityo 0:2071f8e463dd 1166 case 0x00:
mfajaradityo 0:2071f8e463dd 1167 pc.printf("BaudRate : 9600\n\r");
mfajaradityo 0:2071f8e463dd 1168 break;
mfajaradityo 0:2071f8e463dd 1169 case 0x01:
mfajaradityo 0:2071f8e463dd 1170 pc.printf("BaudRate : 57600\n\r");
mfajaradityo 0:2071f8e463dd 1171 break;
mfajaradityo 0:2071f8e463dd 1172 case 0x02:
mfajaradityo 0:2071f8e463dd 1173 pc.printf("BaudRate : 115200\n\r");
mfajaradityo 0:2071f8e463dd 1174 break;
mfajaradityo 0:2071f8e463dd 1175 case 0x04:
mfajaradityo 0:2071f8e463dd 1176 pc.printf("BaudRate : 1000000\n\r");
mfajaradityo 0:2071f8e463dd 1177 break;
mfajaradityo 0:2071f8e463dd 1178 default:
mfajaradityo 0:2071f8e463dd 1179 pc.printf("BaudRate : ??\n\r");
mfajaradityo 0:2071f8e463dd 1180 break;
mfajaradityo 0:2071f8e463dd 1181 }
mfajaradityo 0:2071f8e463dd 1182 break;
mfajaradityo 0:2071f8e463dd 1183
mfajaradityo 0:2071f8e463dd 1184 case RETURN_DELAY_TIME:
mfajaradityo 0:2071f8e463dd 1185 pc.printf("Return Delay Time : '%d'us\n\r", rPacket[9]*2);
mfajaradityo 0:2071f8e463dd 1186 break;
mfajaradityo 0:2071f8e463dd 1187
mfajaradityo 0:2071f8e463dd 1188 case GOAL_POSITION:
mfajaradityo 0:2071f8e463dd 1189 pc.printf("Value : '%d' unit\n\r", (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00));
mfajaradityo 0:2071f8e463dd 1190 break;
mfajaradityo 0:2071f8e463dd 1191
mfajaradityo 0:2071f8e463dd 1192 case GOAL_SPEED:
mfajaradityo 0:2071f8e463dd 1193 pc.printf("Value : '%d' unit\n\r", (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xff00));
mfajaradityo 0:2071f8e463dd 1194 break;
mfajaradityo 0:2071f8e463dd 1195
mfajaradityo 0:2071f8e463dd 1196 default:
mfajaradityo 0:2071f8e463dd 1197 pc.printf("Value : '%d'\n\r", rPacket[9]);
mfajaradityo 0:2071f8e463dd 1198 break;
mfajaradityo 0:2071f8e463dd 1199 }
mfajaradityo 0:2071f8e463dd 1200 }
mfajaradityo 0:2071f8e463dd 1201 }
mfajaradityo 0:2071f8e463dd 1202 }
mfajaradityo 0:2071f8e463dd 1203 pc.printf("\n\r");
mfajaradityo 0:2071f8e463dd 1204 }
mfajaradityo 0:2071f8e463dd 1205
mfajaradityo 0:2071f8e463dd 1206 /***** XL320 Instruction Method *****/
mfajaradityo 0:2071f8e463dd 1207 void XL320::iRead(uint16_t length, uint16_t address)
mfajaradityo 0:2071f8e463dd 1208 {
mfajaradityo 0:2071f8e463dd 1209 targetAddr = address;
mfajaradityo 0:2071f8e463dd 1210
mfajaradityo 0:2071f8e463dd 1211 iPacket[4] = targetID;
mfajaradityo 0:2071f8e463dd 1212 iPacket[5] = 0x07; //[intruction][address_LSB][address_MSB][DataLength_LSB][DataLength_MSB][CRC_LSB][CRC_MSB]
mfajaradityo 0:2071f8e463dd 1213 iPacket[6] = 0x00;
mfajaradityo 0:2071f8e463dd 1214 iPacket[7] = targetInst; //instruction
mfajaradityo 0:2071f8e463dd 1215 iPacket[8] = SHIFT_TO_LSB(address); //Intruction Packet's parameter field 0 (start address_LSB)
mfajaradityo 0:2071f8e463dd 1216 iPacket[9] = SHIFT_TO_MSB(address); //Instruction Packet's parameter field 1 (start address_MSB)
mfajaradityo 0:2071f8e463dd 1217 iPacket[10] = SHIFT_TO_LSB(length); //Instruction Packet's parameter field 2 (read data length_MSB)
mfajaradityo 0:2071f8e463dd 1218 iPacket[11] = SHIFT_TO_MSB(length); //Instruction Packet's parameter field 3 (read data length_LSB)
mfajaradityo 0:2071f8e463dd 1219
mfajaradityo 0:2071f8e463dd 1220 CRC = update_crc(0, iPacket, 12); //All Instruction Packet minus CRC
mfajaradityo 0:2071f8e463dd 1221 iPacket[12] = SHIFT_TO_LSB(CRC);
mfajaradityo 0:2071f8e463dd 1222 iPacket[13] = SHIFT_TO_MSB(CRC);
mfajaradityo 0:2071f8e463dd 1223
mfajaradityo 0:2071f8e463dd 1224 iPacketLength = 14;
mfajaradityo 0:2071f8e463dd 1225 }
mfajaradityo 0:2071f8e463dd 1226
mfajaradityo 0:2071f8e463dd 1227 void XL320::iWrite(uint8_t length, uint16_t address, uint8_t param[])
mfajaradityo 0:2071f8e463dd 1228 {
mfajaradityo 0:2071f8e463dd 1229 int i;
mfajaradityo 0:2071f8e463dd 1230 targetAddr = address;
mfajaradityo 0:2071f8e463dd 1231
mfajaradityo 0:2071f8e463dd 1232 iPacket[4] = targetID;
mfajaradityo 0:2071f8e463dd 1233 iPacket[5] = (5+length); //[intruction][address_LSB][address_MSB][parameter(length)][CRC_LSB][CRC_MSB]
mfajaradityo 0:2071f8e463dd 1234 iPacket[6] = 0x00;
mfajaradityo 0:2071f8e463dd 1235 iPacket[7] = targetInst; //instruction
mfajaradityo 0:2071f8e463dd 1236 iPacket[8] = SHIFT_TO_LSB(address); //Intruction Packet's parameter field 0 (start address_LSB)
mfajaradityo 0:2071f8e463dd 1237 iPacket[9] = SHIFT_TO_MSB(address); //Instruction Packet's parameter field 1 (start address_MSB)
mfajaradityo 0:2071f8e463dd 1238 for(i = 10; i < 10+length ; i++)
mfajaradityo 0:2071f8e463dd 1239 {
mfajaradityo 0:2071f8e463dd 1240 iPacket[i] = param[i-10]; //Instruction Packet's parameter field 2 and so on (write data n-th byte)
mfajaradityo 0:2071f8e463dd 1241 }
mfajaradityo 0:2071f8e463dd 1242
mfajaradityo 0:2071f8e463dd 1243 CRC = update_crc(0, iPacket, 10+length); //All Instruction Packet minus CRC
mfajaradityo 0:2071f8e463dd 1244 iPacket[i] = SHIFT_TO_LSB(CRC);
mfajaradityo 0:2071f8e463dd 1245 iPacket[i+1] = SHIFT_TO_MSB(CRC);
mfajaradityo 0:2071f8e463dd 1246
mfajaradityo 0:2071f8e463dd 1247 iPacketLength = i+2;
mfajaradityo 0:2071f8e463dd 1248 }
mfajaradityo 0:2071f8e463dd 1249
mfajaradityo 0:2071f8e463dd 1250 /***** Dynamixel Protocol 2.0 Methods *****/
mfajaradityo 0:2071f8e463dd 1251 void XL320::sendIPacket()
mfajaradityo 0:2071f8e463dd 1252 {
mfajaradityo 0:2071f8e463dd 1253 int i;
mfajaradityo 0:2071f8e463dd 1254
mfajaradityo 0:2071f8e463dd 1255 //initialize rPacket
mfajaradityo 0:2071f8e463dd 1256 i = 0;
mfajaradityo 0:2071f8e463dd 1257 for(i = 0; i < nBytes; i++)
mfajaradityo 0:2071f8e463dd 1258 {
mfajaradityo 0:2071f8e463dd 1259 rPacket[i] = 0x99;
mfajaradityo 0:2071f8e463dd 1260 }
mfajaradityo 0:2071f8e463dd 1261
mfajaradityo 0:2071f8e463dd 1262 //send Instruction Packet
mfajaradityo 0:2071f8e463dd 1263 dPin = 1;
mfajaradityo 0:2071f8e463dd 1264 wait_ms(1); //wait for 74HC126 enable pin transition delay
mfajaradityo 0:2071f8e463dd 1265
mfajaradityo 0:2071f8e463dd 1266 for(i = 0; i < iPacketLength; i++)
mfajaradityo 0:2071f8e463dd 1267 {
mfajaradityo 0:2071f8e463dd 1268 sXL320.putc(iPacket[i]);
mfajaradityo 0:2071f8e463dd 1269 }
mfajaradityo 0:2071f8e463dd 1270 }
mfajaradityo 0:2071f8e463dd 1271
mfajaradityo 0:2071f8e463dd 1272 void XL320::getRPacket()
mfajaradityo 0:2071f8e463dd 1273 {
mfajaradityo 0:2071f8e463dd 1274 int i;
mfajaradityo 0:2071f8e463dd 1275 const unsigned int wTimeOut = 500;
mfajaradityo 0:2071f8e463dd 1276 bool wait = true;
mfajaradityo 0:2071f8e463dd 1277
mfajaradityo 0:2071f8e463dd 1278 //read Return Packet
mfajaradityo 0:2071f8e463dd 1279 wait_us(180); //strange..default response time delay should be 500..
mfajaradityo 0:2071f8e463dd 1280 dPin = 0;
mfajaradityo 0:2071f8e463dd 1281
mfajaradityo 0:2071f8e463dd 1282 //wait until Return Packet ready to be read
mfajaradityo 0:2071f8e463dd 1283 i = 0;
mfajaradityo 0:2071f8e463dd 1284 while(!sXL320.readable() && wait)
mfajaradityo 0:2071f8e463dd 1285 {
mfajaradityo 0:2071f8e463dd 1286 wait_us(1); //
mfajaradityo 0:2071f8e463dd 1287 i++;
mfajaradityo 0:2071f8e463dd 1288
mfajaradityo 0:2071f8e463dd 1289 if(i == wTimeOut)
mfajaradityo 0:2071f8e463dd 1290 {
mfajaradityo 0:2071f8e463dd 1291 wait = false;
mfajaradityo 0:2071f8e463dd 1292 pc.printf("No Return Packet Detected!, ");
mfajaradityo 0:2071f8e463dd 1293 }
mfajaradityo 0:2071f8e463dd 1294 }
mfajaradityo 1:acad5f85d4f5 1295 pc.printf("has wait Return Packet for :'%d' us\n\r", i);
mfajaradityo 0:2071f8e463dd 1296
mfajaradityo 0:2071f8e463dd 1297 i = 0;
mfajaradityo 0:2071f8e463dd 1298 while(sXL320.readable() && i < rPacketLength)
mfajaradityo 0:2071f8e463dd 1299 {
mfajaradityo 0:2071f8e463dd 1300 rPacket[i] = sXL320.getc();
mfajaradityo 0:2071f8e463dd 1301 i++;
mfajaradityo 0:2071f8e463dd 1302 }
mfajaradityo 0:2071f8e463dd 1303 if(i == 0)
mfajaradityo 0:2071f8e463dd 1304 {
mfajaradityo 0:2071f8e463dd 1305 pc.printf("F*ck! missed the return packet!@#$%^&* \n\r");
mfajaradityo 0:2071f8e463dd 1306 }
mfajaradityo 0:2071f8e463dd 1307
mfajaradityo 0:2071f8e463dd 1308 //printLastPackets();
mfajaradityo 0:2071f8e463dd 1309 }
mfajaradityo 0:2071f8e463dd 1310
mfajaradityo 0:2071f8e463dd 1311 /***** Calculating CRC Method *****/
mfajaradityo 0:2071f8e463dd 1312 unsigned short XL320::update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size)
mfajaradityo 0:2071f8e463dd 1313 {
mfajaradityo 0:2071f8e463dd 1314 unsigned short i, j;
mfajaradityo 0:2071f8e463dd 1315 unsigned short crc_table[256] = {
mfajaradityo 0:2071f8e463dd 1316 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
mfajaradityo 0:2071f8e463dd 1317 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
mfajaradityo 0:2071f8e463dd 1318 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
mfajaradityo 0:2071f8e463dd 1319 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
mfajaradityo 0:2071f8e463dd 1320 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
mfajaradityo 0:2071f8e463dd 1321 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
mfajaradityo 0:2071f8e463dd 1322 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
mfajaradityo 0:2071f8e463dd 1323 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
mfajaradityo 0:2071f8e463dd 1324 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
mfajaradityo 0:2071f8e463dd 1325 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
mfajaradityo 0:2071f8e463dd 1326 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
mfajaradityo 0:2071f8e463dd 1327 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
mfajaradityo 0:2071f8e463dd 1328 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
mfajaradityo 0:2071f8e463dd 1329 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
mfajaradityo 0:2071f8e463dd 1330 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
mfajaradityo 0:2071f8e463dd 1331 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
mfajaradityo 0:2071f8e463dd 1332 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
mfajaradityo 0:2071f8e463dd 1333 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
mfajaradityo 0:2071f8e463dd 1334 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
mfajaradityo 0:2071f8e463dd 1335 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
mfajaradityo 0:2071f8e463dd 1336 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
mfajaradityo 0:2071f8e463dd 1337 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
mfajaradityo 0:2071f8e463dd 1338 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
mfajaradityo 0:2071f8e463dd 1339 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
mfajaradityo 0:2071f8e463dd 1340 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
mfajaradityo 0:2071f8e463dd 1341 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
mfajaradityo 0:2071f8e463dd 1342 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
mfajaradityo 0:2071f8e463dd 1343 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
mfajaradityo 0:2071f8e463dd 1344 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
mfajaradityo 0:2071f8e463dd 1345 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
mfajaradityo 0:2071f8e463dd 1346 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
mfajaradityo 0:2071f8e463dd 1347 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
mfajaradityo 0:2071f8e463dd 1348 };
mfajaradityo 0:2071f8e463dd 1349
mfajaradityo 0:2071f8e463dd 1350 for(j = 0; j < data_blk_size; j++)
mfajaradityo 0:2071f8e463dd 1351 {
mfajaradityo 0:2071f8e463dd 1352 i = ((unsigned short)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
mfajaradityo 0:2071f8e463dd 1353 crc_accum = (crc_accum << 8) ^ crc_table[i];
mfajaradityo 0:2071f8e463dd 1354 }
mfajaradityo 0:2071f8e463dd 1355
mfajaradityo 0:2071f8e463dd 1356 return crc_accum;
mfajaradityo 0:2071f8e463dd 1357 }
mfajaradityo 0:2071f8e463dd 1358
mfajaradityo 0:2071f8e463dd 1359
mfajaradityo 0:2071f8e463dd 1360 XL320::~XL320(){};