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 #include "mbed.h"
00024 #include "AX12.h"
00025 
00026 AX12::AX12(PinName tx, PinName rx, int ID, int baud)
00027         : _ax12(tx,rx,baud) 
00028 {
00029     _baud = baud;
00030     _ID = ID;
00031     _ax12.baud(_baud);
00032 }
00033 
00034 int AX12::FactoryReset (void) {
00035     #ifdef AX12_DEBUG
00036         printf("Resetting to factory...\n");
00037     #endif
00038 
00039     return (write(_ID, 0, 1, 0, 2));
00040 }
00041 // Set the mode of the servo
00042 //  0 = Positional (0-300 degrees)
00043 //  1 = Rotational -1 to 1 speed
00044 int AX12::SetMode(int mode) {
00045 
00046     if (mode == 1) { // set CR
00047         SetCWLimit(0);
00048         SetCCWLimit(0);
00049         SetCRSpeed(0.0);
00050     } else {
00051         SetCWLimit(0);
00052         SetCCWLimit(300);
00053         SetCRSpeed(0.0);
00054     }
00055     return(0);
00056 }
00057 
00058 
00059 // if flag[0] is set, were blocking
00060 // if flag[1] is set, we're registering
00061 // they are mutually exclusive operations
00062 int AX12::SetGoal(int degrees, int flags) {
00063 
00064     char reg_flag = 0;
00065     char data[2];
00066 
00067     // set the flag is only the register bit is set in the flag
00068     if (flags == 0x2) {
00069         reg_flag = 1;
00070     }
00071 
00072     // 1023 / 300 * degrees
00073     short goal = (1023 * degrees) / 300;
00074     if (AX12_DEBUG) {
00075         printf("SetGoal to 0x%x\n",goal);
00076     }
00077 
00078     data[0] = goal & 0xff; // bottom 8 bits
00079     data[1] = goal >> 8;   // top 8 bits
00080 
00081     // write the packet, return the error code
00082     int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
00083 
00084     if (flags == 1) {
00085         // block until it comes to a halt
00086         while (isMoving()) {}
00087     }
00088     return(rVal);
00089 }
00090 
00091 //Set AX12 baud [default 1000000]
00092 int AX12::SetBaud (int baud) {
00093 
00094     char data[1];
00095     data[0] = baud;
00096 
00097 #ifdef AX12_DEBUG
00098     printf("Setting Baud rate to %d\n",baud);
00099 #endif
00100 
00101     return (write(0xFE, AX12_REG_BAUD, 1, data));
00102 
00103 }
00104 
00105 // Set continuous rotation speed from -1 to 1
00106 int AX12::SetCRSpeed(float speed) {
00107 
00108     // bit 10     = direction, 0 = CCW, 1=CW
00109     // bits 9-0   = Speed
00110     char data[2];
00111 
00112     int goal = int(0x3ff * abs(speed));
00113 
00114     // Set direction CW if we have a negative speed
00115     if (speed < 0) {
00116         goal |= (0x1 << 10);
00117     }
00118 
00119     data[0] = goal & 0xff; // bottom 8 bits
00120     data[1] = goal >> 8;   // top 8 bits
00121 
00122     // write the packet, return the error code
00123     int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, data);
00124 
00125     return(rVal);
00126 }
00127 
00128 
00129 int AX12::SetCWLimit (int degrees) {
00130 
00131     char data[2];
00132     
00133     // 1023 / 300 * degrees
00134     short limit = (1023 * degrees) / 300;
00135 
00136     if (AX12_DEBUG) {
00137         printf("SetCWLimit to 0x%x\n",limit);
00138     }
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_CW_LIMIT, 2, data));
00145 }
00146 
00147 
00148 int AX12::SetCCWLimit (int degrees) {
00149 
00150     char data[2];
00151 
00152     // 1023 / 300 * degrees
00153     short limit = (1023 * degrees) / 300;
00154 
00155     if (AX12_DEBUG) {
00156         printf("SetCCWLimit to 0x%x\n",limit);
00157     }
00158 
00159     data[0] = limit & 0xff; // bottom 8 bits
00160     data[1] = limit >> 8;   // top 8 bits
00161 
00162     // write the packet, return the error code
00163     return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
00164 }
00165 
00166 //Permet d'activer/désactiver le couple du servo
00167 int AX12::SetTorque (bool state){
00168     char data[1];
00169     data[0] = state;
00170 
00171     if(AX12_DEBUG){
00172         printf("Setting torque to %i\n",state);
00173     }
00174     // write the packet, return the error code
00175     return (write(_ID, AX12_REG_ENABLE_TORQUE, 1, data));
00176 }
00177 
00178 int AX12::SetMaxTorque (float percentage) {
00179     char data[2];
00180     short limit = 1023 * (float)percentage;
00181 
00182     if (AX12_DEBUG) {
00183         printf("SetMaxTorque to 0x%x\n",limit);
00184     }
00185 
00186     data[0] = limit & 0xff; // bottom 8 bits
00187     data[1] = limit >> 8;   // top 8 bits
00188 
00189     // write the packet, return the error code
00190     return (write(_ID, AX12_REG_MAX_TORQUE, 2, data));
00191 
00192 }
00193 
00194 int AX12::SetID (int CurrentID, int NewID) {
00195 
00196     char data[1];
00197     data[0] = NewID;
00198     if (AX12_DEBUG) {
00199         printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
00200     }
00201     return (write(CurrentID, AX12_REG_ID, 1, data));
00202 
00203 }
00204 
00205 
00206 // return 1 is the servo is still in flight
00207 int AX12::isMoving(void) {
00208 
00209     char data[1];
00210     read(_ID,AX12_REG_MOVING,1,data);
00211     return(data[0]);
00212 }
00213 
00214 
00215 void AX12::trigger(void) {
00216 
00217     char TxBuf[16];
00218     char sum = 0;
00219 
00220     if (AX12_TRIGGER_DEBUG) {
00221         printf("\nTriggered\n");
00222     }
00223 
00224     // Build the TxPacket first in RAM, then we'll send in one go
00225     if (AX12_TRIGGER_DEBUG) {
00226         printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
00227     }
00228 
00229     TxBuf[0] = 0xFF;
00230     TxBuf[1] = 0xFF;
00231 
00232     // ID - Broadcast
00233     TxBuf[2] = 0xFE;
00234     sum += TxBuf[2];
00235 
00236     if (AX12_TRIGGER_DEBUG) {
00237         printf("  ID : %d\n",TxBuf[2]);
00238     }
00239 
00240     // Length
00241     TxBuf[3] = 0x02;
00242     sum += TxBuf[3];
00243     if (AX12_TRIGGER_DEBUG) {
00244         printf("  Length %d\n",TxBuf[3]);
00245     }
00246 
00247     // Instruction - ACTION
00248     TxBuf[4] = 0x04;
00249     sum += TxBuf[4];
00250     if (AX12_TRIGGER_DEBUG) {
00251         printf("  Instruction 0x%X\n",TxBuf[5]);
00252     }
00253 
00254     // Checksum
00255     TxBuf[5] = 0xFF - sum;
00256     if (AX12_TRIGGER_DEBUG) {
00257         printf("  Checksum 0x%X\n",TxBuf[5]);
00258     }
00259 
00260     // Transmit the packet in one burst with no pausing
00261     for (int i = 0; i < 6 ; i++) {
00262         _ax12.putc(TxBuf[i]);
00263     }
00264     // This is a broadcast packet, so there will be no reply
00265 
00266     return;
00267 }
00268 
00269 
00270 float AX12::GetPosition(void) {
00271 
00272     if (AX12_DEBUG) {
00273         printf("\nGetPosition(%d)",_ID);
00274     }
00275 
00276     char data[2];
00277 
00278     int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
00279     short position = data[0] + (data[1] << 8);
00280     float angle = (position * 300)/1023;
00281 
00282     return (angle);
00283 }
00284 
00285 
00286 float AX12::GetTemp (void) {
00287 
00288     if (AX12_DEBUG) {
00289         printf("\nGetTemp(%d)",_ID);
00290     }
00291     char data[1];
00292     int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
00293     float temp = data[0];
00294     return(temp);
00295 }
00296 
00297 
00298 float AX12::GetVolts (void) {
00299     if (AX12_DEBUG) {
00300         printf("\nGetVolts(%d)",_ID);
00301     }
00302     char data[1];
00303     int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
00304     float volts = data[0]/10.0;
00305     return(volts);
00306 }
00307 
00308 //Fonction spécifique au projet, valeur retour peu fiable (comme précisé dans la doc de l'AX12)
00309 //////////////////////////////////////////////////////////////////////////////////////////////
00310 //
00311 //                                   !!!  NOT WORKING !!!
00312 //
00313 //////////////////////////////////////////////////////////////////////////////////////////////
00314 float AX12::GetLoad (void) {
00315 
00316     if (AX12_DEBUG) {
00317         printf("\nGetLoad(%d)",_ID);
00318     }
00319 
00320     char data[2];
00321     /*
00322     float min = 376.0;//60.0; // 376 fer
00323     float minLimit = 3070.0;//1535.0; // 1436 ouv 2000 3070
00324     float maxLimit = 1944.0;//1350.0; // 1084 ouv 1168
00325     float max = 3000.0;//1750.0; // 1944
00326     */
00327     int ErrorCode = read(_ID, AX12_REG_LOAD, 2, data);
00328     short val = data[0] + (data[1] << 8);
00329     if(AX12_CALIB){
00330             printf("Raw value: %i\n",val);
00331         }
00332     if (val == 1944) { //constante fortement dépendante de la vitesse de rotation et du moteur en lui même, à changer si on change de moteur et de vitesse de rot
00333         return 0.8;
00334         }
00335     else return 0.0;
00336    /* if(val>0 && val<2048){
00337         
00338         float signedLoad = 0.0;
00339         if(val<(int)min){
00340             signedLoad = 0.01;
00341         }
00342         else if(val>(int)max){
00343             signedLoad = 0.01;
00344         }
00345         else if(val>(int)maxLimit){
00346             signedLoad = (max-val)/(max-maxLimit);
00347         }
00348         else if(val>=(int)minLimit){
00349             signedLoad = 1.0;
00350         }
00351         else if(val<(int)minLimit){
00352             signedLoad = (min-val)/(minLimit-min);
00353         }
00354         if(AX12_CALIB){
00355             printf("Signed load: %f\n",signedLoad);
00356         }
00357         if(AX12_CALIB){
00358             printf("Raw value: %i\n",val);
00359         }
00360         return signedLoad;
00361     }
00362     else return -1.0;*/
00363 }
00364 
00365 
00366 int AX12::read(int ID, int start, int bytes, char* data) {
00367 
00368     char PacketLength = 0x4;
00369     char TxBuf[16];
00370     char sum = 0;
00371     char Status[16];
00372 
00373     Status[4] = 0xFE; // return code
00374 
00375     if (AX12_READ_DEBUG) {
00376         printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
00377     }
00378 
00379     // Build the TxPacket first in RAM, then we'll send in one go
00380     if (AX12_READ_DEBUG) {
00381         printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
00382     }
00383 
00384     TxBuf[0] = 0xff;
00385     TxBuf[1] = 0xff;
00386 
00387     // ID
00388     TxBuf[2] = ID;
00389     sum += TxBuf[2];
00390     if (AX12_READ_DEBUG) {
00391         printf("  ID : %d\n",TxBuf[2]);
00392     }
00393 
00394     // Packet Length
00395     TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
00396     sum += TxBuf[3];            // Accululate the packet sum
00397     if (AX12_READ_DEBUG) {
00398         printf("  Length : 0x%x\n",TxBuf[3]);
00399     }
00400 
00401     // Instruction - Read
00402     TxBuf[4] = 0x2;
00403     sum += TxBuf[4];
00404     if (AX12_READ_DEBUG) {
00405         printf("  Instruction : 0x%x\n",TxBuf[4]);
00406     }
00407 
00408     // Start Address
00409     TxBuf[5] = start;
00410     sum += TxBuf[5];
00411     if (AX12_READ_DEBUG) {
00412         printf("  Start Address : 0x%x\n",TxBuf[5]);
00413     }
00414 
00415     // Bytes to read
00416     TxBuf[6] = bytes;
00417     sum += TxBuf[6];
00418     if (AX12_READ_DEBUG) {
00419         printf("  No bytes : 0x%x\n",TxBuf[6]);
00420     }
00421 
00422     // Checksum
00423     TxBuf[7] = 0xFF - sum;
00424     if (AX12_READ_DEBUG) {
00425         printf("  Checksum : 0x%x\n",TxBuf[7]);
00426     }
00427 
00428     // Transmit the packet in one burst with no pausing
00429     for (int i = 0; i<8 ; i++) {
00430         _ax12.putc(TxBuf[i]);
00431     }
00432 
00433     // Wait for the bytes to be transmitted
00434     wait_us(20);
00435 
00436     // Skip if the read was to the broadcast address
00437     if (_ID != 0xFE) {
00438         
00439         // Receive the Status packet 6+ number of bytes read
00440         //ThisThread::sleep_for(100ms);
00441         wait_us(100*1000);
00442         for (int i=0; i<(6+bytes) ; i++){          
00443             Status[i] = (char)_ax12.getc(i);
00444         }
00445 
00446         // Copy the data from Status into data for return
00447         for (int i=0; i < Status[3]-2 ; i++) {
00448             data[i] = Status[5+i];
00449         }
00450 
00451         if (AX12_READ_DEBUG) {
00452             printf("\nStatus Packet\n");
00453             printf("  Header : 0x%x\n",Status[0]);
00454             printf("  Header : 0x%x\n",Status[1]);
00455             printf("  ID : 0x%x\n",Status[2]);
00456             printf("  Length : 0x%x\n",Status[3]);
00457             printf("  Error Code : 0x%x\n",Status[4]);
00458 
00459             for (int i=0; i < Status[3]-2 ; i++) {
00460                 printf("  Data : 0x%x\n",Status[5+i]);
00461             }
00462 
00463             printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
00464         }
00465 
00466     } // if (ID!=0xFE)
00467 
00468     return(Status[4]);
00469 }
00470 
00471 
00472 int AX12:: write(int ID, int start, int bytes, char* data, int flag) {
00473 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
00474 
00475     char TxBuf[16];
00476     char sum = 0;
00477     char Status[6];
00478 
00479     if (AX12_WRITE_DEBUG) {
00480         printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
00481     }
00482 
00483     // Build the TxPacket first in RAM, then we'll send in one go
00484     if (AX12_WRITE_DEBUG) {
00485         printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
00486     }
00487 
00488     TxBuf[0] = 0xff;
00489     TxBuf[1] = 0xff;
00490 
00491     // ID
00492     TxBuf[2] = ID;
00493     sum += TxBuf[2];
00494 
00495     if (AX12_WRITE_DEBUG) {
00496         printf("  ID : %d\n",TxBuf[2]);
00497     }
00498 
00499     // packet Length
00500     TxBuf[3] = 3+bytes;
00501     sum += TxBuf[3];
00502 
00503     if (AX12_WRITE_DEBUG) {
00504         printf("  Length : %d\n",TxBuf[3]);
00505     }
00506 
00507     // Instruction
00508     if (flag == 1) {
00509         TxBuf[4]=0x04;
00510         sum += TxBuf[4];
00511     } else {
00512         TxBuf[4]=0x03;
00513         sum += TxBuf[4];
00514     }
00515 
00516     if (AX12_WRITE_DEBUG) {
00517         printf("  Instruction : 0x%x\n",TxBuf[4]);
00518     }
00519 
00520     // Start Address
00521     TxBuf[5] = start;
00522     sum += TxBuf[5];
00523     if (AX12_WRITE_DEBUG) {
00524         printf("  Start : 0x%x\n",TxBuf[5]);
00525     }
00526 
00527     // data
00528     for (char i=0; i<bytes ; i++) {
00529         TxBuf[6+i] = data[i];
00530         sum += TxBuf[6+i];
00531         if (AX12_WRITE_DEBUG) {
00532             printf("  Data : 0x%x\n",TxBuf[6+i]);
00533         }
00534     }
00535 
00536     // checksum
00537     TxBuf[6+bytes] = 0xFF - sum;
00538     if (AX12_WRITE_DEBUG) {
00539         printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
00540     }
00541     
00542     int count = 7+bytes;
00543 
00544     if(flag == 2){
00545         TxBuf[0]=0xFF;
00546         TxBuf[1]=0xFF;
00547         TxBuf[2]=_ID;
00548         TxBuf[3]=0x02;
00549         TxBuf[4]=0x06;
00550         TxBuf[5]=0xFF-(TxBuf[2]+TxBuf[3]+TxBuf[4]);
00551         count = 6;
00552     }
00553 
00554     // Transmit the packet in one burst with no pausing
00555     for (int i = 0; i < count ; i++) {
00556         int retu = _ax12.putc(TxBuf[i]);
00557     }
00558 
00559     // Wait for data to transmit
00560     wait_us(20);
00561 
00562     // make sure we have a valid return
00563     Status[4]=0x00;
00564 
00565     // we'll only get a reply if it was not broadcast
00566     if (_ID!=0xFE) {
00567 
00568         // response is always 6 bytes
00569         // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
00570         //ThisThread::sleep_for(100ms);
00571         wait_us(100*1000);
00572         for (int i=0; i < 6 ; i++) {
00573             Status[i] = (char)_ax12.getc(i);
00574         }
00575         
00576         // Build the TxPacket first in RAM, then we'll send in one go
00577         if (AX12_WRITE_DEBUG) {
00578             printf("\nStatus Packet\n  Header : 0x%X, 0x%X\n",Status[0],Status[1]);
00579             printf("  ID : %d\n",Status[2]);
00580             printf("  Length : %d\n",Status[3]);
00581             printf("  Error : 0x%x\n",Status[4]);
00582             printf("  Checksum : 0x%x\n",Status[5]);
00583         }
00584 
00585 
00586     }
00587 
00588     return(Status[4]); // return error code
00589 
00590 }