CRAC Team
/
carte_straegie_2019
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.
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: AX12-V2/AX12-V2.cpp
- Revision:
- 1:116040d14164
- Parent:
- 0:ad97421fb1fb
- Child:
- 5:dcd817534b57
--- a/AX12-V2/AX12-V2.cpp Wed Apr 13 22:04:54 2016 +0000
+++ b/AX12-V2/AX12-V2.cpp Fri Apr 15 10:49:40 2016 +0000
@@ -6,6 +6,8 @@
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 */
@@ -20,14 +22,14 @@
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);
-
+ */
}
@@ -43,12 +45,12 @@
AX12_data[localID].needToUpdate = 1;
AX12_data[localID].goal = goal;
if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
-
- AX12_data[localID].speed = speed;
+ 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=3;
+ msgTx.len=5;
msgTx.format=CANStandard;
msgTx.type=CANData;
// id de l'AX12 sur 1 octet
@@ -57,8 +59,8 @@
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)speed;
- msgTx.data[4]=(unsigned char)(speed>>8);
+ msgTx.data[3]=(unsigned char)AX12_data[localID].speed;
+ msgTx.data[4]=(unsigned char)(AX12_data[localID].speed>>8);
can1.write(msgTx);
}
@@ -102,12 +104,44 @@
}
/****************************************************************************************/
+/* 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)
{
- //TODO
+ 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);
+ }
+ }
+ }
}
/****************************************************************************************/
@@ -119,7 +153,7 @@
{
int i=0;
int dataToSendLength = 0;
- char dataToSend[50];
+ char dataToSend[100];
for(i=0;i<MAX_AX12;i++)
@@ -129,10 +163,13 @@
if(AX12_data[i].isUsingCAN == 0)//Il faut envoyer la trame en local
{
if(dataToSendLength == 0)
- dataToSend[dataToSendLength++] = 2;//length data
+ 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++] = ((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
}
@@ -147,6 +184,17 @@
}
/****************************************************************************************/
+/* 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 */
/****************************************************************************************/
@@ -397,3 +445,219 @@
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 )
+}
+
+