CRAC Team
/
Strategie_13h30
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
CRAC-Strat_2017_homologation_gros_rob
by 
CRAC Team
AX12-V2/AX12-V2.cpp
- Committer:
- antbig
- Date:
- 2016-04-15
- Revision:
- 1:116040d14164
- Parent:
- 0:ad97421fb1fb
- Child:
- 5:dcd817534b57
File content as of revision 1:116040d14164:
#include "AX12-V2.h"
struct S_AX12 AX12_data[MAX_AX12];//La liste de tous les AX12 du robot possible, aussi bien par CAN que en local
SerialHalfDuplex AX12_Serial = SerialHalfDuplex(p9,p10);
int lastAX12Use = 0;
FunctionPointer AX12_CallbackEnd;//Fonction de callback lors de la fin d'un mouvement d'un AX12
/****************************************************************************************/
/* FUNCTION NAME: AX12_register */
/* DESCRIPTION : Indiquer qu'un AX12 est connecté à la carte */
/****************************************************************************************/
void AX12_register(unsigned char id, unsigned short speed)
{
int localID = AX12_getLocalID(id);
char data[2];
AX12_data[localID].isUsingCAN = 0;//On indique que l'AX12 est connecté localement
if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
AX12_data[lastAX12Use].speed = speed;
/*
data[0] = speed & 0xff; // bottom 8 bits
data[1] = speed >> 8; // top 8 bits
*/
AX12_Serial.baud(1000000);//On indique la vitesse de transmission des AX12
/*
AX12_write(id,AX12_REG_MOVING_SPEED,2,data);
*/
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_setGoal */
/* DESCRIPTION : Definir la position d'un ax12, !!Ne déclanche pas le mouvement */
/****************************************************************************************/
void AX12_setGoal(unsigned char id, unsigned short goal, unsigned short speed)
{
int localID = AX12_getLocalID(id);//On récupère les info sur l'AX12
CANMessage msgTx=CANMessage();
AX12_data[localID].needToUpdate = 1;
AX12_data[localID].goal = goal;
if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
if(speed != 0x3FF)
AX12_data[localID].speed = speed;
if(AX12_data[localID].isUsingCAN != 0) {//Il faut envoyer la trame CAN car l'AX12 est sur une autre carte
msgTx.id=SERVO_AX12_SETGOAL;
msgTx.len=5;
msgTx.format=CANStandard;
msgTx.type=CANData;
// id de l'AX12 sur 1 octet
msgTx.data[0]=(unsigned char)id;
// Position de l'AX12 sur 2 octet
msgTx.data[1]=(unsigned char)goal;
msgTx.data[2]=(unsigned char)(goal>>8);
//Vitesse de l'AX12 sur 2 octet
msgTx.data[3]=(unsigned char)AX12_data[localID].speed;
msgTx.data[4]=(unsigned char)(AX12_data[localID].speed>>8);
can1.write(msgTx);
}
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_isLocal */
/* DESCRIPTION : Savoir si un AX12 est enregistré sur la carte */
/****************************************************************************************/
unsigned char AX12_isLocal(unsigned char id)
{
int i=0;
for(i=0;i<MAX_AX12;i++)
{
if(AX12_data[i].id == id && AX12_data[i].isUsingCAN == 0) return 1;
}
return 0;
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_getLocalID */
/* DESCRIPTION : Obtenir les info sur un AX12 ou l'initialiser si non présent */
/****************************************************************************************/
int AX12_getLocalID(unsigned char id)
{
int i=0;
for(i=0;i<MAX_AX12;i++)
{
if(AX12_data[i].id == id) return i;
}
//Si l'AX12 n'est pas déjà initialisé, on l'initialise
AX12_data[lastAX12Use].id = id;//
AX12_data[lastAX12Use].goal = 0;//
AX12_data[lastAX12Use].speed = 0x320;//
AX12_data[lastAX12Use].isUsingCAN = 1;//Indique qu'il faut envoyer le message via CAN
AX12_data[lastAX12Use].needToUpdate = 0;//
lastAX12Use++;
return lastAX12Use-1;
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_notifyCANEnd */
/* DESCRIPTION : indiquer qu'un mouvement d'AX12 CAN est terminé */
/****************************************************************************************/
void AX12_notifyCANEnd(unsigned char id)
{
if(waitingAckFrom == SERVO_AX12_DONE) {
waitingAckFrom = 0;
waitingAckID = 0;
}
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_doLoop */
/* DESCRIPTION : Boucle de vérification de la position des AX12 */
/****************************************************************************************/
void AX12_doLoop(void)
{
int i=0;
CANMessage msgTx=CANMessage();
for(i=0;i<MAX_AX12;i++)
{
if(AX12_data[i].isUsingCAN == 0 && AX12_data[i].needCheckMoving == 1)//Il faut vérifier si l'AX12 a terminé de bouger
{
if(AX12_isMoving(AX12_data[i].id) == 0) {//L'AX12 a terminé de bouger
AX12_data[i].needCheckMoving = 0;
msgTx.id=SERVO_AX12_DONE;
msgTx.len=1;
msgTx.format=CANStandard;
msgTx.type=CANData;
// id de l'AX12 sur 1 octet
msgTx.data[0]=(unsigned char)AX12_data[i].id;
can1.write(msgTx);
AX12_notifyCANEnd(AX12_data[i].id);
}
}
}
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_processChange */
/* DESCRIPTION : Permet de prendre en compte les changement d'instruction des AX12 */
/* Début du mouvement à partir de l'appel de cette fonction */
/****************************************************************************************/
void AX12_processChange(void)
{
int i=0;
int dataToSendLength = 0;
char dataToSend[100];
for(i=0;i<MAX_AX12;i++)
{
if(AX12_data[i].needToUpdate == 1) //Il faut mettre à jour la position de l'AX12
{
if(AX12_data[i].isUsingCAN == 0)//Il faut envoyer la trame en local
{
if(dataToSendLength == 0)
dataToSend[dataToSendLength++] = 4;//length data
dataToSend[dataToSendLength++] = AX12_data[i].id;//ID servo1
dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) & 0xff;// bottom 8 bits
dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) >> 8; // top 8 bits
dataToSend[dataToSendLength++] = (AX12_data[i].speed) & 0xff;// bottom 8 bits
dataToSend[dataToSendLength++] = (AX12_data[i].speed) >> 8; // top 8 bits
AX12_data[i].needCheckMoving = 1;
}
AX12_data[i].needToUpdate = 0;//Remise à 0 de l'indicatif de mise à jour
}
}
SendRawId(SERVO_AX12_PROCESS);//On indique par CAN qu'il faut bouger les AX12
if(dataToSendLength > 0)//Il y a des données à envoyer en local
{
AX12_syncWrite(AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
}
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_isMoving */
/* DESCRIPTION : Fonction pour savoir si un AX12 local est entrain de bouger */
/****************************************************************************************/
int AX12_isMoving(unsigned char id)
{
char data[1];
AX12_read(id,AX12_REG_MOVING,1,data);
return(data[0]);
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_syncWrite */
/* DESCRIPTION : Fonction pour envoyer des trames aux AX12 en mode syncWrite */
/****************************************************************************************/
int AX12_syncWrite(int start, int bytes, char* data)
{
//0 : 0xff
//1 : 0xff
//2 : ID de l'AX12 ou 0xFE pour le broadcast
//3 : Length => longueur de la trame
//4 : Intruction(write) => id de l'instruction 0x83 pour le syncwrite
//5 : Address => addresse du registre à modifier
//6+ : Data => les données à transmettre
//last : Checksum
int ID = 0xFE;//Toujours 0xFE dans le cas d'un broadcast
char TxBuf[60];
char sum = 0;
int timeout_transmit = 0;
int i = 0;
printf("Start sending moving trame\n");
// Build the TxPacket first in RAM, then we'll send in one go
TxBuf[0] = 0xff;
TxBuf[1] = 0xff;
// ID
TxBuf[2] = ID;
sum += TxBuf[2];
// packet Length
TxBuf[3] = 3+bytes;
sum += TxBuf[3];
// Instruction
TxBuf[4]=0x83;//toujours 0x83 dans le cas d'un syncwrite
sum += TxBuf[4];
// Start Address
TxBuf[5] = start;//addresse du registre à modifier
sum += TxBuf[5];
// data
for (char i=0; i<bytes ; i++) {
TxBuf[6+i] = data[i];
sum += TxBuf[6+i];
printf(" Data : 0x%x\n",TxBuf[6+i]);
}
// checksum
TxBuf[6+bytes] = 0xFF - sum;
/* Transmission de la trame construite precedemment dans le tableau TxBuf
*/
while ((timeout_transmit<100) && (i < (7+bytes))) {
if (AX12_Serial.writeable()) {
AX12_Serial.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 !
return(-1);
}
// Wait for data to transmit
wait (0.005);
return(0);//OK trame envoyé
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_write */
/* DESCRIPTION : Fonction pour envoyer des trames aux AX12 */
/****************************************************************************************/
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;
// Build the TxPacket first in RAM, then we'll send in one go
TxBuf[0] = 0xff;
TxBuf[1] = 0xff;
// ID
TxBuf[2] = ID;
sum += TxBuf[2];
// packet Length
TxBuf[3] = 3+bytes;
sum += TxBuf[3];
// Instruction
if (flag == 1) {
TxBuf[4]=0x04;
sum += TxBuf[4];
} else {
TxBuf[4]=0x03;
sum += TxBuf[4];
}
// Start Address
TxBuf[5] = start;
sum += TxBuf[5];
// data
for (char i=0; i<bytes ; i++) {
TxBuf[6+i] = data[i];
sum += TxBuf[6+i];
}
// checksum
TxBuf[6+bytes] = 0xFF - sum;
/* Transmission de la trame construite precedemment dans le tableau TxBuf
*/
while ((timeout_transmit<100) && (i < (7+bytes))) {
if (AX12_Serial.writeable()) {
AX12_Serial.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 !
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_Serial.readable()) { // reception du premier Header ( 0xFF )
Status[plen] = AX12_Serial.getc();
timeout = 0;
if (Status[plen] == 0xFF ) {
etat = Header2;
plen++;
} else etat = Header1;
} else timeout++;
break;
case Header2:
if (AX12_Serial.readable()) { // reception du second Header ( 0xFF )
Status[plen] = AX12_Serial.getc();
timeout = 0;
if (Status[plen] == 0xFF ) {
etat = ident;
plen++;
} else {
etat = Header1;
plen = 0;
}
} else timeout++;
break;
case ident:
if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur
Status[plen] = AX12_Serial.getc();
timeout = 0;
if (Status[plen] == ID ) {
etat = length;
plen++;
} else {
etat = Header1;
plen = 0;
}
} else timeout++;
break;
case length:
if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
Status[plen] = AX12_Serial.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;
plen++;
} else {
etat = Header1;
plen = 0;
}
} else timeout++;
break;
case erreur:
if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
Status[plen] = AX12_Serial.getc();
timeout = 0;
plen++;
etat = checksum;
} else timeout++;
case checksum:
if (AX12_Serial.readable()) { // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write
Status[plen] = AX12_Serial.getc();
timeout = 0;
flag_out = 1;
etat = Header1;
} else timeout++;
break;
}
}
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
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 )
}
/****************************************************************************************/
/* FUNCTION NAME: AX12_read */
/* DESCRIPTION : Lire des données dans un registre de l'AX12 */
/****************************************************************************************/
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 *****************************************/
// Build the TxPacket first in RAM, then we'll send in one go
TxBuf[0] = 0xff;
TxBuf[1] = 0xff;
// ID
TxBuf[2] = ID;
sum += TxBuf[2];
// Packet Length
TxBuf[3] = PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
sum += TxBuf[3]; // Accululate the packet sum
// Instruction - Read
TxBuf[4] = 0x2;
sum += TxBuf[4];
// Start Address
TxBuf[5] = start;
sum += TxBuf[5];
// Bytes to read
TxBuf[6] = bytes;
sum += TxBuf[6];
// Checksum
TxBuf[7] = 0xFF - sum;
/********************************************TRAME CONSTRUITE DANS TxBuf***************************************/
/* Transmission de la trame construite precedemment dans le tableau TxBuf
*/
while ((timeout_transmit<1000) && (i < (7+bytes))) {
if (AX12_Serial.writeable()) {
AX12_Serial.putc(TxBuf[i]);
i++;
timeout_transmit = 0;
} else timeout_transmit++;
}
if (timeout_transmit == 1000 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
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_Serial.readable()) { // reception du premier Header ( 0xFF )
Status[plen] = AX12_Serial.getc();
timeout = 0;
if (Status[plen] == 0xFF ) {
etat = Header2;
plen++;
} else etat = Header1;
} else timeout++;
break;
case Header2:
if (AX12_Serial.readable()) { // reception du second Header ( 0xFF )
Status[plen] = AX12_Serial.getc();
timeout = 0;
if (Status[plen] == 0xFF ) {
etat = ident;
plen++;
} else if (Status[plen] == ID ) { // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!!
Status[plen] = 0;
plen++;
Status[plen] = ID;
etat = length;
plen++;
} else {
etat = Header1;
plen = 0;
}
} else timeout++;
break;
case ident:
if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur
Status[plen] = AX12_Serial.getc();
timeout = 0;
if (Status[plen] == ID ) {
etat = length;
plen++;
} else {
etat = Header1;
plen = 0;
}
} else timeout++;
break;
case length:
if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
Status[plen] = AX12_Serial.getc();
timeout = 0;
if (Status[plen] == (bytes+2) ) {
etat = erreur;
plen++;
} else {
etat = Header1;
plen = 0;
}
} else timeout++;
break;
case erreur:
if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
Status[plen] = AX12_Serial.getc();
timeout = 0;
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_Serial.readable()) {
Status[plen] = AX12_Serial.getc();
timeout = 0;
plen++;
enable++;
} else timeout++;
}
etat = checksum;
break;
case checksum:
if (AX12_Serial.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_Serial.getc();
timeout = 0;
flag_out = 1;
etat = Header1;
} 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
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 )
}