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, PinName tx_ena, int ID, int baud) : _ax12(tx,rx), _ena_tx(tx_ena)
00028 {
00029     _ena_tx = 1;
00030     _ax12.baud(baud);
00031     _baud_bit = 1000000/baud;
00032     _wait_sent = 20 * _baud_bit;
00033     _timeout_tick = 500 + 12 * _baud_bit;
00034     _ID = ID;
00035     _error_code = 0;
00036     _data[0] = 0;
00037     _reg_flag = 0;
00038 }
00039 
00040 // Set the mode of the servo
00041 //  0 = Positional (0-300 degrees)
00042 //  1 = Rotational -1 to 1 speed
00043 int AX12::SetMode(int mode) 
00044 {
00045     if (mode == 1) 
00046     { // set CR
00047         SetCWLimit(0);
00048         SetCCWLimit(0);
00049         SetCRSpeed(0.0);
00050     } 
00051     else 
00052     {
00053         SetCWLimit(0);
00054         SetCCWLimit(300);
00055         SetCRSpeed(0.0);
00056     }
00057     return(0);
00058 }
00059 
00060 // if flag[0] is set, were blocking
00061 // if flag[1] is set, we're registering
00062 // they are mutually exclusive operations
00063 int AX12::SetGoal(int degrees, int flags) 
00064 {
00065     _reg_flag = 0;
00066 
00067     // set the flag is only the register bit is set in the flag
00068     if (flags == 0x2) 
00069     {
00070         _reg_flag = 1;
00071     }
00072 
00073     // 1023 / 300 * degrees
00074     short goal = (1023 * degrees) / 300;
00075 
00076     _data[0] = goal & 0xff; // bottom 8 bits
00077     _data[1] = goal >> 8;   // top 8 bits
00078 
00079     // write the packet, return the error code
00080     int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, _data, _reg_flag);
00081 
00082     if (flags == 1) 
00083     {
00084         // block until it comes to a halt
00085         while (isMoving()) 
00086         {
00087 
00088         }
00089     }
00090     return(rVal);
00091 }
00092 
00093 void AX12::MultSetGoal(int degrees1, float speed1, 
00094                         int degrees2, float speed2, 
00095                         int degrees3, float speed3, 
00096                         int degrees4, float speed4, 
00097                         int degrees5, float speed5, 
00098                         int degrees6, float speed6, 
00099                         int degrees7, float speed7, 
00100                         int degrees8, float speed8, 
00101                         int degrees9, float speed9, 
00102                         int degrees10, float speed10, 
00103                         int degrees11, float speed11,
00104                         int degrees12, float speed12, 
00105                         int degrees13, float speed13, 
00106                         int degrees14, float speed14, 
00107                         int degrees15, float speed15, 
00108                         int degrees16, float speed16, 
00109                         int degrees17, float speed17, 
00110                         int degrees18, float speed18)
00111 {
00112      char data[(total_ID*length_data) + 1];
00113 
00114     int goal;
00115     int goal_speed;
00116 
00117     int alamat;
00118 
00119     int degrees[19];
00120     float speed[19];
00121    
00122     // assignment value from parameter to array
00123 
00124     degrees[1] = degrees1; 
00125     degrees[2] = degrees2; 
00126     degrees[3] = degrees3; 
00127     degrees[4] = degrees4; 
00128     degrees[5] = degrees5; 
00129     degrees[6] = degrees6; 
00130     degrees[7] = degrees7; 
00131     degrees[8] = degrees8; 
00132     degrees[9] = degrees9; 
00133     degrees[10] = degrees10;
00134     degrees[11] = degrees11;
00135     degrees[12] = degrees12;
00136     degrees[13] = degrees13;
00137     degrees[14] = degrees14;
00138     degrees[15] = degrees15;
00139     degrees[16] = degrees16;
00140     degrees[17] = degrees17;
00141     degrees[18] = degrees18;
00142     
00143     speed[1] = speed1; 
00144     speed[2] = speed2; 
00145     speed[3] = speed3; 
00146     speed[4] = speed4; 
00147     speed[5] = speed5; 
00148     speed[6] = speed6; 
00149     speed[7] = speed7; 
00150     speed[8] = speed8; 
00151     speed[9] = speed9; 
00152     speed[10] = speed10;
00153     speed[11] = speed11;
00154     speed[12] = speed12;
00155     speed[13] = speed13;
00156     speed[14] = speed14;
00157     speed[15] = speed15;
00158     speed[16] = speed16;
00159     speed[17] = speed17;
00160     speed[18] = speed18;
00161 
00162 /*
00163     for (char k=1; k<=total_ID; k++)
00164     {
00165         degrees[k] = degrees1;
00166         speed[k] = speed1;
00167     }
00168 */
00169 
00170     for (int i=1; i<=total_ID; i++)
00171     {
00172         alamat = length_data*(i-1);
00173 
00174         goal = (1023 * degrees[i]) / 300;
00175 
00176         data[alamat+1] = goal & 0xff; // bottom 8 bits
00177         data[alamat+2] = goal >> 8;   // top 8 bits
00178 
00179         float temp = (speed[i]<0)? (-1)*speed[i]:speed[i];
00180         goal_speed = (0x3ff * temp);
00181 
00182         // Set direction CW if we have a negative speed
00183         if (speed[i] < 0) 
00184         {
00185             goal_speed |= (0x1 << 10);
00186         }
00187 
00188         data[alamat+3] = goal_speed & 0xff; // bottom 8 bits
00189         data[alamat+4] = goal_speed >> 8;   // top 8 bits
00190     }
00191 
00192     // Write
00193     sync_write(data);
00194 
00195     return;
00196 }
00197 
00198 // Set continuous rotation speed from -1 to 1
00199 int AX12::SetCRSpeed(float speed) 
00200 {
00201     // bit 10     = direction, 0 = CCW, 1=CW
00202     // bits 9-0   = Speed
00203     float temp = (speed<0)? speed * (-1): speed;
00204     int goal = (0x3ff * temp);
00205 
00206     // Set direction CW if we have a negative speed
00207     if (speed < 0) 
00208     {
00209         goal |= (0x1 << 10);
00210     }
00211 
00212     _data[0] = goal & 0xff; // bottom 8 bits
00213     _data[1] = goal >> 8;   // top 8 bits
00214 
00215     // write the packet, return the error code
00216     int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, _data);
00217 
00218     return(rVal);
00219 }
00220 
00221 int AX12::SetCWLimit (int degrees) 
00222 {
00223     // 1023 / 300 * degrees
00224     short limit = (1023 * degrees) / 300;
00225 
00226     _data[0] = limit & 0xff; // bottom 8 bits
00227     _data[1] = limit >> 8;   // top 8 bits
00228 
00229     // write the packet, return the error code
00230     return (write(_ID, AX12_REG_CW_LIMIT, 2, _data));
00231 }
00232 
00233 int AX12::SetCCWLimit (int degrees) 
00234 {
00235     // 1023 / 300 * degrees
00236     short limit = (1023 * degrees) / 300;
00237 
00238     _data[0] = limit & 0xff; // bottom 8 bits
00239     _data[1] = limit >> 8;   // top 8 bits
00240 
00241     // write the packet, return the error code
00242     return (write(_ID, AX12_REG_CCW_LIMIT, 2, _data));
00243 }
00244 
00245 void AX12::ControlID (int ID) 
00246 {
00247     _ID = ID;
00248 
00249     return;
00250 }
00251 
00252 int AX12::SetID (int CurrentID, int NewID) 
00253 {
00254     _data[0] = NewID;
00255     return (write(CurrentID, AX12_REG_ID, 1, _data));
00256 }
00257 
00258 int AX12::SetBaud (int baud) 
00259 {
00260     _data[0] = baud;
00261     return (write(0xFE, AX12_REG_BAUD, 1, _data));
00262 }
00263 
00264 // return 1 is the servo is still in flight
00265 int AX12::isMoving(void) 
00266 {
00267     read(_ID,AX12_REG_MOVING,1,_data);
00268     return(_data[0]);
00269 }
00270 
00271 void AX12::trigger(void) 
00272 {
00273     char TxBuf[16];
00274     char sum = 0;
00275 
00276     TxBuf[0] = 0xFF;
00277     TxBuf[1] = 0xFF;
00278 
00279     // ID - Broadcast
00280     TxBuf[2] = 0xFE;
00281     sum += TxBuf[2];
00282 
00283     // Length
00284     TxBuf[3] = 0x02;
00285     sum += TxBuf[3];
00286 
00287     // Instruction - ACTION
00288     TxBuf[4] = 0x04;
00289     sum += TxBuf[4];
00290 
00291     // Checksum
00292     TxBuf[5] = 0xFF - sum;
00293 
00294     // Transmit the packet in one burst with no pausing
00295     for (int i = 0; i < 6 ; i++) {
00296         _ax12.putc(TxBuf[i]);
00297     }
00298 
00299     // This is a broadcast packet, so there will be no reply
00300     return;
00301 }
00302 
00303 float AX12::GetPosition(void) 
00304 {
00305     _error_code = read(_ID, AX12_REG_POSITION, 2, _data);
00306     return ((short)(_data[0] + (_data[1] << 8)) * 300.0f / 1024.0f);
00307 }
00308 
00309 float AX12::GetTemp (void) 
00310 {
00311     _error_code = read(_ID, AX12_REG_TEMP, 1, _data);
00312     return(_data[0]);
00313 }
00314 
00315 float AX12::GetVolts (void) 
00316 {
00317     _error_code = read(_ID, AX12_REG_VOLTS, 1, _data);
00318     return(_data[0]/10.0f);
00319 }
00320 
00321 int AX12::read(int ID, int start, int bytes, char* data) 
00322 {
00323     char PacketLength = 0x4;
00324     char TxBuf[8];
00325     char sum = 0;
00326 //    char Status[6 + bytes];
00327 
00328     Status[4] = 0xFE; // return code
00329 
00330     // Build the TxPacket first in RAM, then we'll send in one go
00331 
00332     TxBuf[0] = 0xff;
00333     TxBuf[1] = 0xff;
00334 
00335     // ID
00336     TxBuf[2] = ID;
00337     sum += TxBuf[2];
00338 
00339     // Packet Length
00340     TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
00341     sum += TxBuf[3];            // Accululate the packet sum
00342 
00343     // Instruction - Read
00344     TxBuf[4] = 0x2;
00345     sum += TxBuf[4];
00346 
00347     // Start Address
00348     TxBuf[5] = start;
00349     sum += TxBuf[5];
00350 
00351     // Bytes to read
00352     TxBuf[6] = bytes;
00353     sum += TxBuf[6];
00354 
00355     // Checksum
00356     TxBuf[7] = 0xFF - sum;
00357 
00358     // Transmit the packet in one burst with no pausing
00359     _ena_tx = 1;
00360     for (int i = 0; i<8 ; i++) 
00361     {
00362         _ax12.putc(TxBuf[i]);
00363     }
00364 
00365     // Wait for the bytes to be transmitted
00366     wait_us (_wait_sent);
00367     _ena_tx = 0;
00368 
00369     // Skip if the read was to the broadcast address
00370     if (_ID != 0xFE) 
00371     {
00372         // response packet is always 6 + bytes
00373         // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
00374         // timeout is a little more than the time to transmit
00375         // the packet back, i.e. (6+bytes)*10 bit periods
00376         int plen = 0;
00377         int timeout = 0;
00378         int state = 0;
00379         while ((timeout < _timeout_tick) && (plen < (bytes + 3)))
00380         {
00381             if (_ax12.readable()) 
00382             {
00383                 switch(state)
00384                 {
00385                     case 0:
00386                     case 1:
00387                         Status[state] = _ax12.getc();
00388                         if(Status[state] == 0xFF)
00389                         {
00390                             state++;
00391                             timeout = 0;
00392                         }
00393                         else
00394                             state = 0;
00395                         break;
00396                     case 2:
00397                         Status[state] = _ax12.getc();
00398                         if(Status[state] == ID)
00399                         {
00400                             state++;
00401                             timeout = 0;
00402                         }
00403                         else
00404                             state = 0;
00405                         break;
00406                     case 3:
00407                         Status[plen+state] = _ax12.getc();
00408                         plen++;
00409                         timeout = 0;
00410                         break;
00411                     default:
00412                         state = 0;
00413                 }
00414             }
00415 
00416             // wait for the bit period
00417             wait_us(1);
00418             timeout++;
00419         }
00420 
00421         if (timeout == 550)
00422         {
00423             return(-1);
00424         }
00425 
00426         // Copy the data from Status into data for return
00427         for (int i=0; i < Status[3]-2 ; i++) 
00428         {
00429             data[i] = Status[5+i];
00430         }
00431     } 
00432 
00433     // if (ID!=0xFE)
00434     return(Status[4]);
00435 }
00436 
00437 int AX12::write(int ID, int start, int bytes, char* data, int flag) {
00438 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
00439 
00440 //    char TxBuf[7+bytes];
00441     char sum = 0;
00442     char Status[6];
00443 
00444     // Build the TxPacket first in RAM, then we'll send in one go
00445 
00446     TxBuf[0] = 0xff;
00447     TxBuf[1] = 0xff;
00448 
00449     // ID
00450     TxBuf[2] = ID;
00451     sum += TxBuf[2];
00452 
00453     // packet Length
00454     TxBuf[3] = 3+bytes;
00455     sum += TxBuf[3];
00456 
00457     // Instruction
00458     if (flag == 1) 
00459     {
00460         TxBuf[4]=0x04;
00461         sum += TxBuf[4];
00462     } 
00463     else 
00464     {
00465         TxBuf[4]=0x03;
00466         sum += TxBuf[4];
00467     }
00468 
00469     // Start Address
00470     TxBuf[5] = start;
00471     sum += TxBuf[5];
00472 
00473     // data
00474     for (uint8_t i=0; i<bytes ; i++)
00475     {
00476         TxBuf[6+i] = data[i];
00477         sum += TxBuf[6+i];
00478     }
00479 
00480     // checksum
00481     TxBuf[6+bytes] = 0xFF - sum;
00482 
00483     // Transmit the packet in one burst with no pausing
00484     _ena_tx = 1;
00485     for (int i = 0; i < (7 + bytes) ; i++) 
00486     {
00487         _ax12.putc(TxBuf[i]);
00488     }
00489 
00490     // Wait for data to transmit
00491     wait_us (_wait_sent);
00492     _ena_tx = 0;
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         // response packet is always 6 bytes
00501         // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
00502         // timeout is a little more than the time to transmit
00503         // the packet back, i.e. 60 bit periods, round up to 100
00504         int timeout = 0;
00505         int state = 0;
00506         int plen = 0;
00507         while ((timeout < _timeout_tick) && (plen<3))
00508         {
00509             if (_ax12.readable()) 
00510             {
00511                 switch(state)
00512                 {
00513                     case 0:
00514                     case 1:
00515                         Status[state] = _ax12.getc();
00516                         if(Status[state] == 0xFF)
00517                         {
00518                             state++;
00519                             timeout = 0;
00520                         }
00521                         else
00522                             state = 0;
00523                         break;
00524                     case 2:
00525                         Status[state] = _ax12.getc();
00526                         if(Status[state] == ID)
00527                         {
00528                             state++;
00529                             timeout = 0;
00530                         }
00531                         else
00532                             state = 0;
00533                         break;
00534                     case 3:
00535                         Status[plen+state] = _ax12.getc();
00536                         plen++;
00537                         timeout = 0;
00538                         break;
00539                     default:
00540                         state = 0;
00541                 }
00542             }
00543 
00544             // wait for the bit period
00545             wait_us (1);
00546             timeout++;
00547         }
00548 
00549         // Build the TxPacket first in RAM, then we'll send in one go
00550     }
00551     return(Status[4]); // return error code
00552 }
00553 
00554 int AX12::sync_write(char* data) 
00555 {
00556 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
00557 
00558     char sum = 0;
00559     char Status[6];
00560     char TxBuf[(total_ID*(length_data+1)) + 8];
00561     
00562     int pra_alamat, alamat;
00563 
00564     // Set ID
00565     char ID[total_ID+1];
00566     
00567     for (uint8_t k=1; k<=total_ID; k++)
00568     {
00569         ID[k] = k;
00570     }
00571 
00572     TxBuf[0] = 0xFF;
00573     TxBuf[1] = 0xFF;
00574 
00575     // ID
00576     TxBuf[2] = 0xFE;
00577     sum += TxBuf[2];
00578 
00579     // packet Length
00580     TxBuf[3] = ((length_data+1) * total_ID) + 4;
00581     sum += TxBuf[3];
00582 
00583     // Instruction
00584     TxBuf[4]=0x83;
00585     sum += TxBuf[4];
00586 
00587     // Start Address
00588     TxBuf[5] = start_reg;
00589     sum += TxBuf[5];
00590 
00591     // Length of Data to Write
00592     TxBuf[6] = length_data;
00593     sum += TxBuf[6];
00594 
00595     for (uint8_t j=1; j<=total_ID; j++)
00596     {
00597         pra_alamat = length_data*(j-1);
00598         alamat = 6 + j + pra_alamat;
00599     
00600         TxBuf[alamat] = ID[j];
00601         sum += TxBuf[alamat];
00602 
00603         for (char i=1; i<=length_data; i++)
00604         {
00605             TxBuf[alamat+i] = data[pra_alamat+i];
00606             sum += TxBuf[alamat+i];
00607         }
00608     }
00609 
00610     // checksum
00611     TxBuf[((total_ID*(length_data+1)) + 7)] = 0xFF - (sum & 0xFF);
00612 
00613     // Transmit the packet in one burst with no pausing
00614     _ena_tx = 1;
00615     for (int i = 0; i <= ((total_ID*(length_data+1)) + 7) ; i++) 
00616     {
00617         _ax12.putc(TxBuf[i]);
00618     }
00619 
00620     // Wait for data to transmit
00621     wait_us (_wait_sent);
00622     _ena_tx = 0;
00623 
00624     // make sure we have a valid return
00625     Status[4]=0x00;
00626 
00627     return(Status[4]); // return error code
00628 }
00629 
00630 void AX12::SetTorqueEnable(char val) {
00631     // write(int ID, int start, int length, char* data, int flag=0);
00632     char en = val;
00633     write(_ID, AX12_TORQUE_ENABLE, 1, &en, 0);
00634 }