CRAC Team
/
command_AX12_petit_robot_V3
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
command_AX12_petit_robot_v2
by 
CRAC Team
AX12/AX12.cpp
- Committer:
- mathieulegros
- Date:
- 2017-03-15
- Revision:
- 0:a2a44c043932
- Child:
- 2:99b1cb0d9f5e
File content as of revision 0:a2a44c043932:
/* 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)",_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==0x01) //ID Socle
{ k=1;}
else if( _ID==0x02) //ID Bas
{ k=6;}
else if( _ID==0x05) //ID Milieu
{ k=11;}
else if( _ID==0x0D) //ID Haut
{ k=16;}
else if( _ID==0x09) //ID Ventouse
{ k=21;}
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 )
}