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.
AX12.cpp
00001 /* mbed AX-12+ Servo Library 00002 * 00003 * Copyright (c) 2010, cstyles (http://mbed.org) 00004 * 00005 * Permission is hereby granted, free of charge, to any person obtaining a copy 00006 * of this software and associated documentation files (the "Software"), to deal 00007 * in the Software without restriction, including without limitation the rights 00008 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00009 * copies of the Software, and to permit persons to whom the Software is 00010 * furnished to do so, subject to the following conditions: 00011 * 00012 * The above copyright notice and this permission notice shall be included in 00013 * all copies or substantial portions of the Software. 00014 * 00015 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00016 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00017 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00018 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00019 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00020 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00021 * THE SOFTWARE. 00022 */ 00023 00024 #include "AX12.h" 00025 #include "mbed.h" 00026 00027 AX12::AX12(PinName tx, PinName rx, int ID, int baud) 00028 : _ax12(tx,rx) { 00029 _baud = baud; 00030 _ID = ID; 00031 _ax12.baud(_baud); 00032 00033 } 00034 00035 // Set the mode of the servo 00036 // 0 = Positional (0-300 degrees) 00037 // 1 = Rotational -1 to 1 speed 00038 int AX12::SetMode(int mode) { 00039 00040 if (mode == 1) { // set CR 00041 SetCWLimit(0); 00042 SetCCWLimit(0); 00043 SetCRSpeed(0.0); 00044 } else { 00045 SetCWLimit(0); 00046 SetCCWLimit(300); 00047 SetCRSpeed(0.0); 00048 } 00049 return(0); 00050 } 00051 00052 00053 // if flag[0] is set, were blocking 00054 // if flag[1] is set, we're registering 00055 // they are mutually exclusive operations 00056 int AX12::SetGoal(int degrees, int flags) { 00057 00058 char reg_flag = 0; 00059 char data[2]; 00060 00061 // set the flag is only the register bit is set in the flag 00062 if (flags == 0x2) { 00063 reg_flag = 1; 00064 } 00065 00066 // 1023 / 300 * degrees 00067 short goal = (1023 * degrees) / 300; 00068 #ifdef AX12_DEBUG 00069 printf("SetGoal to 0x%x\n",goal); 00070 #endif 00071 00072 data[0] = goal & 0xff; // bottom 8 bits 00073 data[1] = goal >> 8; // top 8 bits 00074 00075 // write the packet, return the error code 00076 int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag); 00077 00078 if (flags == 1) { 00079 // block until it comes to a halt 00080 while (isMoving()) {} 00081 } 00082 return(rVal); 00083 } 00084 00085 00086 // Set continuous rotation speed from -1 to 1 00087 int AX12::SetCRSpeed(float speed) { 00088 00089 // bit 10 = direction, 0 = CCW, 1=CW 00090 // bits 9-0 = Speed 00091 char data[2]; 00092 00093 int goal = (0x3ff * abs(speed)); 00094 00095 // Set direction CW if we have a negative speed 00096 if (speed < 0) { 00097 goal |= (0x1 << 10); 00098 } 00099 00100 data[0] = goal & 0xff; // bottom 8 bits 00101 data[1] = goal >> 8; // top 8 bits 00102 00103 // write the packet, return the error code 00104 int rVal = write(_ID, 0x20, 2, data); 00105 00106 return(rVal); 00107 } 00108 00109 00110 int AX12::SetCWLimit (int degrees) { 00111 00112 char data[2]; 00113 00114 // 1023 / 300 * degrees 00115 short limit = (1023 * degrees) / 300; 00116 00117 #ifdef AX12_DEBUG 00118 printf("SetCWLimit to 0x%x\n",limit); 00119 #endif 00120 00121 data[0] = limit & 0xff; // bottom 8 bits 00122 data[1] = limit >> 8; // top 8 bits 00123 00124 // write the packet, return the error code 00125 return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); 00126 00127 } 00128 00129 int AX12::SetCCWLimit (int degrees) { 00130 00131 char data[2]; 00132 00133 // 1023 / 300 * degrees 00134 short limit = (1023 * degrees) / 300; 00135 00136 #ifdef AX12_DEBUG 00137 printf("SetCCWLimit to 0x%x\n",limit); 00138 #endif 00139 00140 data[0] = limit & 0xff; // bottom 8 bits 00141 data[1] = limit >> 8; // top 8 bits 00142 00143 // write the packet, return the error code 00144 return (write(_ID, AX12_REG_CCW_LIMIT, 2, data)); 00145 } 00146 00147 00148 int AX12::SetID (int CurrentID, int NewID) { 00149 00150 char data[1]; 00151 data[0] = NewID; 00152 00153 #ifdef AX12_DEBUG 00154 printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); 00155 #endif 00156 00157 return (write(CurrentID, AX12_REG_ID, 1, data)); 00158 00159 } 00160 00161 00162 int AX12::SetBaud (int baud) { 00163 00164 char data[1]; 00165 data[0] = baud; 00166 00167 #ifdef AX12_DEBUG 00168 printf("Setting Baud rate to %d\n",baud); 00169 #endif 00170 00171 return (write(0xFE, AX12_REG_BAUD, 1, data)); 00172 00173 } 00174 00175 00176 00177 // return 1 is the servo is still in flight 00178 int AX12::isMoving(void) { 00179 00180 char data[1]; 00181 read(_ID,AX12_REG_MOVING,1,data); 00182 return(data[0]); 00183 } 00184 00185 00186 void AX12::trigger(void) { 00187 00188 char TxBuf[16]; 00189 char sum = 0; 00190 00191 #ifdef AX12_TRIGGER_DEBUG 00192 // Build the TxPacket first in RAM, then we'll send in one go 00193 printf("\nTriggered\n"); 00194 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); 00195 #endif 00196 00197 TxBuf[0] = 0xFF; 00198 TxBuf[1] = 0xFF; 00199 00200 // ID - Broadcast 00201 TxBuf[2] = 0xFE; 00202 sum += TxBuf[2]; 00203 00204 #ifdef AX12_TRIGGER_DEBUG 00205 printf(" ID : %d\n",TxBuf[2]); 00206 #endif 00207 00208 // Length 00209 TxBuf[3] = 0x02; 00210 sum += TxBuf[3]; 00211 00212 #ifdef AX12_TRIGGER_DEBUG 00213 printf(" Length %d\n",TxBuf[3]); 00214 #endif 00215 00216 // Instruction - ACTION 00217 TxBuf[4] = 0x04; 00218 sum += TxBuf[4]; 00219 00220 #ifdef AX12_TRIGGER_DEBUG 00221 printf(" Instruction 0x%X\n",TxBuf[5]); 00222 #endif 00223 00224 // Checksum 00225 TxBuf[5] = 0xFF - sum; 00226 #ifdef AX12_TRIGGER_DEBUG 00227 printf(" Checksum 0x%X\n",TxBuf[5]); 00228 #endif 00229 00230 // Transmit the packet in one burst with no pausing 00231 for (int i = 0; i < 6 ; i++) { 00232 _ax12.putc(TxBuf[i]); 00233 } 00234 00235 // This is a broadcast packet, so there will be no reply 00236 return; 00237 } 00238 00239 00240 float AX12::GetPosition(void) { 00241 00242 #ifdef AX12_DEBUG 00243 printf("\nGetPosition(%d)",_ID); 00244 #endif 00245 00246 char data[2]; 00247 00248 int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); 00249 short position = data[0] + (data[1] << 8); 00250 float angle = (position * 300)/1024; 00251 00252 return (angle); 00253 } 00254 00255 00256 float AX12::GetTemp (void) { 00257 00258 #ifdef AX12_DEBUG 00259 printf("\nGetTemp(%d)",_ID); 00260 #endif 00261 00262 char data[1]; 00263 int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); 00264 float temp = data[0]; 00265 return(temp); 00266 } 00267 00268 00269 float AX12::GetVolts (void) { 00270 00271 #ifdef AX12_DEBUG 00272 printf("\nGetVolts(%d)",_ID); 00273 #endif 00274 00275 char data[1]; 00276 int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); 00277 float volts = data[0]/10.0; 00278 return(volts); 00279 } 00280 00281 00282 int AX12::read(int ID, int start, int bytes, char* data) { 00283 00284 char PacketLength = 0x4; 00285 char TxBuf[16]; 00286 char sum = 0; 00287 char Status[16]; 00288 00289 Status[4] = 0xFE; // return code 00290 00291 #ifdef AX12_READ_DEBUG 00292 printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); 00293 #endif 00294 00295 // Build the TxPacket first in RAM, then we'll send in one go 00296 #ifdef AX12_READ_DEBUG 00297 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); 00298 #endif 00299 00300 TxBuf[0] = 0xff; 00301 TxBuf[1] = 0xff; 00302 00303 // ID 00304 TxBuf[2] = ID; 00305 sum += TxBuf[2]; 00306 00307 #ifdef AX12_READ_DEBUG 00308 printf(" ID : %d\n",TxBuf[2]); 00309 #endif 00310 00311 // Packet Length 00312 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) 00313 sum += TxBuf[3]; // Accululate the packet sum 00314 00315 #ifdef AX12_READ_DEBUG 00316 printf(" Length : 0x%x\n",TxBuf[3]); 00317 #endif 00318 00319 // Instruction - Read 00320 TxBuf[4] = 0x2; 00321 sum += TxBuf[4]; 00322 00323 #ifdef AX12_READ_DEBUG 00324 printf(" Instruction : 0x%x\n",TxBuf[4]); 00325 #endif 00326 00327 // Start Address 00328 TxBuf[5] = start; 00329 sum += TxBuf[5]; 00330 00331 #ifdef AX12_READ_DEBUG 00332 printf(" Start Address : 0x%x\n",TxBuf[5]); 00333 #endif 00334 00335 // Bytes to read 00336 TxBuf[6] = bytes; 00337 sum += TxBuf[6]; 00338 00339 #ifdef AX12_READ_DEBUG 00340 printf(" No bytes : 0x%x\n",TxBuf[6]); 00341 #endif 00342 00343 // Checksum 00344 TxBuf[7] = 0xFF - sum; 00345 #ifdef AX12_READ_DEBUG 00346 printf(" Checksum : 0x%x\n",TxBuf[7]); 00347 #endif 00348 00349 // Transmit the packet in one burst with no pausing 00350 for (int i = 0; i<8 ; i++) { 00351 _ax12.putc(TxBuf[i]); 00352 } 00353 00354 // Wait for the bytes to be transmitted 00355 wait (0.00002); 00356 00357 // Skip if the read was to the broadcast address 00358 if (_ID != 0xFE) { 00359 00360 00361 00362 // response packet is always 6 + bytes 00363 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum 00364 // timeout is a little more than the time to transmit 00365 // the packet back, i.e. (6+bytes)*10 bit periods 00366 00367 int timeout = 0; 00368 int plen = 0; 00369 while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) { 00370 00371 if (_ax12.readable()) { 00372 Status[plen] = _ax12.getc(); 00373 plen++; 00374 timeout = 0; 00375 } 00376 00377 // wait for the bit period 00378 wait (1.0/_baud); 00379 timeout++; 00380 } 00381 00382 if (timeout == ((6+bytes)*10) ) { 00383 return(-1); 00384 } 00385 00386 // Copy the data from Status into data for return 00387 for (int i=0; i < Status[3]-2 ; i++) { 00388 data[i] = Status[5+i]; 00389 } 00390 00391 #ifdef AX12_READ_DEBUG 00392 printf("\nStatus Packet\n"); 00393 printf(" Header : 0x%x\n",Status[0]); 00394 printf(" Header : 0x%x\n",Status[1]); 00395 printf(" ID : 0x%x\n",Status[2]); 00396 printf(" Length : 0x%x\n",Status[3]); 00397 printf(" Error Code : 0x%x\n",Status[4]); 00398 00399 for (int i=0; i < Status[3]-2 ; i++) { 00400 printf(" Data : 0x%x\n",Status[5+i]); 00401 } 00402 00403 printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]); 00404 #endif 00405 00406 } // if (ID!=0xFE) 00407 00408 return(Status[4]); 00409 } 00410 00411 00412 int AX12::write(int ID, int start, int bytes, char* data, int flag) { 00413 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum 00414 00415 char TxBuf[16]; 00416 char sum = 0; 00417 char Status[6]; 00418 00419 #ifdef AX12_WRITE_DEBUG 00420 printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); 00421 #endif 00422 00423 // Build the TxPacket first in RAM, then we'll send in one go 00424 #ifdef AX12_WRITE_DEBUG 00425 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); 00426 #endif 00427 00428 TxBuf[0] = 0xff; 00429 TxBuf[1] = 0xff; 00430 00431 // ID 00432 TxBuf[2] = ID; 00433 sum += TxBuf[2]; 00434 00435 #ifdef AX12_WRITE_DEBUG 00436 printf(" ID : %d\n",TxBuf[2]); 00437 #endif 00438 00439 // packet Length 00440 TxBuf[3] = 3+bytes; 00441 sum += TxBuf[3]; 00442 00443 #ifdef AX12_WRITE_DEBUG 00444 printf(" Length : %d\n",TxBuf[3]); 00445 #endif 00446 00447 // Instruction 00448 if (flag == 1) { 00449 TxBuf[4]=0x04; 00450 sum += TxBuf[4]; 00451 } else { 00452 TxBuf[4]=0x03; 00453 sum += TxBuf[4]; 00454 } 00455 00456 #ifdef AX12_WRITE_DEBUG 00457 printf(" Instruction : 0x%x\n",TxBuf[4]); 00458 #endif 00459 00460 // Start Address 00461 TxBuf[5] = start; 00462 sum += TxBuf[5]; 00463 00464 #ifdef AX12_WRITE_DEBUG 00465 printf(" Start : 0x%x\n",TxBuf[5]); 00466 #endif 00467 00468 // data 00469 for (char i=0; i<bytes ; i++) { 00470 TxBuf[6+i] = data[i]; 00471 sum += TxBuf[6+i]; 00472 00473 #ifdef AX12_WRITE_DEBUG 00474 printf(" Data : 0x%x\n",TxBuf[6+i]); 00475 #endif 00476 00477 } 00478 00479 // checksum 00480 TxBuf[6+bytes] = 0xFF - sum; 00481 00482 #ifdef AX12_WRITE_DEBUG 00483 printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); 00484 #endif 00485 00486 // Transmit the packet in one burst with no pausing 00487 for (int i = 0; i < (7 + bytes) ; i++) { 00488 _ax12.putc(TxBuf[i]); 00489 } 00490 00491 // Wait for data to transmit 00492 wait (0.00002); 00493 00494 // make sure we have a valid return 00495 Status[4]=0x00; 00496 00497 // we'll only get a reply if it was not broadcast 00498 if (_ID!=0xFE) { 00499 00500 00501 // response packet is always 6 bytes 00502 // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum 00503 // timeout is a little more than the time to transmit 00504 // the packet back, i.e. 60 bit periods, round up to 100 00505 int timeout = 0; 00506 int plen = 0; 00507 while ((timeout < 100) && (plen<6)) { 00508 00509 if (_ax12.readable()) { 00510 Status[plen] = _ax12.getc(); 00511 plen++; 00512 timeout = 0; 00513 } 00514 00515 // wait for the bit period 00516 wait (1.0/_baud); 00517 timeout++; 00518 } 00519 00520 00521 // Build the TxPacket first in RAM, then we'll send in one go 00522 #ifdef AX12_WRITE_DEBUG 00523 printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]); 00524 printf(" ID : %d\n",Status[2]); 00525 printf(" Length : %d\n",Status[3]); 00526 printf(" Error : 0x%x\n",Status[4]); 00527 printf(" Checksum : 0x%x\n",Status[5]); 00528 #endif 00529 00530 00531 } 00532 00533 return(Status[4]); // return error code 00534 }
Generated on Tue Jul 12 2022 13:29:26 by
