Diff: AX12-V2/AX12-V2.cpp

Revision:

8:0edc7dfb7f7e

Parent:

5:dcd817534b57

Child:

12:14729d584500

--- a/AX12-V2/AX12-V2.cpp	Tue Apr 26 15:30:54 2016 +0000
+++ b/AX12-V2/AX12-V2.cpp	Wed Apr 27 13:05:03 2016 +0000
@@ -16,15 +16,14 @@
 void AX12_register(unsigned char id, unsigned char serial, 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;
-    AX12_data[lastAX12Use].serial = serial;
-    
+    AX12_data[localID].speed = speed;
+    AX12_data[localID].serial = serial;
+    //printf("registering AX12 id: %d local: %d\n",id,localID);
     AX12_Serial1.baud(1000000);//On indique la vitesse de transmission des AX12
     AX12_Serial2.baud(1000000);//On indique la vitesse de transmission des AX12
     
@@ -86,7 +85,7 @@
 int AX12_getLocalID(unsigned char id)
 {
     int i=0;
-    for(i=0;i<MAX_AX12;i++)
+    for(i=0;i<lastAX12Use;i++)
     {
         if(AX12_data[i].id == id) return i;
     } 
@@ -121,7 +120,7 @@
     int i=0;
     CANMessage msgTx=CANMessage();
 
-    for(i=0;i<MAX_AX12;i++)
+    for(i=0;i<lastAX12Use;i++)
     {
         if(AX12_data[i].isUsingCAN == 0 && AX12_data[i].needCheckMoving == 1)//Il faut vérifier si l'AX12 a terminé de bouger
         {
@@ -153,12 +152,15 @@
     char dataToSend[100];
 
     
-    for(i=0;i<MAX_AX12;i++)
+    for(i=0;i<lastAX12Use;i++)
     {
+        //printf("checking AX12 id: %d",AX12_data[i].id);
         if(AX12_data[i].needToUpdate == 1) //Il faut mettre à jour la position de l'AX12
         {
+            //printf(" => update");
             if(AX12_data[i].isUsingCAN == 0)//Il faut envoyer la trame en local
             {
+                //printf(" local");
                 if(dataToSendLength == 0)
                     dataToSend[dataToSendLength++] = 4;//length data
                 dataToSend[dataToSendLength++] = AX12_data[i].id;//ID servo1
@@ -170,13 +172,15 @@
             }
             AX12_data[i].needToUpdate = 0;//Remise à 0 de l'indicatif de mise à jour
         }
+        //printf("\n");
     } 
     if(fromCan == 0)
         SendRawId(SERVO_AX12_PROCESS);//On indique par CAN qu'il faut bouger les AX12
-
+    //printf("need to send %d data\n",dataToSendLength);
     if(dataToSendLength > 0)//Il y a des données à envoyer en local
     {
         AX12_syncWrite(AX12_Serial1, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
+        //wait_ms(10);
         AX12_syncWrite(AX12_Serial2, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
     }
     
@@ -217,7 +221,7 @@
 
     int timeout_transmit = 0;
     int i = 0;
-    printf("Start sending moving trame\n");
+    //printf("Start sending moving trame\n");
     // Build the TxPacket first in RAM, then we'll send in one go
 
     TxBuf[0] = 0xff;
@@ -245,7 +249,7 @@
     for (char i=0; i<bytes ; i++) {
         TxBuf[6+i] = data[i];
         sum += TxBuf[6+i];
-        printf("  Data : 0x%x\n",TxBuf[6+i]);
+        //printf("  Data : 0x%x\n",TxBuf[6+i]);
     }
 
     // checksum