Diff: fct.h

Revision:

0:96bdb0d08de1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fct.h	Fri Oct 05 14:54:54 2018 +0000
@@ -0,0 +1,102 @@
+
+#define PERIOD 0.00005
+#define GET_INFO 0x50
+#define GET_HEALTH 0x52
+#define RESET 0x40 
+#define SYNC 0xA5
+#define SCAN 0x20
+#define STOP 0x25
+#define BOFF 7
+
+Serial Lidar (PC_10,PC_11); 
+Serial pc(USBTX,USBRX);
+
+PwmOut LIDAR(A3);
+
+char msg[50] = {0};
+int i=0;
+int flag=0;
+
+
+
+void get_health(void)
+{
+    Lidar.putc(SYNC);
+    Lidar.putc(GET_HEALTH);
+    wait(0.1);
+    int statut=msg[BOFF];
+    if( statut==0)
+    {
+        pc.printf("Statut = 0 : GOOD");
+    }
+    else if( statut==1)
+    {
+        pc.printf("Statut = 1 : WARNING");
+    }    
+    else if( statut==2)
+    {
+        pc.printf("Statut = 2 : ERROR");
+    } 
+    pc.printf("\n\r");
+    for(int j=0; j<=50;j++)
+    {
+        msg[i]=0;
+    }
+    i=0; 
+}   
+
+
+void get_info(void)
+{
+    Lidar.putc(SYNC);
+    Lidar.putc(GET_INFO);
+    wait(0.1);
+    pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",msg[BOFF],msg[BOFF+2],msg[BOFF+1]);
+    for(int j=BOFF+3; j<=16+BOFF+3;j++)
+    {
+        pc.printf("%x",msg[j]);
+    }
+    pc.printf("\n\r");
+    for(int j=0; j<=50;j++)
+    {
+        msg[i]=0;
+    }
+    i=0;    
+}
+
+void reception (void)
+{
+    msg[i] =Lidar.getc();
+    /*if((flag==0)&&(i==6))
+    {
+        pc.printf("%d ,%d ,%d,%d ,%d, %d ,%d\n\r",msg[0],msg[1],msg[2],msg[3],msg[4], msg[5], msg[6]);
+        flag=1;
+        i=-1;
+        for(int j=0; j<=50;j++)
+        {
+            msg[i]=0;
+        }  
+    }
+    //pc.printf("%d",c);
+    
+    if((i==4) && (flag==1))
+    {
+        if(msg[1] & 0xb1)
+        int qualite= msg[0]>>2;
+        if(qualite>=2)
+        {
+            int angle=((msg[1] >> 1) + (msg[2] << 7)) / 64.0;
+            int dist=((msg[3]) + ((msg[4]) << 8)) / 4.0;
+            pc.printf("%d ,%d ,%d\n\r",qualite,angle,dist); 
+        }
+        //pc.printf("%d",i);
+        i=-1;
+        for(int j=0; j<=50;j++)
+        {
+            msg[i]=0;
+        }        
+    }*/
+    
+    i++;
+
+}
\ No newline at end of file