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 
00026 #define MAX_TIMEOUT 500
00027 
00028 extern Timer t;
00029 
00030 typedef struct 
00031 {
00032     unsigned short Model_Number;
00033     unsigned char Firmware;
00034     unsigned char ID;
00035     unsigned char Baud_Rate;
00036     unsigned char Return_Delay_Time;
00037     unsigned short CW_Angle_Limit;
00038     unsigned short CCW_Angle_Limit;
00039     unsigned char Reserved1;
00040     unsigned char Highest_Limit_Temperature;
00041     unsigned char Lowest_Limit_voltage;
00042     unsigned char Highest_Limit_voltage;
00043     unsigned short Max_Torque;
00044     unsigned char Status_Return_Level;
00045     unsigned char Alarm_LED;
00046     unsigned char Alarm_Shutdown;
00047     unsigned char Reserved2;
00048     unsigned short Down_Calibration;
00049     unsigned short Up_Calibration;
00050     unsigned char Torque_Enable;
00051     unsigned char LED;
00052     unsigned char CW_Compliance_Margin;
00053     unsigned char CCW_Compliance_Margin;
00054     unsigned char CW_Compliance_Slope;
00055     unsigned char CCW_Compliance_Slope;
00056     unsigned short Goal_Position;
00057     unsigned short Moving_Speed;
00058     unsigned short Torque_Limit;
00059     unsigned short Present_Position;
00060     unsigned short Present_Speed;
00061     unsigned short Present_Load;
00062     unsigned char Present_Voltage;
00063     unsigned char Present_Temperature;
00064     unsigned char Registered_Instruction;
00065     unsigned char Reserved3;
00066     unsigned char Moving;    
00067     unsigned char Lock;
00068     unsigned short Punch;
00069 } T_AX12;
00070 
00071 
00072 AX12::AX12(PinName tx, PinName rx, int ID, int baud)
00073     : _ax12(tx,rx)
00074 {
00075     _baud = baud;
00076     _ID = ID;
00077     _ax12.baud(_baud);
00078     
00079 
00080 }
00081 
00082 int AX12::Set_Secure_Goal(int degres)
00083 {
00084     int error = 0;
00085     int position = 0;
00086     int difference = 0;
00087     int timeout_secure = 0;
00088     int autorisation = 0;
00089     
00090     position = Get_Position();
00091     error = Set_Goal(degres);
00092     
00093     while ((autorisation == 0) && (timeout_secure < 100) ) 
00094     {
00095         position = Get_Position();
00096         //printf("position : %d", position );
00097         error = Set_Goal(degres);
00098         //printf("degres : %d", degres);
00099         difference = degres - position;
00100         //printf ("difference : %d", difference );
00101         if (((difference < 2) && (difference > (-2) )))
00102         autorisation = 1;
00103         
00104         timeout_secure++;
00105     }
00106     
00107     if ( timeout_secure == 100)
00108     {
00109         #ifdef AX12_DEBUG
00110         printf (" timeout secure error ");
00111         #endif
00112         return(-1);    
00113     }
00114     return(error);
00115 }
00116 
00117 
00118 int AX12::Get_Return_Delay_Time(void)
00119 {
00120     char data[1];
00121     int ErrorCode = read(_ID, AX12_REG_DELAY_TIME, 1, data);
00122     int time = data[0];
00123     time = 2.0 * time;
00124     return(time);
00125 }
00126 
00127 
00128 int AX12::Get_Baud_Rate(void)
00129 {
00130     char data[1];
00131     int ErrorCode = read(_ID, AX12_REG_BAUD, 1, data);
00132     int baud = data[0];
00133     baud = 2000000 / ( baud + 1 );
00134     return(baud);
00135 }
00136 
00137 
00138 /** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur 
00139 //  minimum >>  Ox000    >>  decimal  0
00140 //  maximum >>  0x3FF    >>  decimal  1023
00141 //  deflaut >>  0x20     >>  decimal  32
00142 */
00143 int AX12::Set_Punch(int punch)
00144 {
00145     char data[2];
00146 
00147     data[0] = punch & 0xff; // bottom 8 bits
00148     data[1] = punch >> 8;   // top 8 bits
00149 
00150     // write the packet, return the error code
00151     return (write(_ID, AX12_REG_PUNCH, 2, data));
00152 
00153 }
00154 
00155 /** Reset
00156 */
00157 int AX12::Reset(int punch)
00158 {
00159     char data[2];
00160 
00161     data[0] = punch & 0xff; // bottom 8 bits
00162     data[1] = punch >> 8;   // top 8 bits
00163 
00164     // write the packet, return the error code
00165     return (write(_ID, 0x06, 1,data));
00166 
00167 }
00168 
00169 int AX12::Get_Punch (void)
00170 {
00171     char data[2];
00172     int ErrorCode = read(_ID, AX12_REG_PUNCH, 2, data);
00173     int punch = data[0] | (data[1]<<8);
00174     return(punch);
00175 }
00176 
00177 
00178 int AX12::Get_Load_Direction (void)
00179 {
00180     char data[2];
00181     int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
00182     int direction = (data[1]>>2) & 0x01;
00183     return(direction);
00184 }
00185 
00186 
00187 int AX12::Get_Load_Value (void)
00188 {
00189     char data[2];
00190     int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
00191     int Load = (data[0] | (data[1]<<8)) & 0x3FF;
00192     return(Load);
00193 }
00194 
00195 
00196 int AX12::Get_Present_Speed (void)
00197 {
00198     char data[2];
00199     int ErrorCode = read(_ID, AX12_REG_PRESENT_SPEED, 2, data);
00200     int speed = data[0] | (data[1]<<8);
00201     return(speed);
00202 }
00203 
00204 
00205 int AX12::Get_CCW_Angle_Limit (void)
00206 {
00207     char data[2];
00208     int ErrorCode = read(_ID, AX12_REG_CCW_LIMIT, 2, data);
00209     int angle = data[0] | (data[1]<<8);
00210     angle = (angle * 300) / 1023;
00211     return(angle);
00212 }
00213 
00214 
00215 int AX12::Get_CW_Angle_Limit (void)
00216 {
00217     char data[2];
00218     int ErrorCode = read(_ID, AX12_REG_CW_LIMIT, 2, data);
00219     int angle = data[0] | (data[1]<<8);
00220     angle = (angle * 300) / 1023;
00221     return(angle);
00222 }
00223 
00224 
00225 
00226 int AX12::Get_Torque_Enable(void)
00227 {
00228     char data[1];
00229     int ErrorCode = read(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
00230     int enable = data[0];
00231     return(enable);
00232 }
00233 
00234 
00235 int AX12::Set_Torque_Enable(int etat) 
00236 {
00237     char data[1];
00238     data [0] = etat;
00239     
00240     int error = write(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
00241     return (error);
00242 }
00243 
00244 
00245 
00246 int AX12::Get_Up_Calibration (void)
00247 {
00248     char data[1];
00249     int ErrorCode = read(_ID, AX12_REG_UP_CALIBRATION, 2, data);
00250     int Up_calibration = data[0] | (data[1]<<8);
00251     return(Up_calibration);
00252 }
00253 
00254 
00255 
00256 int AX12::Get_Down_Calibration (void)
00257 {
00258     char data[1];
00259     int ErrorCode = read(_ID, AX12_REG_DOWN_CALIBRATION, 2, data);
00260     int Dowm_calibration = data[0] | (data[1]<<8);
00261     return(Dowm_calibration);
00262 }
00263 
00264 
00265 
00266 int AX12::Get_ID(void)
00267 {
00268 
00269     char data[1]={-1};
00270     int ErrorCode = read(_ID, AX12_REG_ID, 1, data);
00271     int id = data[0];
00272     return(id);
00273 }
00274 
00275 
00276 // Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 ) 
00277 int AX12::Get_Max_Torque (void)
00278 {
00279     char data[2];
00280     int ErrorCode = read(_ID, AX12_REG_MAX_TORQUE, 2, data);
00281     int torque = data[0] | (data[1]<<8);
00282     return(torque);
00283 }
00284 
00285 
00286 
00287 /** Reglage du couple maximum de l'actionneur 
00288 //  minimum >>  Ox000    >>  decimal  0
00289 //  maximum >>  0x3FF    >>  decimal  1023
00290 //  deflaut >>           >>  decimal  
00291 */
00292 int AX12::Set_Max_Torque(int torque)
00293 {
00294     char data[2];
00295 
00296     data[0] = torque & 0xff; // bottom 8 bits
00297     data[1] = torque >> 8;   // top 8 bits
00298 
00299     // write the packet, return the error code
00300     return (write(_ID, AX12_REG_MAX_TORQUE, 2, data));
00301 
00302 }
00303 
00304 
00305 
00306 
00307 /** Reglage de la desactivation des actionneurs si une erreur le concernant se produit 
00308 Bit                 Function
00309 Bit 7               0
00310 Bit 6               If set to 1, torque off when an Instruction Error occurs 
00311 Bit 5               If set to 1, torque off when an Overload Error occurs 
00312 Bit 4               If set to 1, torque off when a Checksum Error occurs 
00313 Bit 3               If set to 1, torque off when a Range Error occurs 
00314 Bit 2               If set to 1, torque off when an Overheating Error occurs 
00315 Bit 1               If set to 1, torque off when an Angle Limit Error occurs 
00316 Bit 0               If set to 1, torque off when an Input Voltage Error occurs
00317 */
00318 int AX12::Set_Alarm_Shutdown(int valeur) 
00319 {
00320     char data[1];
00321     data [0] = valeur;
00322     
00323     int val_alarm_shutdown = write(_ID, AX12_REG_ALARM_SHUTDOWN, 1, data);
00324     return (val_alarm_shutdown);
00325 }
00326 
00327 
00328 
00329 /** Reglage de l'activation de l'alarme
00330 Bit                 Function
00331 Bit 7               0
00332 Bit 6               If set to 1, the LED blinks when an Instruction Error occurs
00333 Bit 5               If set to 1, the LED blinks when an Overload Error occurs
00334 Bit 4               If set to 1, the LED blinks when a Checksum Error occurs
00335 Bit 3               If set to 1, the LED blinks when a Range Error occurs
00336 Bit 2               If set to 1, the LED blinks when an Overheating Error occurs
00337 Bit 1               If set to 1, the LED blinks when an Angle Limit Error occurs
00338 Bit 0               If set to 1, the LED blinks when an Input Voltage Error occurs
00339 */
00340 int AX12::Set_Alarm_LED(int valeur) 
00341 {
00342     char data[1];
00343     data [0] = valeur;
00344     
00345     int val_alarmLED = write(_ID, AX12_REG_ALARM_LED, 1, data);
00346     return (val_alarmLED);
00347 }
00348 
00349 
00350 
00351 
00352 // Reglage de la réponse à une instruction  
00353 //  0   >>  ne repond a aucune instructions
00354 //  1   >>  repond seulement aux instructions READ_DATA 
00355 //  2   >>  repond à toutes les instructions 
00356 int AX12::Set_Status_Return_Level(int etat) 
00357 {
00358     char data[1];
00359     data [0] = etat;
00360     
00361     int val_return_lvl = write(_ID, AX12_REG_SATUS_RETURN, 1, data);
00362     return (val_return_lvl);
00363 }
00364 
00365 
00366 // Reglage de la tension minimale 
00367 //  minimum >>  Ox32    >>  decimal  50
00368 //  maximum >>  0xFA    >>  decimal  250
00369 //  deflaut >>  0x3C    >>  decimal  60
00370 int AX12::Set_Lowest_Voltage(int val_lowest_voltage) 
00371 {
00372     char data[1];
00373     data [0] = val_lowest_voltage;
00374     
00375     int val_lowvolt = write(_ID, AX12_REG_LOWEST_VOLTAGE, 1, data);
00376     return (val_lowvolt);
00377 }
00378 
00379 
00380 // Reglage de la tension maximale
00381 //  minimum >>  Ox32    >>  decimal  50
00382 //  maximum >>  0xFA    >>  decimal  250
00383 //  deflaut >>  0xBE    >>  decimal  190
00384 int AX12::Set_Highest_Voltage(int val_highest_voltage) 
00385 {
00386     char data[1];
00387     data [0] = val_highest_voltage;
00388     
00389     int val_highvolt = write(_ID, AX12_REG_HIGHEST_VOLTAGE, 1, data);
00390     return (val_highvolt);
00391 }
00392 
00393 
00394 // Reglage du return time delay  EN MICRO SECONDE              2uSec * val_delay_time
00395 //  minimum >>  0       us 
00396 //  maximum >>  508     us
00397 //  deflaut >>  125     us
00398 int AX12::Set_Delay_Time (int val_delay_time )
00399  {
00400     char data[1];
00401     data [0] = val_delay_time/2.0;
00402     
00403     int valdelay_time = write(_ID, AX12_REG_DELAY_TIME, 1, data);          
00404     return (valdelay_time );
00405  }
00406 
00407 
00408 // Reglage de la température max du cervo 
00409 //  minimum >>  Ox00    >>  decimal  0
00410 //  maximum >>  0x96    >>  decimal  150
00411 int AX12::Set_Temperature_Max (int val_temperature )
00412  {
00413     char data[1];
00414     data [0] = val_temperature;
00415     
00416     int valtemp_max = write(_ID, AX12_REG_TEMP_MAX, 1, data);          
00417     return (valtemp_max );
00418  }
00419 
00420 // Etat LED 
00421 //  0 = off
00422 //  1 = on 
00423 int AX12::Set_Etat_LED(int etat) 
00424 {
00425     char data[1];
00426     data [0] = etat;
00427     
00428     int valLED = write(_ID, AX12_REG_LED, 1, data);
00429     return (valLED);
00430 }
00431 
00432 // Set the mode of the servo
00433 //  0 = Positional (0-300 degrees)
00434 //  1 = Rotational -1 to 1 speed
00435 int AX12::Set_Mode(int mode)
00436 {
00437 
00438     if (mode == 1) { // set CR
00439         //wait(0.001);
00440         Set_CW_Angle_Limit(0);
00441         //wait(0.001);
00442         Set_CCW_Angle_Limit(0);
00443         //wait(0.001);
00444         Set_CR_Speed(0.0);
00445         //wait(0.001);
00446     } else {
00447         //wait(0.001);
00448         Set_CW_Angle_Limit(0);
00449         //wait(0.001);
00450         Set_CCW_Angle_Limit(300);
00451         //wait(0.001);
00452         Set_CR_Speed(0.0);
00453         //wait(0.001);
00454     }
00455     return(0);
00456 }
00457 
00458 int AX12::Set_Goal_speed(int speed, int flags)
00459 {
00460 
00461      char reg_flag = 0;
00462     char data[2];
00463 
00464     // set the flag is only the register bit is set in the flag
00465     if (flags == 0x2) {
00466         reg_flag = 1;
00467     }
00468 
00469     data[0] = speed & 0xff; // bottom 8 bits
00470     data[1] = speed >> 8;   // top 8 bits
00471 
00472     // write the packet, return the error code
00473     int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, data, reg_flag);
00474 
00475     /*if (flags == 1) {
00476         // block until it comes to a halt
00477         while (isMoving()) 
00478             {
00479             }
00480     
00481     }*/
00482     return(rVal);
00483 }
00484 
00485 
00486 // if flag[0] is set, were blocking
00487 // if flag[1] is set, we're registering
00488 // they are mutually exclusive operations
00489 int AX12::Set_Goal(int degrees, int flags)
00490 {
00491 
00492     char reg_flag = 0;
00493     char data[2];
00494 
00495     // set the flag is only the register bit is set in the flag
00496     if (flags == 0x2) {
00497         reg_flag = 1;
00498     }
00499 
00500     // 1023 / 300 * degrees
00501     short goal = (1023 * degrees) / 300;
00502 #ifdef AX12_DEBUG
00503     printf("SetGoal to 0x%x\n",goal);
00504 #endif
00505 
00506     data[0] = goal & 0xff; // bottom 8 bits
00507     data[1] = goal >> 8;   // top 8 bits
00508 
00509     // write the packet, return the error code
00510     int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
00511 
00512     /*if (flags == 1) {
00513         // block until it comes to a halt
00514         while (isMoving()) 
00515             {
00516             }
00517     
00518     }*/
00519     return(rVal);
00520 }
00521 
00522 
00523 // Set continuous rotation speed from -1 to 1
00524 int AX12::Set_CR_Speed(float speed)
00525 {
00526 
00527     // bit 10     = direction, 0 = CCW, 1=CW
00528     // bits 9-0   = Speed
00529     char data[2];
00530 
00531     int goal = (0x3ff * abs(speed));
00532 
00533     // Set direction CW if we have a negative speed
00534     if (speed < 0) {
00535         goal |= (0x1 << 10);
00536     }
00537 
00538     data[0] = goal & 0xff; // bottom 8 bits
00539     data[1] = goal >> 8;   // top 8 bits
00540 
00541     // write the packet, return the error code
00542     int rVal = write(_ID, 0x20, 2, data);
00543 
00544     return(rVal);
00545 }
00546 
00547 
00548 int AX12::Set_CW_Angle_Limit (int degrees)
00549 {
00550 
00551     char data[2];
00552 
00553     // 1023 / 300 * degrees
00554     short limit = (1023 * degrees) / 300;
00555 
00556 #ifdef AX12_DEBUG
00557     printf("SetCWLimit to 0x%x\n",limit);
00558 #endif
00559 
00560     data[0] = limit & 0xff; // bottom 8 bits
00561     data[1] = limit >> 8;   // top 8 bits
00562 
00563     // write the packet, return the error code
00564     return (write(_ID, AX12_REG_CW_LIMIT, 2, data));
00565 
00566 }
00567 
00568 int AX12::Set_CCW_Angle_Limit (int degrees)
00569 {
00570 
00571     char data[2];
00572 
00573     // 1023 / 300 * degrees
00574     short limit = (1023 * degrees) / 300;
00575 
00576 #ifdef AX12_DEBUG
00577     printf("SetCCWLimit to 0x%x\n",limit);
00578 #endif
00579 
00580     data[0] = limit & 0xff; // bottom 8 bits
00581     data[1] = limit >> 8;   // top 8 bits
00582 
00583     // write the packet, return the error code
00584     return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
00585 }
00586 
00587 
00588 int AX12::Set_ID (int CurrentID, int NewID)
00589 {
00590 
00591     char data[1];
00592     data[0] = NewID;
00593 
00594 #ifdef AX12_DEBUG
00595     printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
00596 #endif
00597 
00598     return (write(CurrentID, AX12_REG_ID, 1, data));
00599 
00600 }
00601 
00602 
00603 int AX12::Set_Baud (int baud)
00604 {
00605 
00606     char data[1];
00607     data[0] = baud;
00608 
00609 #ifdef AX12_DEBUG
00610     printf("Setting Baud rate to %d\n",baud);
00611 #endif
00612 
00613     return (write(_ID, AX12_REG_BAUD, 1, data));
00614 
00615 }
00616 
00617 
00618 
00619 // return 1 is the servo is still in flight
00620 int AX12::isMoving(void)
00621 {
00622 
00623     char data[1];
00624     read(_ID,AX12_REG_MOVING,1,data);
00625     return(data[0]);
00626 }
00627 
00628 void AX12::reset()
00629 {
00630 
00631     unsigned char TxBuf[16];
00632     unsigned char sum = 0;
00633     unsigned long debut=0;
00634 
00635 #ifdef AX12_TRIGGER_DEBUG
00636     // Build the TxPacket first in RAM, then we'll send in one go
00637     printf("\nreset\n");
00638     printf("\nreset Packet\n  Header : 0xFF, 0xFF\n");
00639 #endif
00640 
00641     TxBuf[0] = 0xFF;
00642     TxBuf[1] = 0xFF;
00643 
00644     // ID - Broadcast
00645     TxBuf[2] =_ID;
00646     sum += TxBuf[2];
00647 
00648 #ifdef AX12_TRIGGER_DEBUG
00649     printf("  ID : %d\n",TxBuf[2]);
00650 #endif
00651 
00652     // Length
00653     TxBuf[3] = 0x02;
00654     sum += TxBuf[3];
00655 
00656 #ifdef AX12_TRIGGER_DEBUG
00657     printf("  Length %d\n",TxBuf[3]);
00658 #endif
00659 
00660     // Instruction - ACTION
00661     TxBuf[4] = 0x06;  //reset
00662     sum += TxBuf[4];
00663 
00664 #ifdef AX12_TRIGGER_DEBUG
00665     printf("  Instruction 0x%X\n",TxBuf[5]);
00666 #endif
00667 
00668     // Checksum
00669     TxBuf[5] = 0xFF - sum;
00670 //#ifdef AX12_TRIGGER_DEBUG
00671     printf("  Checksum 0x%X\n",TxBuf[5]);
00672 //#endif
00673 
00674     // Transmit the packet in one burst with no pausing
00675     for (int i = 0; i < 6 ; i++) 
00676     {
00677         while(_ax12.writeable()==0); 
00678             _ax12.putc(TxBuf[i]);
00679             
00680     }
00681     wait(0.001);
00682     debut=t.read_ms();
00683     
00684         do
00685         {
00686             if (_ax12.readable()==-1)       // reception du premier Header ( 0xFF )
00687                 printf("%02x",_ax12.getc());
00688         }
00689         while((t.read_ms()-debut)<500);
00690     
00691     printf("\n");
00692     return;
00693 }
00694 
00695 void AX12::read_all_info(unsigned char start, unsigned char longueur)
00696 {
00697 
00698     unsigned char TxBuf[16];
00699     unsigned char sum = 0;
00700     unsigned long debut=0;
00701 
00702 #ifdef AX12_TRIGGER_DEBUG
00703     // Build the TxPacket first in RAM, then we'll send in one go
00704     printf("\nreset\n");
00705     printf("\nreset Packet\n  Header : 0xFF, 0xFF\n");
00706 #endif
00707 
00708     TxBuf[0] = 0xFF;
00709     TxBuf[1] = 0xFF;
00710 
00711     // ID - Broadcast
00712     TxBuf[2] =_ID;
00713     sum += TxBuf[2];
00714 
00715 #ifdef AX12_TRIGGER_DEBUG
00716     printf("  ID : %d\n",TxBuf[2]);
00717 #endif
00718 
00719     // Length
00720     TxBuf[3] = 0x04;
00721     sum += TxBuf[3];
00722 
00723 #ifdef AX12_TRIGGER_DEBUG
00724     printf("  Length %d\n",TxBuf[3]);
00725 #endif
00726 
00727     // Instruction - ACTION
00728     TxBuf[4] = INST_READ;  //reset
00729     sum += TxBuf[4];
00730 
00731 #ifdef AX12_TRIGGER_DEBUG
00732     printf("  Instruction 0x%X\n",TxBuf[4]);
00733 #endif
00734 
00735     TxBuf[5] = start;  //reset
00736     sum += TxBuf[5];
00737 
00738     TxBuf[6] = longueur;  //reset
00739     sum += TxBuf[6];
00740 
00741 
00742     // Checksum
00743     TxBuf[7] = 0xFF - sum;
00744 //#ifdef AX12_TRIGGER_DEBUG
00745     //printf("  Checksum 0x%X\n\r",TxBuf[7]);
00746 //#endif
00747 
00748     // Transmit the packet in one burst with no pausing
00749     for (int i = 0; i < 8 ; i++) 
00750     {
00751         while(_ax12.writeable()==0); 
00752             _ax12.putc(TxBuf[i]);
00753             
00754     }
00755     
00756     debut=t.read_ms();
00757     int i=0;
00758         do
00759         {
00760             if (_ax12.readable())
00761             {       // reception du premier Header ( 0xFF )
00762                 printf("%02d:%02x ",start+i,_ax12.getc());
00763                 i++;
00764             }
00765         }
00766         while((t.read_ms()-debut)<5000);
00767     
00768     printf("\n");
00769     return;
00770 }
00771 
00772 
00773 void AX12::trigger(void)
00774 {
00775 
00776     char TxBuf[16];
00777     char sum = 0;
00778 
00779 #ifdef AX12_TRIGGER_DEBUG
00780     // Build the TxPacket first in RAM, then we'll send in one go
00781     printf("\nTriggered\n");
00782     printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
00783 #endif
00784 
00785     TxBuf[0] = 0xFF;
00786     TxBuf[1] = 0xFF;
00787 
00788     // ID - Broadcast
00789     TxBuf[2] = 0xFE;
00790     sum += TxBuf[2];
00791 
00792 #ifdef AX12_TRIGGER_DEBUG
00793     printf("  ID : %d\n",TxBuf[2]);
00794 #endif
00795 
00796     // Length
00797     TxBuf[3] = 0x02;
00798     sum += TxBuf[3];
00799 
00800 #ifdef AX12_TRIGGER_DEBUG
00801     printf("  Length %d\n",TxBuf[3]);
00802 #endif
00803 
00804     // Instruction - ACTION
00805     TxBuf[4] = 0x04;
00806     sum += TxBuf[4];
00807 
00808 #ifdef AX12_TRIGGER_DEBUG
00809     printf("  Instruction 0x%X\n",TxBuf[5]);
00810 #endif
00811 
00812     // Checksum
00813     TxBuf[5] = 0xFF - sum;
00814 #ifdef AX12_TRIGGER_DEBUG
00815     printf("  Checksum 0x%X\n",TxBuf[5]);
00816 #endif
00817 
00818     // Transmit the packet in one burst with no pausing
00819     for (int i = 0; i < 6 ; i++) {
00820         _ax12.putc(TxBuf[i]);
00821     }
00822 
00823     // This is a broadcast packet, so there will be no reply
00824     return;
00825 }
00826 
00827 
00828 float AX12::Get_Position(void)
00829 {
00830 
00831     #ifdef AX12_DEBUG
00832     printf("\nGetPositionID(%d) \n\r",_ID);
00833     #endif
00834 
00835     char data[2];
00836 
00837     int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
00838     int position = data[0] | (data[1] << 8);
00839     float angle = ((float)position * 300.0)/1023.0;
00840 
00841     return (angle);
00842 }
00843 
00844 
00845 float AX12::Get_Temp ()
00846 {
00847 
00848 #ifdef AX12_DEBUG
00849     printf("\nGetTemp(%d)",_ID);
00850 #endif
00851 
00852     char data[1];
00853     int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
00854     float temp = data[0];
00855     return(temp);
00856 }
00857 
00858 
00859 float AX12::Get_Volts (void)
00860 {
00861 
00862 #ifdef AX12_DEBUG
00863     printf("\nGetVolts(%d)",_ID);
00864 #endif
00865 
00866     char data[1];
00867     int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
00868     float volts = data[0]/10.0;
00869     return(volts);
00870 }
00871 
00872 
00873 int AX12::read(int ID, int start, int bytes, char* data) {
00874 
00875 
00876     char PacketLength = 0x3;
00877     char TxBuf[16];
00878     char sum = 0;
00879     char Status[16];
00880     
00881     int timeout = 0;
00882     int plen = 0;
00883     int flag_out = 0;
00884     int timeout_transmit = 0;
00885     int i = 0;
00886     int enable = 0;
00887 //    int poubelle = 0;
00888 //    int count = 0;
00889 //    char vidage[50];
00890         
00891     typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat;
00892     type_etat etat = Header1;
00893  
00894     Status[4] = 0xFE; // return code
00895     
00896     
00897     
00898     
00899     
00900 /*********************************** CREATION DE LA TRAME A EVOYER *****************************************/
00901  
00902 #ifdef AX12_READ_DEBUG
00903     printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
00904 #endif
00905  
00906     // Build the TxPacket first in RAM, then we'll send in one go
00907 #ifdef AX12_READ_DEBUG
00908     printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
00909 #endif
00910  
00911     TxBuf[0] = 0xff;
00912     TxBuf[1] = 0xff;
00913  
00914     // ID
00915     TxBuf[2] = ID;
00916     sum += TxBuf[2];
00917  
00918 #ifdef AX12_READ_DEBUG
00919     printf("  ID : %d\n",TxBuf[2]);
00920 #endif
00921  
00922     // Packet Length
00923     TxBuf[3] = 4;//PacketLength+bytes;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
00924     sum += TxBuf[3];            // Accululate the packet sum
00925  
00926 #ifdef AX12_READ_DEBUG
00927     printf("  Length : 0x%x\n",TxBuf[3]);
00928 #endif
00929  
00930     // Instruction - Read
00931     TxBuf[4] = 0x2;
00932     sum += TxBuf[4];
00933  
00934 #ifdef AX12_READ_DEBUG
00935     printf("  Instruction : 0x%x\n",TxBuf[4]);
00936 #endif
00937  
00938     // Start Address
00939     TxBuf[5] = start;
00940     sum += TxBuf[5];
00941  
00942 #ifdef AX12_READ_DEBUG
00943     printf("  Start Address : 0x%x\n",TxBuf[5]);
00944 #endif
00945  
00946     // Bytes to read
00947     TxBuf[6] = bytes;
00948     sum += TxBuf[6];
00949  
00950 #ifdef AX12_READ_DEBUG
00951     printf("  No bytes : 0x%x\n",TxBuf[6]);
00952 #endif
00953  
00954     // Checksum
00955     TxBuf[7] = 0xFF - sum;
00956 #ifdef AX12_READ_DEBUG
00957     printf("  Checksum : 0x%x\n",TxBuf[7]);
00958 #endif
00959 /********************************************TRAME CONSTRUITE DANS TxBuf***************************************/
00960 
00961 
00962 
00963     
00964     /* Transmission de la trame construite precedemment dans le tableau TxBuf 
00965     */
00966     while ((timeout_transmit<5000) && (i < (7+bytes)))
00967     {
00968         if (_ax12.writeable())  
00969         {
00970             _ax12.putc(TxBuf[i]);
00971             i++;
00972             timeout_transmit = 0;
00973         }
00974         else timeout_transmit++;
00975     }
00976     
00977     if (timeout_transmit == 5000 )   // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
00978         {
00979             #ifdef AX12_DEBUG
00980             printf ("timeout transmit erreur\r\n");
00981             #endif
00982             return(-1);
00983         }
00984      /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
00985     */
00986     
00987     
00988     // Wait for the bytes to be transmitted
00989     wait (0.001);
00990     
00991     
00992  
00993     // Skip if the read was to the broadcast address
00994     if (_ID != 0xFE) {
00995     
00996     
00997     
00998     /* Partie de reception de la trame de retour 
00999     */
01000     while ((flag_out != 1) && (timeout < (1000*bytes)))
01001     {
01002     // Les differents etats de l'automate on été créés au debut de la fonction write ! 
01003     switch (etat) 
01004     {
01005      case Header1:      if (_ax12.readable())       // reception du premier Header ( 0xFF )
01006                         { 
01007                         Status[plen] = _ax12.getc();
01008                         timeout = 0;
01009                             if (Status[plen] == 0xFF )
01010                             {
01011                                 etat = Header2;
01012                                 #ifdef AX12_DEBUG_READ
01013                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01014                                 #endif
01015                                 plen++; 
01016                                 
01017                             }
01018                             else etat = Header1;
01019                         }
01020                         else timeout++;
01021                         break;
01022                     
01023      
01024      case Header2:      if (_ax12.readable())       // reception du second Header ( 0xFF )
01025                         { 
01026                             Status[plen] = _ax12.getc();
01027                             timeout = 0;
01028                             if (Status[plen] == 0xFF )
01029                             {
01030                                 etat = ident;
01031                                 #ifdef AX12_DEBUG_READ
01032                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01033                                 #endif
01034                                 plen++; 
01035                                 
01036                             }
01037                             else if (Status[plen] == ID )  // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!!
01038                             {   
01039                                 Status[plen] = 0;
01040                                 #ifdef AX12_DEBUG_READ
01041                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01042                                 #endif
01043                                 plen++;
01044                                 Status[plen] = ID;
01045                                 etat = length;
01046                                 #ifdef AX12_DEBUG_READ
01047                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01048                                 #endif
01049                                 plen++; 
01050                                 
01051                             }
01052                             else 
01053                             {
01054                                 
01055                                 etat = Header1;
01056                                 plen = 0;
01057                             }
01058                         }
01059                         else timeout++;
01060                         break;
01061      
01062      case ident:        if (_ax12.readable())       // reception de l'octet correspondant à l'ID du servomoteur
01063                         {
01064                             Status[plen] = _ax12.getc();
01065                             timeout = 0;
01066                             if (Status[plen] == ID )
01067                             {
01068                                 etat = length;
01069                                 #ifdef AX12_DEBUG_READ
01070                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01071                                 #endif
01072                                 plen++; 
01073                                 
01074                             }
01075                             else 
01076                             {
01077                                 etat = Header1;
01078                                 plen = 0;
01079                             }
01080                         }
01081                         else timeout++;
01082                         break;
01083      
01084      case length:       if (_ax12.readable())          // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
01085                         {
01086                             Status[plen] = _ax12.getc();
01087                             timeout = 0;
01088                             if (Status[plen] == (bytes+2) )
01089                             {
01090                                 etat = erreur;
01091                                 #ifdef AX12_DEBUG_READ
01092                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01093                                 #endif
01094                                 plen++; 
01095                                 
01096                             }
01097                             else 
01098                             {
01099                                 etat = Header1;
01100                                 plen = 0;
01101                             }
01102                         }
01103                         else timeout++;
01104                         break;
01105     
01106      case erreur:       if (_ax12.readable())       //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) 
01107                         {
01108                             Status[plen] = _ax12.getc();
01109                             timeout = 0; 
01110                             #ifdef AX12_DEBUG_READ
01111                                 printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01112                             #endif
01113                             plen++;
01114                             
01115                             etat = reception;
01116                         }
01117                         else timeout++;
01118      
01119      case reception:    while ( enable < bytes )       // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length )
01120                         {
01121                             if (_ax12.readable())  
01122                             {
01123                                 Status[plen] = _ax12.getc();
01124                                 timeout = 0; 
01125                                 #ifdef AX12_DEBUG_READ
01126                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01127                                 #endif
01128                                 plen++;
01129                                 enable++;
01130                                 
01131                             }
01132                             else timeout++;
01133                         }
01134                         etat = checksum;
01135                         break;
01136                         
01137      case checksum:     if (_ax12.readable())   // reception du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length + somme des données ) >>>> dans le cas d'un retour d'un read!! 
01138                             {
01139                                 Status[plen] = _ax12.getc();
01140                                 timeout = 0;
01141                                 flag_out = 1;
01142                                 etat = Header1;
01143                                 
01144                                 #ifdef AX12_DEBUG_READ
01145                                     printf("data[%d]  : %d\r\n\n", plen, (int)Status[plen]);
01146                                 #endif
01147                             } 
01148                         else timeout++;
01149                         break;
01150                         
01151         default:    break;    
01152         }
01153     }
01154     
01155     
01156     if (timeout == (1000*bytes) )   // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
01157         {
01158             #ifdef AX12_DEBUG
01159             printf ("timeout erreur\n");
01160             #endif
01161             return(-1);
01162         }
01163  
01164  
01165         // copie des données dans le tableau data 
01166         for (int i=0; i < Status[3]-2 ; i++) 
01167         {
01168             data[i] = Status[5+i];    
01169         }
01170  
01171     } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE)
01172     
01173     return(Status[4]);    // retourne le code d'erreur ( octect 5 de la trame de retour )
01174 }
01175 
01176 void AX12::multiple_goal_and_speed(int number_ax12,char* tab)
01177 {
01178     char TxBuf[50];
01179     char sum = 0;      
01180     int timeout_transmit =0;
01181     int i=0, k=0, j=0;
01182     int L=4;                                                                // nombre instructions par paquets
01183     int bytes= ((L+1)*number_ax12)+4;
01184     
01185     typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
01186     type_etat etat= Header1;
01187     
01188     for(j=0; j<50; j++)
01189     {
01190       TxBuf[i]=0;  
01191     }
01192     
01193    #ifdef AX12_WRITE_DEBUG
01194     //printf(" MULTIPLE_GOAL_AND_SPEED \n ");
01195     #endif  
01196 
01197 // Build the TxPacket first in RAM, then we'll send in one go
01198     #ifdef AX12_WRITE_DEBUG
01199     //printf("\nInstruction Packet\n  Header :%d, %d\n",TxBuf[0], TxBuf[1]);
01200     #endif
01201     
01202     TxBuf[0]=0xFF;                                                          // bit de start
01203     TxBuf[1]=0xFF;                                                          // bit de start
01204     
01205     TxBuf[2] = 0xFE;                                                        //ID broadcast
01206     sum += TxBuf[2];
01207     
01208     #ifdef AX12_WRITE_DEBUG
01209     printf("  adresse de difusion : %d\n",TxBuf[2]);
01210     #endif
01211     
01212     TxBuf[3] =bytes;                                                        // longueur
01213     sum += TxBuf[3];
01214     
01215     #ifdef AX12_WRITE_DEBUG
01216     printf("  Longueur : %d\n",TxBuf[3]);
01217     #endif
01218     
01219     TxBuf[4]=0x83;                                                          //SYNC_WRITE
01220     sum += TxBuf[4];
01221     
01222     #ifdef AX12_WRITE_DEBUG
01223     printf("  Instruction : 0x%x\n",TxBuf[4]);
01224     #endif
01225     
01226     TxBuf[5] = 0x1E;                                                        // addresse "GOAL_POSITION"
01227     sum += TxBuf[5];
01228     
01229     #ifdef AX12_WRITE_DEBUG
01230     printf(" Adresse de debut : 0x%x\n",TxBuf[5]);
01231     #endif
01232     
01233     TxBuf[6]=L;                                                             // Nombre instruction par paquets
01234     sum += TxBuf[6];
01235     
01236      #ifdef AX12_WRITE_DEBUG
01237     printf("  nombre instruction/paquet : 0x%x\n",TxBuf[6]);
01238     #endif
01239     
01240     for(i=0; i<(number_ax12*5); i++)                                            // Copie des data de TAB sur TxBuf
01241     {         
01242        
01243          TxBuf[i+7]=tab[i];
01244          sum += TxBuf[i+7];            
01245         
01246     }
01247     
01248     #ifdef AX12_WRITE_DEBUG
01249     for(i=0; i<(number_ax12*5); i++)
01250     {
01251         
01252         printf("  Data : 0x%x\n",TxBuf[i+7]);
01253         
01254     }  
01255     #endif
01256     
01257    TxBuf[(number_ax12*5)+7] = 0xFF - sum ;                                             // CHECKSUM
01258    
01259    #ifdef AX12_WRITE_DEBUG
01260    printf("  Checksum : 0x%x\n",TxBuf[(number_ax12*5)+9]);
01261    #endif
01262      
01263    for(k=0;  k<((number_ax12*5)+8); k++)                                               // TRANSMISSION DE LA TRAME
01264     {
01265             _ax12.putc(TxBuf[k]);          
01266             #ifdef AX12_WRITE_DEBUG
01267                  printf("  transmission : 0x%x\n",TxBuf[k]);
01268              #endif         
01269     }
01270     
01271 
01272 }
01273 
01274 float AX12::read_and_test(float angle,char* Tab)
01275 {
01276     int k=0;
01277     unsigned short val_angle=0, val_reche=0;
01278      
01279      #ifdef AX12_DEBUG
01280       printf("\nread_and_test");
01281       #endif
01282       
01283     if( _ID==0x12)                                                              
01284     { k=1;}
01285     else if( _ID==0x04)                                                      
01286     { k=6;}
01287     else if( _ID==0x07)                                                       
01288     { k=11;}
01289     else if( _ID==0x0F)                                                     
01290     { k=16;}
01291     
01292     val_angle = (unsigned short) (angle/0.3);
01293     val_reche = (unsigned short) Tab[k] + ((unsigned short)Tab[k+1]<<8);
01294      
01295     if((val_angle < (val_reche+(28))) && (val_angle > (val_reche-(28))))
01296     {
01297       #ifdef AX12_DEBUG
01298       printf("\nreturn1");
01299       #endif
01300       return 1;  
01301     }
01302     else 
01303     {
01304       #ifdef AX12_DEBUG
01305       printf("\nreturn0");
01306       #endif
01307       return 0;
01308     }
01309     
01310 }
01311 
01312 int AX12::write(int ID, int start, int bytes, char* data, int flag)
01313 {
01314 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
01315 
01316     char TxBuf[16];
01317     char sum = 0;
01318     char Status[6];
01319     
01320     int timeout = 0;
01321     int plen = 0;
01322     int flag_out = 0;
01323     int timeout_transmit = 0;
01324     int i = 0;
01325     int poubelle = 0;
01326     int count = 0;
01327     char vidage[50];
01328     
01329     typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
01330     type_etat etat = Header1;
01331 
01332 #ifdef AX12_WRITE_DEBUG
01333     printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
01334 #endif
01335 
01336     // Build the TxPacket first in RAM, then we'll send in one go
01337 #ifdef AX12_WRITE_DEBUG
01338     printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
01339 #endif
01340 
01341     TxBuf[0] = 0xff;
01342     TxBuf[1] = 0xff;
01343 
01344     // ID
01345     TxBuf[2] = ID;
01346     sum += TxBuf[2];
01347 
01348 #ifdef AX12_WRITE_DEBUG
01349     printf("  ID : %d\n",TxBuf[2]);
01350 #endif
01351 
01352     // packet Length
01353     TxBuf[3] = 3+bytes;
01354     sum += TxBuf[3];
01355 
01356 #ifdef AX12_WRITE_DEBUG
01357     printf("  Length : %d\n",TxBuf[3]);
01358 #endif
01359 
01360     // Instruction
01361     if (flag == 1) {
01362         TxBuf[4]=0x04;
01363         sum += TxBuf[4];  
01364     } else {
01365         TxBuf[4]=0x03;
01366         sum += TxBuf[4];
01367     }
01368 
01369 #ifdef AX12_WRITE_DEBUG
01370     printf("  Instruction : 0x%x\n",TxBuf[4]);
01371 #endif
01372 
01373     // Start Address
01374     TxBuf[5] = start;
01375     sum += TxBuf[5];
01376 
01377 #ifdef AX12_WRITE_DEBUG
01378     printf("  Start : 0x%x\n",TxBuf[5]);
01379 #endif
01380 
01381     // data
01382     for (char i=0; i<bytes ; i++) {
01383         TxBuf[6+i] = data[i];
01384         sum += TxBuf[6+i];
01385 
01386 #ifdef AX12_WRITE_DEBUG
01387         printf("  Data : 0x%x\n",TxBuf[6+i]);
01388 #endif
01389 
01390     }
01391 
01392     // checksum
01393     TxBuf[6+bytes] = 0xFF - sum;
01394 
01395 #ifdef AX12_WRITE_DEBUG
01396     printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
01397 #endif
01398     
01399     
01400     /* Transmission de la trame construite precedemment dans le tableau TxBuf 
01401     */
01402     while ((timeout_transmit<100) && (i < (7+bytes)))
01403     {
01404         if (_ax12.writeable())  
01405         {
01406             _ax12.putc(TxBuf[i]);
01407             i++;
01408             timeout_transmit = 0;
01409         }
01410         else timeout_transmit++;
01411     }
01412     
01413     if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
01414         {
01415             #ifdef AX12_DEBUG
01416             printf ("TIMEOUT TRANSMIT ERROR\r\n");
01417             #endif
01418             return(-1);
01419         }
01420      /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
01421     */
01422     
01423     
01424     // Wait for data to transmit
01425     wait (0.005);
01426     
01427     // make sure we have a valid return
01428     Status[4]=0x00;
01429 
01430     // we'll only get a reply if it was not broadcast
01431     if (_ID!=0xFE) {
01432 
01433 
01434     /* Partie de reception de la trame de retour 
01435     */
01436     while ((flag_out != 1) && (timeout < MAX_TIMEOUT))
01437     {
01438     // Les differents etats de l'automate on été créés au debut de la fonction write ! 
01439     switch (etat) 
01440     {
01441      case Header1:      if (_ax12.readable())       // reception du premier Header ( 0xFF )
01442                         { 
01443                         Status[plen] = _ax12.getc();
01444                         timeout = 0;
01445                             if (Status[plen] == 0xFF )
01446                             {
01447                                 etat = Header2;
01448                                 #ifdef AX12_DEBUG_WRITE
01449                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01450                                 #endif
01451                                 plen++; 
01452                                 
01453                             }
01454                             else etat = Header1;
01455                         }
01456                         else timeout++;
01457                         break;
01458                     
01459      
01460      case Header2:      if (_ax12.readable())        // reception du second Header ( 0xFF )                               
01461                         { 
01462                             Status[plen] = _ax12.getc();
01463                             timeout = 0;
01464                             if (Status[plen] == 0xFF )
01465                             {
01466                                 etat = ident;
01467                                 #ifdef AX12_DEBUG_WRITE
01468                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01469                                 #endif
01470                                 plen++;             
01471                             }   
01472                             else 
01473                             {
01474                                 etat = Header1;
01475                                 plen = 0;
01476                             }
01477                         }
01478                         else timeout++;
01479                         break;
01480      
01481      case ident:        if (_ax12.readable())                   // reception de l'octet correspondant à l'ID du servomoteur
01482                         {
01483                             Status[plen] = _ax12.getc();
01484                             timeout = 0;
01485                             if (Status[plen] == ID )
01486                             {
01487                                 etat = length;
01488                                 #ifdef AX12_DEBUG_WRITE
01489                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01490                                 #endif
01491                                 plen++; 
01492                             }
01493                             else 
01494                             {
01495                                 etat = Header1;
01496                                 plen = 0;
01497                             }
01498                         }
01499                         else timeout++;
01500                         break;
01501      
01502      case length:       if (_ax12.readable())       // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
01503                         {
01504                             Status[plen] = _ax12.getc();
01505                             timeout = 0;
01506                             if (Status[plen] == 2 )  // dans la trame de retour d'un write il n'y a pas de paramètre la taille vaudra donc 2!!
01507                             {
01508                                 etat = erreur;
01509                                 #ifdef AX12_DEBUG_WRITE
01510                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01511                                 #endif
01512                                 plen++; 
01513                             }
01514                             else 
01515                             {
01516                                 etat = Header1;
01517                                 plen = 0;
01518                             }
01519                         }
01520                         else timeout++;
01521                         break;
01522     
01523      case erreur:       if (_ax12.readable())           //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) 
01524                         {
01525                             Status[plen] = _ax12.getc();
01526                             timeout = 0;
01527                             #ifdef AX12_DEBUG_WRITE
01528                                     printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01529                             #endif
01530                             plen++;
01531                             etat = checksum;
01532                         } 
01533                         else timeout++;
01534                         
01535      case checksum:     if (_ax12.readable())              // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write 
01536                         {
01537                             Status[plen] = _ax12.getc();
01538                             timeout = 0;
01539                             flag_out = 1;
01540                             etat = Header1;
01541                             #ifdef AX12_DEBUG_WRITE
01542                                     printf("data[%d]  : %d\r\n\n", plen, (int)Status[plen]);
01543                             #endif
01544                         } 
01545                         else timeout++;
01546                         break;    
01547         }
01548     }
01549   
01550     
01551     if ( Status[4] != 0 )
01552     {
01553         #ifdef AX12_DEBUG
01554         printf ("erreur ! \r\n");
01555         #endif
01556         for (int i = 0; i<5; i++)
01557         {
01558             #ifdef AX12_DEBUG
01559             printf("data[%d]  : %d\r\n", plen, (int)Status[plen]);
01560             #endif
01561         }   
01562     }
01563     
01564     if (timeout == MAX_TIMEOUT )    // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
01565         {
01566             #ifdef AX12_DEBUG
01567             printf ("timeout erreur\n\r");
01568             #endif
01569             return(-1);
01570         }
01571     
01572         // Build the TxPacket first in RAM, then we'll send in one go
01573     }
01574 
01575     return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) 
01576 }