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 }