CRAC Team
strat des robots
Fork of
CRAC-Strat_2017
by 
CRAC Team
AX12/AX12.cpp
- Committer:
- ClementBreteau
- Date:
- 2017-05-19
- Revision:
- 17:d1594579eec6
- Parent:
- 16:7321fb3bb396
File content as of revision 17:d1594579eec6:
/* 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 "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 )
}