Diff: CAN_asser.cpp

Revision:

7:753e901d441b

Parent:

5:34048faec367

Child:

8:94ecfe411d02

--- a/CAN_asser.cpp	Thu May 09 12:36:58 2019 +0000
+++ b/CAN_asser.cpp	Mon May 20 14:58:09 2019 +0000
@@ -1,20 +1,19 @@
-#include "robot_general.h"
+#include "mbed.h"
+#include "CAN_asser.h"
 
-double KppD = 0.5;     //2     
-double KipD = 0.001*TE/0.02;     //0.1   //0.001
-double KdpD = 2.5/TE*0.02;     //3     //2.5
-
-double KppG = 0.5;     //2    
-double KipG = 0.001*TE/0.02;     //0.1   //0.001
-double KdpG = 2.5/TE*0.02;     //3     //2.5 
-
+CANMessage msg_can[256];
 CAN can(PB_8, PB_9,1000000); 
 unsigned char pt_lecture_can = 0, pt_ecriture_can = 0;
 int nb_msg_can = 0;
-CANMessage msg_can[256];
 short pos[3];
-int flagFinDpl = 0; 
- 
+int flagFinDpl = 0;
+Serial dbug(USBTX, USBRX, 115200);
+
+void can_init(void)
+{
+    can.attach(&isr_can, CAN::RxIrq);
+}
+
 void isr_can()
 {
     can.read(msg_can[pt_ecriture_can]);
@@ -22,68 +21,38 @@
     pt_ecriture_can++;
     if(nb_msg_can > 6)
     {
-        dbug.printf("!!!!!! CAN BUFFER OVERFLOW !!!!!!!!\n");
+        //dbug.printf("!!!!!! CAN BUFFER OVERFLOW !!!!!!!!\n");
          pt_lecture_can++;
          nb_msg_can--;
     }
-} 
- 
+}
+
 void trait_can()
 {
     while(nb_msg_can>0)
     {
-        //dbug.printf("tram CAN recvived %x\n", msg_can[pt_lecture_can].id);
-        if(msg_can[pt_lecture_can].id == ODOMETRIE_POSITION) get_pos(msg_can[pt_lecture_can], pos);
+        if(msg_can[pt_lecture_can].id == ODOMETRIE_POSITION)
+            get_pos(msg_can[pt_lecture_can], pos);
+        
         else if(msg_can[pt_lecture_can].id == 0x111)
         {
-            flagFinDpl = 1;dbug.printf("CAN fin dpl\n");
+            flagFinDpl = 1;
+            dbug.printf("CAN fin dpl\n");
         }
+            
         else if(msg_can[pt_lecture_can].id == 0x101)
-        {
             dbug.printf("CAN revived\n");
-        }
+            
         nb_msg_can--;
         pt_lecture_can++;
-        //remonter_tab_can(msg_can, nb_msg);
-        //if(msg_can[0].id != 0x26 && msg_can[0].id != 0x762) dbug.printf("CAN_id : %x\n", msg_can[0].id);
-        //dbug.printf("CAN_id : %x\n", msg_can[0].id);
     }
-} 
- 
+}
+
 void get_pos(CANMessage msg_pos, short* position)
 {
     memcpy((void *)((void *)&position[0]), &(msg_pos.data[0]), 2);
-    //memcpy((void *)((void *)&x[0] + 1), &(msg_can[0].data[1]), 1);
     memcpy((void *)((void *)&position[1]), &(msg_pos.data[2]), 2);
-    //memcpy((void *)((void *)&x[1] + 1), &(msg_can[0].data[3]), 1);
     memcpy((void *)((void *)&position[2]), &(msg_pos.data[4]), 2);
-    //memcpy((void *)((void *)&x[2] + 1), &(msg_can[0].data[5]), 1);
-    
-    //pc.printf("odo: x = %d, y = %d, te = %d.\n\r", position[0], position[1], position[2]);
-}
- 
-void can_init(void)
-{
-    
-    can.attach(&isr_can, CAN::RxIrq);
-}
-
-int compare_tab(short* t1, short* t2, int size)
-{
-    int flag = 0;
-    for(int i=0; i<size; i++)
-    {
-        if(t1[i] != t2[i]) flag++;
-    }
-    return flag;
-}
-
-void remonter_tab_can(CANMessage* tab, int taille)
-{
-    for(int i=0;i<(taille);i++)
-    {
-        tab[i] = tab[i+1];
-    }
 }
 
 void SendCoefK (unsigned short id, double K)
@@ -105,7 +74,6 @@
         memcpy(&(msgTx.data[i]), (void *)((void *)&K + i), 1);
     }
     
-    
     can.write(msgTx);
 }
 /*********************************************************************************************************/