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