CRAC Team
mouvement AX12 petit robot version 2
Fork of
command_AX12_petit_robot
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 "mbed.h" 00025 #include "AX12.h" 00026 00027 #define MAX_TIMEOUT 500 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[4]); 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("\nGetPositionID(%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 () 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] = 4;//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<5000) && (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 == 5000 ) // 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 void AX12::multiple_goal_and_speed(int number_ax12,char* tab) 01178 { 01179 char TxBuf[50]; 01180 char sum = 0; 01181 int timeout_transmit =0; 01182 int i=0, k=0, j=0; 01183 int L=4; // nombre instructions par paquets 01184 int bytes= ((L+1)*number_ax12)+4; 01185 01186 typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; 01187 type_etat etat= Header1; 01188 01189 for(j=0; j<50; j++) 01190 { 01191 TxBuf[i]=0; 01192 } 01193 01194 #ifdef AX12_WRITE_DEBUG 01195 //printf(" MULTIPLE_GOAL_AND_SPEED \n "); 01196 #endif 01197 01198 // Build the TxPacket first in RAM, then we'll send in one go 01199 #ifdef AX12_WRITE_DEBUG 01200 //printf("\nInstruction Packet\n Header :%d, %d\n",TxBuf[0], TxBuf[1]); 01201 #endif 01202 01203 TxBuf[0]=0xFF; // bit de start 01204 TxBuf[1]=0xFF; // bit de start 01205 01206 TxBuf[2] = 0xFE; //ID broadcast 01207 sum += TxBuf[2]; 01208 01209 #ifdef AX12_WRITE_DEBUG 01210 printf(" adresse de difusion : %d\n",TxBuf[2]); 01211 #endif 01212 01213 TxBuf[3] =bytes; // longueur 01214 sum += TxBuf[3]; 01215 01216 #ifdef AX12_WRITE_DEBUG 01217 printf(" Longueur : %d\n",TxBuf[3]); 01218 #endif 01219 01220 TxBuf[4]=0x83; //SYNC_WRITE 01221 sum += TxBuf[4]; 01222 01223 #ifdef AX12_WRITE_DEBUG 01224 printf(" Instruction : 0x%x\n",TxBuf[4]); 01225 #endif 01226 01227 TxBuf[5] = 0x1E; // addresse "GOAL_POSITION" 01228 sum += TxBuf[5]; 01229 01230 #ifdef AX12_WRITE_DEBUG 01231 printf(" Adresse de debut : 0x%x\n",TxBuf[5]); 01232 #endif 01233 01234 TxBuf[6]=L; // Nombre instruction par paquets 01235 sum += TxBuf[6]; 01236 01237 #ifdef AX12_WRITE_DEBUG 01238 printf(" nombre instruction/paquet : 0x%x\n",TxBuf[6]); 01239 #endif 01240 01241 for(i=0; i<(number_ax12*5); i++) // Copie des data de TAB sur TxBuf 01242 { 01243 01244 TxBuf[i+7]=tab[i]; 01245 sum += TxBuf[i+7]; 01246 01247 } 01248 01249 #ifdef AX12_WRITE_DEBUG 01250 for(i=0; i<(number_ax12*5); i++) 01251 { 01252 01253 printf(" Data : 0x%x\n",TxBuf[i+7]); 01254 01255 } 01256 #endif 01257 01258 TxBuf[(number_ax12*5)+7] = 0xFF - sum ; // CHECKSUM 01259 01260 #ifdef AX12_WRITE_DEBUG 01261 printf(" Checksum : 0x%x\n",TxBuf[(number_ax12*5)+9]); 01262 #endif 01263 01264 for(k=0; k<((number_ax12*5)+8); k++) // TRANSMISSION DE LA TRAME 01265 { 01266 _ax12.putc(TxBuf[k]); 01267 #ifdef AX12_WRITE_DEBUG 01268 printf(" transmission : 0x%x\n",TxBuf[k]); 01269 #endif 01270 } 01271 01272 01273 } 01274 01275 float AX12::read_and_test(float angle,char* Tab) 01276 { 01277 int k=0; 01278 unsigned short val_angle=0, val_reche=0; 01279 01280 #ifdef AX12_DEBUG 01281 printf("\nread_and_test"); 01282 #endif 01283 01284 if( _ID==0x12) 01285 { k=1;} 01286 else if( _ID==0x04) 01287 { k=6;} 01288 else if( _ID==0x07) 01289 { k=11;} 01290 else if( _ID==0x0F) 01291 { k=16;} 01292 01293 val_angle = (unsigned short) (angle/0.3); 01294 val_reche = (unsigned short) Tab[k] + ((unsigned short)Tab[k+1]<<8); 01295 01296 if((val_angle < (val_reche+(28))) && (val_angle > (val_reche-(28)))) 01297 { 01298 #ifdef AX12_DEBUG 01299 printf("\nreturn1"); 01300 #endif 01301 return 1; 01302 } 01303 else 01304 { 01305 #ifdef AX12_DEBUG 01306 printf("\nreturn0"); 01307 #endif 01308 return 0; 01309 } 01310 01311 } 01312 01313 int AX12::write(int ID, int start, int bytes, char* data, int flag) 01314 { 01315 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum 01316 01317 char TxBuf[16]; 01318 char sum = 0; 01319 char Status[6]; 01320 01321 int timeout = 0; 01322 int plen = 0; 01323 int flag_out = 0; 01324 int timeout_transmit = 0; 01325 int i = 0; 01326 int poubelle = 0; 01327 int count = 0; 01328 char vidage[50]; 01329 01330 typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; 01331 type_etat etat = Header1; 01332 01333 #ifdef AX12_WRITE_DEBUG 01334 printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); 01335 #endif 01336 01337 // Build the TxPacket first in RAM, then we'll send in one go 01338 #ifdef AX12_WRITE_DEBUG 01339 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); 01340 #endif 01341 01342 TxBuf[0] = 0xff; 01343 TxBuf[1] = 0xff; 01344 01345 // ID 01346 TxBuf[2] = ID; 01347 sum += TxBuf[2]; 01348 01349 #ifdef AX12_WRITE_DEBUG 01350 printf(" ID : %d\n",TxBuf[2]); 01351 #endif 01352 01353 // packet Length 01354 TxBuf[3] = 3+bytes; 01355 sum += TxBuf[3]; 01356 01357 #ifdef AX12_WRITE_DEBUG 01358 printf(" Length : %d\n",TxBuf[3]); 01359 #endif 01360 01361 // Instruction 01362 if (flag == 1) { 01363 TxBuf[4]=0x04; 01364 sum += TxBuf[4]; 01365 } else { 01366 TxBuf[4]=0x03; 01367 sum += TxBuf[4]; 01368 } 01369 01370 #ifdef AX12_WRITE_DEBUG 01371 printf(" Instruction : 0x%x\n",TxBuf[4]); 01372 #endif 01373 01374 // Start Address 01375 TxBuf[5] = start; 01376 sum += TxBuf[5]; 01377 01378 #ifdef AX12_WRITE_DEBUG 01379 printf(" Start : 0x%x\n",TxBuf[5]); 01380 #endif 01381 01382 // data 01383 for (char i=0; i<bytes ; i++) { 01384 TxBuf[6+i] = data[i]; 01385 sum += TxBuf[6+i]; 01386 01387 #ifdef AX12_WRITE_DEBUG 01388 printf(" Data : 0x%x\n",TxBuf[6+i]); 01389 #endif 01390 01391 } 01392 01393 // checksum 01394 TxBuf[6+bytes] = 0xFF - sum; 01395 01396 #ifdef AX12_WRITE_DEBUG 01397 printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); 01398 #endif 01399 01400 01401 /* Transmission de la trame construite precedemment dans le tableau TxBuf 01402 */ 01403 while ((timeout_transmit<100) && (i < (7+bytes))) 01404 { 01405 if (_ax12.writeable()) 01406 { 01407 _ax12.putc(TxBuf[i]); 01408 i++; 01409 timeout_transmit = 0; 01410 } 01411 else timeout_transmit++; 01412 } 01413 01414 if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! 01415 { 01416 #ifdef AX12_DEBUG 01417 printf ("TIMEOUT TRANSMIT ERROR\r\n"); 01418 #endif 01419 return(-1); 01420 } 01421 /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur 01422 */ 01423 01424 01425 // Wait for data to transmit 01426 wait (0.005); 01427 01428 // make sure we have a valid return 01429 Status[4]=0x00; 01430 01431 // we'll only get a reply if it was not broadcast 01432 if (_ID!=0xFE) { 01433 01434 01435 /* Partie de reception de la trame de retour 01436 */ 01437 while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) 01438 { 01439 // Les differents etats de l'automate on été créés au debut de la fonction write ! 01440 switch (etat) 01441 { 01442 case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) 01443 { 01444 Status[plen] = _ax12.getc(); 01445 timeout = 0; 01446 if (Status[plen] == 0xFF ) 01447 { 01448 etat = Header2; 01449 #ifdef AX12_DEBUG_WRITE 01450 printf("data[%d] : %d\r\n", plen, (int)Status[plen]); 01451 #endif 01452 plen++; 01453 01454 } 01455 else etat = Header1; 01456 } 01457 else timeout++; 01458 break; 01459 01460 01461 case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) 01462 { 01463 Status[plen] = _ax12.getc(); 01464 timeout = 0; 01465 if (Status[plen] == 0xFF ) 01466 { 01467 etat = ident; 01468 #ifdef AX12_DEBUG_WRITE 01469 printf("data[%d] : %d\r\n", plen, (int)Status[plen]); 01470 #endif 01471 plen++; 01472 } 01473 else 01474 { 01475 etat = Header1; 01476 plen = 0; 01477 } 01478 } 01479 else timeout++; 01480 break; 01481 01482 case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur 01483 { 01484 Status[plen] = _ax12.getc(); 01485 timeout = 0; 01486 if (Status[plen] == ID ) 01487 { 01488 etat = length; 01489 #ifdef AX12_DEBUG_WRITE 01490 printf("data[%d] : %d\r\n", plen, (int)Status[plen]); 01491 #endif 01492 plen++; 01493 } 01494 else 01495 { 01496 etat = Header1; 01497 plen = 0; 01498 } 01499 } 01500 else timeout++; 01501 break; 01502 01503 case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) 01504 { 01505 Status[plen] = _ax12.getc(); 01506 timeout = 0; 01507 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!! 01508 { 01509 etat = erreur; 01510 #ifdef AX12_DEBUG_WRITE 01511 printf("data[%d] : %d\r\n", plen, (int)Status[plen]); 01512 #endif 01513 plen++; 01514 } 01515 else 01516 { 01517 etat = Header1; 01518 plen = 0; 01519 } 01520 } 01521 else timeout++; 01522 break; 01523 01524 case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) 01525 { 01526 Status[plen] = _ax12.getc(); 01527 timeout = 0; 01528 #ifdef AX12_DEBUG_WRITE 01529 printf("data[%d] : %d\r\n", plen, (int)Status[plen]); 01530 #endif 01531 plen++; 01532 etat = checksum; 01533 } 01534 else timeout++; 01535 01536 case checksum: if (_ax12.readable()) // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write 01537 { 01538 Status[plen] = _ax12.getc(); 01539 timeout = 0; 01540 flag_out = 1; 01541 etat = Header1; 01542 #ifdef AX12_DEBUG_WRITE 01543 printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); 01544 #endif 01545 } 01546 else timeout++; 01547 break; 01548 } 01549 } 01550 01551 01552 if ( Status[4] != 0 ) 01553 { 01554 #ifdef AX12_DEBUG 01555 printf ("erreur ! \r\n"); 01556 #endif 01557 for (int i = 0; i<5; i++) 01558 { 01559 #ifdef AX12_DEBUG 01560 printf("data[%d] : %d\r\n", plen, (int)Status[plen]); 01561 #endif 01562 } 01563 } 01564 01565 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 01566 { 01567 #ifdef AX12_DEBUG 01568 printf ("timeout erreur\n\r"); 01569 #endif 01570 return(-1); 01571 } 01572 01573 // Build the TxPacket first in RAM, then we'll send in one go 01574 } 01575 01576 return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) 01577 }
Generated on Tue Jul 12 2022 20:39:16 by
1.7.2