CRAC Team
Strategie_13h30
Fork of
CRAC-Strat_2017_homologation_gros_rob
by 
CRAC Team
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 12 2022 19:28:47 by
1.7.2