mouvement AX12 petit robot version 2
Fork of command_AX12_petit_robot_v2 by
AX12/AX12.cpp
- Committer:
- ClementBreteau
- Date:
- 2017-05-12
- Revision:
- 7:ad4a19e26b84
- Parent:
- 4:888b32bc5b63
File content as of revision 7:ad4a19e26b84:
/* mbed AX-12+ Servo Library * * Copyright (c) 2010, cstyles (http://mbed.org) * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ #include "mbed.h" #include "AX12.h" #define MAX_TIMEOUT 500 extern Timer t; typedef struct { unsigned short Model_Number; unsigned char Firmware; unsigned char ID; unsigned char Baud_Rate; unsigned char Return_Delay_Time; unsigned short CW_Angle_Limit; unsigned short CCW_Angle_Limit; unsigned char Reserved1; unsigned char Highest_Limit_Temperature; unsigned char Lowest_Limit_voltage; unsigned char Highest_Limit_voltage; unsigned short Max_Torque; unsigned char Status_Return_Level; unsigned char Alarm_LED; unsigned char Alarm_Shutdown; unsigned char Reserved2; unsigned short Down_Calibration; unsigned short Up_Calibration; unsigned char Torque_Enable; unsigned char LED; unsigned char CW_Compliance_Margin; unsigned char CCW_Compliance_Margin; unsigned char CW_Compliance_Slope; unsigned char CCW_Compliance_Slope; unsigned short Goal_Position; unsigned short Moving_Speed; unsigned short Torque_Limit; unsigned short Present_Position; unsigned short Present_Speed; unsigned short Present_Load; unsigned char Present_Voltage; unsigned char Present_Temperature; unsigned char Registered_Instruction; unsigned char Reserved3; unsigned char Moving; unsigned char Lock; unsigned short Punch; } T_AX12; AX12::AX12(PinName tx, PinName rx, int ID, int baud) : _ax12(tx,rx) { _baud = baud; _ID = ID; _ax12.baud(_baud); } int AX12::Set_Secure_Goal(int degres) { int error = 0; int position = 0; int difference = 0; int timeout_secure = 0; int autorisation = 0; position = Get_Position(); error = Set_Goal(degres); while ((autorisation == 0) && (timeout_secure < 100) ) { position = Get_Position(); //printf("position : %d", position ); error = Set_Goal(degres); //printf("degres : %d", degres); difference = degres - position; //printf ("difference : %d", difference ); if (((difference < 2) && (difference > (-2) ))) autorisation = 1; timeout_secure++; } if ( timeout_secure == 100) { #ifdef AX12_DEBUG printf (" timeout secure error "); #endif return(-1); } return(error); } int AX12::Get_Return_Delay_Time(void) { char data[1]; int ErrorCode = read(_ID, AX12_REG_DELAY_TIME, 1, data); int time = data[0]; time = 2.0 * time; return(time); } int AX12::Get_Baud_Rate(void) { char data[1]; int ErrorCode = read(_ID, AX12_REG_BAUD, 1, data); int baud = data[0]; baud = 2000000 / ( baud + 1 ); return(baud); } /** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur // minimum >> Ox000 >> decimal 0 // maximum >> 0x3FF >> decimal 1023 // deflaut >> 0x20 >> decimal 32 */ int AX12::Set_Punch(int punch) { char data[2]; data[0] = punch & 0xff; // bottom 8 bits data[1] = punch >> 8; // top 8 bits // write the packet, return the error code return (write(_ID, AX12_REG_PUNCH, 2, data)); } /** Reset */ int AX12::Reset(int punch) { char data[2]; data[0] = punch & 0xff; // bottom 8 bits data[1] = punch >> 8; // top 8 bits // write the packet, return the error code return (write(_ID, 0x06, 1,data)); } int AX12::Get_Punch (void) { char data[2]; int ErrorCode = read(_ID, AX12_REG_PUNCH, 2, data); int punch = data[0] | (data[1]<<8); return(punch); } int AX12::Get_Load_Direction (void) { char data[2]; int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data); int direction = (data[1]>>2) & 0x01; return(direction); } int AX12::Get_Load_Value (void) { char data[2]; int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data); int Load = (data[0] | (data[1]<<8)) & 0x3FF; return(Load); } int AX12::Get_Present_Speed (void) { char data[2]; int ErrorCode = read(_ID, AX12_REG_PRESENT_SPEED, 2, data); int speed = data[0] | (data[1]<<8); return(speed); } int AX12::Get_CCW_Angle_Limit (void) { char data[2]; int ErrorCode = read(_ID, AX12_REG_CCW_LIMIT, 2, data); int angle = data[0] | (data[1]<<8); angle = (angle * 300) / 1023; return(angle); } int AX12::Get_CW_Angle_Limit (void) { char data[2]; int ErrorCode = read(_ID, AX12_REG_CW_LIMIT, 2, data); int angle = data[0] | (data[1]<<8); angle = (angle * 300) / 1023; return(angle); } int AX12::Get_Torque_Enable(void) { char data[1]; int ErrorCode = read(_ID, AX12_REG_TORQUE_ENABLE, 1, data); int enable = data[0]; return(enable); } int AX12::Set_Torque_Enable(int etat) { char data[1]; data [0] = etat; int error = write(_ID, AX12_REG_TORQUE_ENABLE, 1, data); return (error); } int AX12::Get_Up_Calibration (void) { char data[1]; int ErrorCode = read(_ID, AX12_REG_UP_CALIBRATION, 2, data); int Up_calibration = data[0] | (data[1]<<8); return(Up_calibration); } int AX12::Get_Down_Calibration (void) { char data[1]; int ErrorCode = read(_ID, AX12_REG_DOWN_CALIBRATION, 2, data); int Dowm_calibration = data[0] | (data[1]<<8); return(Dowm_calibration); } int AX12::Get_ID(void) { char data[1]={-1}; int ErrorCode = read(_ID, AX12_REG_ID, 1, data); int id = data[0]; return(id); } // Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 ) int AX12::Get_Max_Torque (void) { char data[2]; int ErrorCode = read(_ID, AX12_REG_MAX_TORQUE, 2, data); int torque = data[0] | (data[1]<<8); return(torque); } /** Reglage du couple maximum de l'actionneur // minimum >> Ox000 >> decimal 0 // maximum >> 0x3FF >> decimal 1023 // deflaut >> >> decimal */ int AX12::Set_Max_Torque(int torque) { char data[2]; data[0] = torque & 0xff; // bottom 8 bits data[1] = torque >> 8; // top 8 bits // write the packet, return the error code return (write(_ID, AX12_REG_MAX_TORQUE, 2, data)); } /** Reglage de la desactivation des actionneurs si une erreur le concernant se produit Bit Function Bit 7 0 Bit 6 If set to 1, torque off when an Instruction Error occurs Bit 5 If set to 1, torque off when an Overload Error occurs Bit 4 If set to 1, torque off when a Checksum Error occurs Bit 3 If set to 1, torque off when a Range Error occurs Bit 2 If set to 1, torque off when an Overheating Error occurs Bit 1 If set to 1, torque off when an Angle Limit Error occurs Bit 0 If set to 1, torque off when an Input Voltage Error occurs */ int AX12::Set_Alarm_Shutdown(int valeur) { char data[1]; data [0] = valeur; int val_alarm_shutdown = write(_ID, AX12_REG_ALARM_SHUTDOWN, 1, data); return (val_alarm_shutdown); } /** Reglage de l'activation de l'alarme Bit Function Bit 7 0 Bit 6 If set to 1, the LED blinks when an Instruction Error occurs Bit 5 If set to 1, the LED blinks when an Overload Error occurs Bit 4 If set to 1, the LED blinks when a Checksum Error occurs Bit 3 If set to 1, the LED blinks when a Range Error occurs Bit 2 If set to 1, the LED blinks when an Overheating Error occurs Bit 1 If set to 1, the LED blinks when an Angle Limit Error occurs Bit 0 If set to 1, the LED blinks when an Input Voltage Error occurs */ int AX12::Set_Alarm_LED(int valeur) { char data[1]; data [0] = valeur; int val_alarmLED = write(_ID, AX12_REG_ALARM_LED, 1, data); return (val_alarmLED); } // Reglage de la réponse à une instruction // 0 >> ne repond a aucune instructions // 1 >> repond seulement aux instructions READ_DATA // 2 >> repond à toutes les instructions int AX12::Set_Status_Return_Level(int etat) { char data[1]; data [0] = etat; int val_return_lvl = write(_ID, AX12_REG_SATUS_RETURN, 1, data); return (val_return_lvl); } // Reglage de la tension minimale // minimum >> Ox32 >> decimal 50 // maximum >> 0xFA >> decimal 250 // deflaut >> 0x3C >> decimal 60 int AX12::Set_Lowest_Voltage(int val_lowest_voltage) { char data[1]; data [0] = val_lowest_voltage; int val_lowvolt = write(_ID, AX12_REG_LOWEST_VOLTAGE, 1, data); return (val_lowvolt); } // Reglage de la tension maximale // minimum >> Ox32 >> decimal 50 // maximum >> 0xFA >> decimal 250 // deflaut >> 0xBE >> decimal 190 int AX12::Set_Highest_Voltage(int val_highest_voltage) { char data[1]; data [0] = val_highest_voltage; int val_highvolt = write(_ID, AX12_REG_HIGHEST_VOLTAGE, 1, data); return (val_highvolt); } // Reglage du return time delay EN MICRO SECONDE 2uSec * val_delay_time // minimum >> 0 us // maximum >> 508 us // deflaut >> 125 us int AX12::Set_Delay_Time (int val_delay_time ) { char data[1]; data [0] = val_delay_time/2.0; int valdelay_time = write(_ID, AX12_REG_DELAY_TIME, 1, data); return (valdelay_time ); } // Reglage de la température max du cervo // minimum >> Ox00 >> decimal 0 // maximum >> 0x96 >> decimal 150 int AX12::Set_Temperature_Max (int val_temperature ) { char data[1]; data [0] = val_temperature; int valtemp_max = write(_ID, AX12_REG_TEMP_MAX, 1, data); return (valtemp_max ); } // Etat LED // 0 = off // 1 = on int AX12::Set_Etat_LED(int etat) { char data[1]; data [0] = etat; int valLED = write(_ID, AX12_REG_LED, 1, data); return (valLED); } // Set the mode of the servo // 0 = Positional (0-300 degrees) // 1 = Rotational -1 to 1 speed int AX12::Set_Mode(int mode) { if (mode == 1) { // set CR //wait(0.001); Set_CW_Angle_Limit(0); //wait(0.001); Set_CCW_Angle_Limit(0); //wait(0.001); Set_CR_Speed(0.0); //wait(0.001); } else { //wait(0.001); Set_CW_Angle_Limit(0); //wait(0.001); Set_CCW_Angle_Limit(300); //wait(0.001); Set_CR_Speed(0.0); //wait(0.001); } return(0); } int AX12::Set_Goal_speed(int speed, int flags) { char reg_flag = 0; char data[2]; // set the flag is only the register bit is set in the flag if (flags == 0x2) { reg_flag = 1; } data[0] = speed & 0xff; // bottom 8 bits data[1] = speed >> 8; // top 8 bits // write the packet, return the error code int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, data, reg_flag); /*if (flags == 1) { // block until it comes to a halt while (isMoving()) { } }*/ return(rVal); } // if flag[0] is set, were blocking // if flag[1] is set, we're registering // they are mutually exclusive operations int AX12::Set_Goal(int degrees, int flags) { char reg_flag = 0; char data[2]; // set the flag is only the register bit is set in the flag if (flags == 0x2) { reg_flag = 1; } // 1023 / 300 * degrees short goal = (1023 * degrees) / 300; #ifdef AX12_DEBUG printf("SetGoal to 0x%x\n",goal); #endif data[0] = goal & 0xff; // bottom 8 bits data[1] = goal >> 8; // top 8 bits // write the packet, return the error code int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag); /*if (flags == 1) { // block until it comes to a halt while (isMoving()) { } }*/ return(rVal); } // Set continuous rotation speed from -1 to 1 int AX12::Set_CR_Speed(float speed) { // bit 10 = direction, 0 = CCW, 1=CW // bits 9-0 = Speed char data[2]; int goal = (0x3ff * abs(speed)); // Set direction CW if we have a negative speed if (speed < 0) { goal |= (0x1 << 10); } data[0] = goal & 0xff; // bottom 8 bits data[1] = goal >> 8; // top 8 bits // write the packet, return the error code int rVal = write(_ID, 0x20, 2, data); return(rVal); } int AX12::Set_CW_Angle_Limit (int degrees) { char data[2]; // 1023 / 300 * degrees short limit = (1023 * degrees) / 300; #ifdef AX12_DEBUG printf("SetCWLimit to 0x%x\n",limit); #endif data[0] = limit & 0xff; // bottom 8 bits data[1] = limit >> 8; // top 8 bits // write the packet, return the error code return (write(_ID, AX12_REG_CW_LIMIT, 2, data)); } int AX12::Set_CCW_Angle_Limit (int degrees) { char data[2]; // 1023 / 300 * degrees short limit = (1023 * degrees) / 300; #ifdef AX12_DEBUG printf("SetCCWLimit to 0x%x\n",limit); #endif data[0] = limit & 0xff; // bottom 8 bits data[1] = limit >> 8; // top 8 bits // write the packet, return the error code return (write(_ID, AX12_REG_CCW_LIMIT, 2, data)); } int AX12::Set_ID (int CurrentID, int NewID) { char data[1]; data[0] = NewID; #ifdef AX12_DEBUG printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); #endif return (write(CurrentID, AX12_REG_ID, 1, data)); } int AX12::Set_Baud (int baud) { char data[1]; data[0] = baud; #ifdef AX12_DEBUG printf("Setting Baud rate to %d\n",baud); #endif return (write(_ID, AX12_REG_BAUD, 1, data)); } // return 1 is the servo is still in flight int AX12::isMoving(void) { char data[1]; read(_ID,AX12_REG_MOVING,1,data); return(data[0]); } void AX12::reset() { unsigned char TxBuf[16]; unsigned char sum = 0; unsigned long debut=0; #ifdef AX12_TRIGGER_DEBUG // Build the TxPacket first in RAM, then we'll send in one go printf("\nreset\n"); printf("\nreset Packet\n Header : 0xFF, 0xFF\n"); #endif TxBuf[0] = 0xFF; TxBuf[1] = 0xFF; // ID - Broadcast TxBuf[2] =_ID; sum += TxBuf[2]; #ifdef AX12_TRIGGER_DEBUG printf(" ID : %d\n",TxBuf[2]); #endif // Length TxBuf[3] = 0x02; sum += TxBuf[3]; #ifdef AX12_TRIGGER_DEBUG printf(" Length %d\n",TxBuf[3]); #endif // Instruction - ACTION TxBuf[4] = 0x06; //reset sum += TxBuf[4]; #ifdef AX12_TRIGGER_DEBUG printf(" Instruction 0x%X\n",TxBuf[5]); #endif // Checksum TxBuf[5] = 0xFF - sum; //#ifdef AX12_TRIGGER_DEBUG printf(" Checksum 0x%X\n",TxBuf[5]); //#endif // Transmit the packet in one burst with no pausing for (int i = 0; i < 6 ; i++) { while(_ax12.writeable()==0); _ax12.putc(TxBuf[i]); } wait(0.001); debut=t.read_ms(); do { if (_ax12.readable()==-1) // reception du premier Header ( 0xFF ) printf("%02x",_ax12.getc()); } while((t.read_ms()-debut)<500); printf("\n"); return; } void AX12::read_all_info(unsigned char start, unsigned char longueur) { unsigned char TxBuf[16]; unsigned char sum = 0; unsigned long debut=0; #ifdef AX12_TRIGGER_DEBUG // Build the TxPacket first in RAM, then we'll send in one go printf("\nreset\n"); printf("\nreset Packet\n Header : 0xFF, 0xFF\n"); #endif TxBuf[0] = 0xFF; TxBuf[1] = 0xFF; // ID - Broadcast TxBuf[2] =_ID; sum += TxBuf[2]; #ifdef AX12_TRIGGER_DEBUG printf(" ID : %d\n",TxBuf[2]); #endif // Length TxBuf[3] = 0x04; sum += TxBuf[3]; #ifdef AX12_TRIGGER_DEBUG printf(" Length %d\n",TxBuf[3]); #endif // Instruction - ACTION TxBuf[4] = INST_READ; //reset sum += TxBuf[4]; #ifdef AX12_TRIGGER_DEBUG printf(" Instruction 0x%X\n",TxBuf[4]); #endif TxBuf[5] = start; //reset sum += TxBuf[5]; TxBuf[6] = longueur; //reset sum += TxBuf[6]; // Checksum TxBuf[7] = 0xFF - sum; //#ifdef AX12_TRIGGER_DEBUG //printf(" Checksum 0x%X\n\r",TxBuf[7]); //#endif // Transmit the packet in one burst with no pausing for (int i = 0; i < 8 ; i++) { while(_ax12.writeable()==0); _ax12.putc(TxBuf[i]); } debut=t.read_ms(); int i=0; do { if (_ax12.readable()) { // reception du premier Header ( 0xFF ) printf("%02d:%02x ",start+i,_ax12.getc()); i++; } } while((t.read_ms()-debut)<5000); printf("\n"); return; } void AX12::trigger(void) { char TxBuf[16]; char sum = 0; #ifdef AX12_TRIGGER_DEBUG // Build the TxPacket first in RAM, then we'll send in one go printf("\nTriggered\n"); printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); #endif TxBuf[0] = 0xFF; TxBuf[1] = 0xFF; // ID - Broadcast TxBuf[2] = 0xFE; sum += TxBuf[2]; #ifdef AX12_TRIGGER_DEBUG printf(" ID : %d\n",TxBuf[2]); #endif // Length TxBuf[3] = 0x02; sum += TxBuf[3]; #ifdef AX12_TRIGGER_DEBUG printf(" Length %d\n",TxBuf[3]); #endif // Instruction - ACTION TxBuf[4] = 0x04; sum += TxBuf[4]; #ifdef AX12_TRIGGER_DEBUG printf(" Instruction 0x%X\n",TxBuf[5]); #endif // Checksum TxBuf[5] = 0xFF - sum; #ifdef AX12_TRIGGER_DEBUG printf(" Checksum 0x%X\n",TxBuf[5]); #endif // Transmit the packet in one burst with no pausing for (int i = 0; i < 6 ; i++) { _ax12.putc(TxBuf[i]); } // This is a broadcast packet, so there will be no reply return; } float AX12::Get_Position(void) { #ifdef AX12_DEBUG printf("\nGetPositionID(%d) \n\r",_ID); #endif char data[2]; int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); int position = data[0] | (data[1] << 8); float angle = ((float)position * 300.0)/1023.0; return (angle); } float AX12::Get_Temp () { #ifdef AX12_DEBUG printf("\nGetTemp(%d)",_ID); #endif char data[1]; int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data); float temp = data[0]; return(temp); } float AX12::Get_Volts (void) { #ifdef AX12_DEBUG printf("\nGetVolts(%d)",_ID); #endif char data[1]; int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data); float volts = data[0]/10.0; return(volts); } int AX12::read(int ID, int start, int bytes, char* data) { char PacketLength = 0x3; char TxBuf[16]; char sum = 0; char Status[16]; int timeout = 0; int plen = 0; int flag_out = 0; int timeout_transmit = 0; int i = 0; int enable = 0; // int poubelle = 0; // int count = 0; // char vidage[50]; typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat; type_etat etat = Header1; Status[4] = 0xFE; // return code /*********************************** CREATION DE LA TRAME A EVOYER *****************************************/ #ifdef AX12_READ_DEBUG printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); #endif // Build the TxPacket first in RAM, then we'll send in one go #ifdef AX12_READ_DEBUG printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); #endif TxBuf[0] = 0xff; TxBuf[1] = 0xff; // ID TxBuf[2] = ID; sum += TxBuf[2]; #ifdef AX12_READ_DEBUG printf(" ID : %d\n",TxBuf[2]); #endif // Packet Length TxBuf[3] = 4;//PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes) sum += TxBuf[3]; // Accululate the packet sum #ifdef AX12_READ_DEBUG printf(" Length : 0x%x\n",TxBuf[3]); #endif // Instruction - Read TxBuf[4] = 0x2; sum += TxBuf[4]; #ifdef AX12_READ_DEBUG printf(" Instruction : 0x%x\n",TxBuf[4]); #endif // Start Address TxBuf[5] = start; sum += TxBuf[5]; #ifdef AX12_READ_DEBUG printf(" Start Address : 0x%x\n",TxBuf[5]); #endif // Bytes to read TxBuf[6] = bytes; sum += TxBuf[6]; #ifdef AX12_READ_DEBUG printf(" No bytes : 0x%x\n",TxBuf[6]); #endif // Checksum TxBuf[7] = 0xFF - sum; #ifdef AX12_READ_DEBUG printf(" Checksum : 0x%x\n",TxBuf[7]); #endif /********************************************TRAME CONSTRUITE DANS TxBuf***************************************/ /* Transmission de la trame construite precedemment dans le tableau TxBuf */ while ((timeout_transmit<5000) && (i < (7+bytes))) { if (_ax12.writeable()) { _ax12.putc(TxBuf[i]); i++; timeout_transmit = 0; } else timeout_transmit++; } if (timeout_transmit == 5000 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! { #ifdef AX12_DEBUG printf ("timeout transmit erreur\r\n"); #endif return(-1); } /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur */ // Wait for the bytes to be transmitted wait (0.001); // Skip if the read was to the broadcast address if (_ID != 0xFE) { /* Partie de reception de la trame de retour */ while ((flag_out != 1) && (timeout < (1000*bytes))) { // Les differents etats de l'automate on été créés au debut de la fonction write ! switch (etat) { case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) { Status[plen] = _ax12.getc(); timeout = 0; if (Status[plen] == 0xFF ) { etat = Header2; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else etat = Header1; } else timeout++; break; case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) { Status[plen] = _ax12.getc(); timeout = 0; if (Status[plen] == 0xFF ) { etat = ident; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else if (Status[plen] == ID ) // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!! { Status[plen] = 0; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; Status[plen] = ID; etat = length; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur { Status[plen] = _ax12.getc(); timeout = 0; if (Status[plen] == ID ) { etat = length; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) { Status[plen] = _ax12.getc(); timeout = 0; if (Status[plen] == (bytes+2) ) { etat = erreur; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) { Status[plen] = _ax12.getc(); timeout = 0; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; etat = reception; } else timeout++; case reception: while ( enable < bytes ) // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length ) { if (_ax12.readable()) { Status[plen] = _ax12.getc(); timeout = 0; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; enable++; } else timeout++; } etat = checksum; break; 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!! { Status[plen] = _ax12.getc(); timeout = 0; flag_out = 1; etat = Header1; #ifdef AX12_DEBUG_READ printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); #endif } else timeout++; break; default: break; } } 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 { #ifdef AX12_DEBUG printf ("timeout erreur\n"); #endif return(-1); } // copie des données dans le tableau data for (int i=0; i < Status[3]-2 ; i++) { data[i] = Status[5+i]; } } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE) return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) } void AX12::multiple_goal_and_speed(int number_ax12,char* tab) { char TxBuf[50]; char sum = 0; int timeout_transmit =0; int i=0, k=0, j=0; int L=4; // nombre instructions par paquets int bytes= ((L+1)*number_ax12)+4; typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; type_etat etat= Header1; for(j=0; j<50; j++) { TxBuf[i]=0; } #ifdef AX12_WRITE_DEBUG //printf(" MULTIPLE_GOAL_AND_SPEED \n "); #endif // Build the TxPacket first in RAM, then we'll send in one go #ifdef AX12_WRITE_DEBUG //printf("\nInstruction Packet\n Header :%d, %d\n",TxBuf[0], TxBuf[1]); #endif TxBuf[0]=0xFF; // bit de start TxBuf[1]=0xFF; // bit de start TxBuf[2] = 0xFE; //ID broadcast sum += TxBuf[2]; #ifdef AX12_WRITE_DEBUG printf(" adresse de difusion : %d\n",TxBuf[2]); #endif TxBuf[3] =bytes; // longueur sum += TxBuf[3]; #ifdef AX12_WRITE_DEBUG printf(" Longueur : %d\n",TxBuf[3]); #endif TxBuf[4]=0x83; //SYNC_WRITE sum += TxBuf[4]; #ifdef AX12_WRITE_DEBUG printf(" Instruction : 0x%x\n",TxBuf[4]); #endif TxBuf[5] = 0x1E; // addresse "GOAL_POSITION" sum += TxBuf[5]; #ifdef AX12_WRITE_DEBUG printf(" Adresse de debut : 0x%x\n",TxBuf[5]); #endif TxBuf[6]=L; // Nombre instruction par paquets sum += TxBuf[6]; #ifdef AX12_WRITE_DEBUG printf(" nombre instruction/paquet : 0x%x\n",TxBuf[6]); #endif for(i=0; i<(number_ax12*5); i++) // Copie des data de TAB sur TxBuf { TxBuf[i+7]=tab[i]; sum += TxBuf[i+7]; } #ifdef AX12_WRITE_DEBUG for(i=0; i<(number_ax12*5); i++) { printf(" Data : 0x%x\n",TxBuf[i+7]); } #endif TxBuf[(number_ax12*5)+7] = 0xFF - sum ; // CHECKSUM #ifdef AX12_WRITE_DEBUG printf(" Checksum : 0x%x\n",TxBuf[(number_ax12*5)+9]); #endif for(k=0; k<((number_ax12*5)+8); k++) // TRANSMISSION DE LA TRAME { _ax12.putc(TxBuf[k]); #ifdef AX12_WRITE_DEBUG printf(" transmission : 0x%x\n",TxBuf[k]); #endif } } float AX12::read_and_test(float angle,char* Tab) { int k=0; unsigned short val_angle=0, val_reche=0; #ifdef AX12_DEBUG printf("\nread_and_test"); #endif if( _ID==0x12) { k=1;} else if( _ID==0x04) { k=6;} else if( _ID==0x07) { k=11;} else if( _ID==0x0F) { k=16;} val_angle = (unsigned short) (angle/0.3); val_reche = (unsigned short) Tab[k] + ((unsigned short)Tab[k+1]<<8); if((val_angle < (val_reche+(28))) && (val_angle > (val_reche-(28)))) { #ifdef AX12_DEBUG printf("\nreturn1"); #endif return 1; } else { #ifdef AX12_DEBUG printf("\nreturn0"); #endif return 0; } } int AX12::write(int ID, int start, int bytes, char* data, int flag) { // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum char TxBuf[16]; char sum = 0; char Status[6]; int timeout = 0; int plen = 0; int flag_out = 0; int timeout_transmit = 0; int i = 0; int poubelle = 0; int count = 0; char vidage[50]; typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat; type_etat etat = Header1; #ifdef AX12_WRITE_DEBUG printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); #endif // Build the TxPacket first in RAM, then we'll send in one go #ifdef AX12_WRITE_DEBUG printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); #endif TxBuf[0] = 0xff; TxBuf[1] = 0xff; // ID TxBuf[2] = ID; sum += TxBuf[2]; #ifdef AX12_WRITE_DEBUG printf(" ID : %d\n",TxBuf[2]); #endif // packet Length TxBuf[3] = 3+bytes; sum += TxBuf[3]; #ifdef AX12_WRITE_DEBUG printf(" Length : %d\n",TxBuf[3]); #endif // Instruction if (flag == 1) { TxBuf[4]=0x04; sum += TxBuf[4]; } else { TxBuf[4]=0x03; sum += TxBuf[4]; } #ifdef AX12_WRITE_DEBUG printf(" Instruction : 0x%x\n",TxBuf[4]); #endif // Start Address TxBuf[5] = start; sum += TxBuf[5]; #ifdef AX12_WRITE_DEBUG printf(" Start : 0x%x\n",TxBuf[5]); #endif // data for (char i=0; i<bytes ; i++) { TxBuf[6+i] = data[i]; sum += TxBuf[6+i]; #ifdef AX12_WRITE_DEBUG printf(" Data : 0x%x\n",TxBuf[6+i]); #endif } // checksum TxBuf[6+bytes] = 0xFF - sum; #ifdef AX12_WRITE_DEBUG printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); #endif /* Transmission de la trame construite precedemment dans le tableau TxBuf */ while ((timeout_transmit<100) && (i < (7+bytes))) { if (_ax12.writeable()) { _ax12.putc(TxBuf[i]); i++; timeout_transmit = 0; } else timeout_transmit++; } if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer ! { #ifdef AX12_DEBUG printf ("TIMEOUT TRANSMIT ERROR\r\n"); #endif return(-1); } /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur */ // Wait for data to transmit wait (0.005); // make sure we have a valid return Status[4]=0x00; // we'll only get a reply if it was not broadcast if (_ID!=0xFE) { /* Partie de reception de la trame de retour */ while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) { // Les differents etats de l'automate on été créés au debut de la fonction write ! switch (etat) { case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF ) { Status[plen] = _ax12.getc(); timeout = 0; if (Status[plen] == 0xFF ) { etat = Header2; #ifdef AX12_DEBUG_WRITE printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else etat = Header1; } else timeout++; break; case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF ) { Status[plen] = _ax12.getc(); timeout = 0; if (Status[plen] == 0xFF ) { etat = ident; #ifdef AX12_DEBUG_WRITE printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur { Status[plen] = _ax12.getc(); timeout = 0; if (Status[plen] == ID ) { etat = length; #ifdef AX12_DEBUG_WRITE printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres ) { Status[plen] = _ax12.getc(); timeout = 0; 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!! { etat = erreur; #ifdef AX12_DEBUG_WRITE printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; } else { etat = Header1; plen = 0; } } else timeout++; break; case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur ) { Status[plen] = _ax12.getc(); timeout = 0; #ifdef AX12_DEBUG_WRITE printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif plen++; etat = checksum; } else timeout++; case checksum: if (_ax12.readable()) // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write { Status[plen] = _ax12.getc(); timeout = 0; flag_out = 1; etat = Header1; #ifdef AX12_DEBUG_WRITE printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]); #endif } else timeout++; break; } } if ( Status[4] != 0 ) { #ifdef AX12_DEBUG printf ("erreur ! \r\n"); #endif for (int i = 0; i<5; i++) { #ifdef AX12_DEBUG printf("data[%d] : %d\r\n", plen, (int)Status[plen]); #endif } } 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 { #ifdef AX12_DEBUG printf ("timeout erreur\n\r"); #endif return(-1); } // Build the TxPacket first in RAM, then we'll send in one go } return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour ) }