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@4:2df002cf5f6c, 2016-02-28 (annotated)

Committer:
mfajaradityo
Date:
Sun Feb 28 14:48:03 2016 +0000
Revision:
4:2df002cf5f6c
Parent:
3:89bed7a6d852 
Benerin deskripsi repository yang sebelumnya

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