fct.h

00001 
00002 #define PERIOD 0.00005
00003 #define GET_INFO 0x50
00004 #define GET_HEALTH 0x52
00005 #define RESET 0x40 
00006 #define SYNC 0xA5
00007 #define SCAN 0x20
00008 #define STOP 0x25
00009 #define BOFF 7
00010 
00011 Serial Lidar (PC_10,PC_11); 
00012 Serial pc(USBTX,USBRX);
00013 
00014 PwmOut LIDAR(A3);
00015 
00016 char msg[50] = {0};
00017 int i=0;
00018 int flag=0;
00019 
00020 
00021 
00022 void get_health(void)
00023 {
00024     Lidar.putc(SYNC);
00025     Lidar.putc(GET_HEALTH);
00026     wait(0.1);
00027     int statut=msg[BOFF];
00028     if( statut==0)
00029     {
00030         pc.printf("Statut = 0 : GOOD");
00031     }
00032     else if( statut==1)
00033     {
00034         pc.printf("Statut = 1 : WARNING");
00035     }    
00036     else if( statut==2)
00037     {
00038         pc.printf("Statut = 2 : ERROR");
00039     } 
00040     pc.printf("\n\r");
00041     for(int j=0; j<=50;j++)
00042     {
00043         msg[i]=0;
00044     }
00045     i=0; 
00046 }   
00047 
00048 
00049 void get_info(void)
00050 {
00051     Lidar.putc(SYNC);
00052     Lidar.putc(GET_INFO);
00053     wait(0.1);
00054     pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",msg[BOFF],msg[BOFF+2],msg[BOFF+1]);
00055     for(int j=BOFF+3; j<=16+BOFF+3;j++)
00056     {
00057         pc.printf("%x",msg[j]);
00058     }
00059     pc.printf("\n\r");
00060     for(int j=0; j<=50;j++)
00061     {
00062         msg[i]=0;
00063     }
00064     i=0;    
00065 }
00066 
00067 void reception (void)
00068 {
00069     msg[i] =Lidar.getc();
00070     /*if((flag==0)&&(i==6))
00071     {
00072         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]);
00073         flag=1;
00074         i=-1;
00075         for(int j=0; j<=50;j++)
00076         {
00077             msg[i]=0;
00078         }  
00079     }
00080     //pc.printf("%d",c);
00081     
00082     if((i==4) && (flag==1))
00083     {
00084         if(msg[1] & 0xb1)
00085         int qualite= msg[0]>>2;
00086         if(qualite>=2)
00087         {
00088             int angle=((msg[1] >> 1) + (msg[2] << 7)) / 64.0;
00089             int dist=((msg[3]) + ((msg[4]) << 8)) / 4.0;
00090             pc.printf("%d ,%d ,%d\n\r",qualite,angle,dist); 
00091         }
00092         //pc.printf("%d",i);
00093         i=-1;
00094         for(int j=0; j<=50;j++)
00095         {
00096             msg[i]=0;
00097         }        
00098     }*/
00099     
00100     i++;
00101 
00102 }