Alexandre Pirotte / Mbed 2 deprecated NUCLEO_LIDAR

LUIS

Dependencies:   mbed

fct.h@0:96bdb0d08de1, 2018-10-05 (annotated)

Committer:
pirottealex
Date:
Fri Oct 05 14:54:54 2018 +0000
Revision:
0:96bdb0d08de1
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pirottealex 0:96bdb0d08de1 1
pirottealex 0:96bdb0d08de1 2 #define PERIOD 0.00005
pirottealex 0:96bdb0d08de1 3 #define GET_INFO 0x50
pirottealex 0:96bdb0d08de1 4 #define GET_HEALTH 0x52
pirottealex 0:96bdb0d08de1 5 #define RESET 0x40
pirottealex 0:96bdb0d08de1 6 #define SYNC 0xA5
pirottealex 0:96bdb0d08de1 7 #define SCAN 0x20
pirottealex 0:96bdb0d08de1 8 #define STOP 0x25
pirottealex 0:96bdb0d08de1 9 #define BOFF 7
pirottealex 0:96bdb0d08de1 10
pirottealex 0:96bdb0d08de1 11 Serial Lidar (PC_10,PC_11);
pirottealex 0:96bdb0d08de1 12 Serial pc(USBTX,USBRX);
pirottealex 0:96bdb0d08de1 13
pirottealex 0:96bdb0d08de1 14 PwmOut LIDAR(A3);
pirottealex 0:96bdb0d08de1 15
pirottealex 0:96bdb0d08de1 16 char msg[50] = {0};
pirottealex 0:96bdb0d08de1 17 int i=0;
pirottealex 0:96bdb0d08de1 18 int flag=0;
pirottealex 0:96bdb0d08de1 19
pirottealex 0:96bdb0d08de1 20
pirottealex 0:96bdb0d08de1 21
pirottealex 0:96bdb0d08de1 22 void get_health(void)
pirottealex 0:96bdb0d08de1 23 {
pirottealex 0:96bdb0d08de1 24 Lidar.putc(SYNC);
pirottealex 0:96bdb0d08de1 25 Lidar.putc(GET_HEALTH);
pirottealex 0:96bdb0d08de1 26 wait(0.1);
pirottealex 0:96bdb0d08de1 27 int statut=msg[BOFF];
pirottealex 0:96bdb0d08de1 28 if( statut==0)
pirottealex 0:96bdb0d08de1 29 {
pirottealex 0:96bdb0d08de1 30 pc.printf("Statut = 0 : GOOD");
pirottealex 0:96bdb0d08de1 31 }
pirottealex 0:96bdb0d08de1 32 else if( statut==1)
pirottealex 0:96bdb0d08de1 33 {
pirottealex 0:96bdb0d08de1 34 pc.printf("Statut = 1 : WARNING");
pirottealex 0:96bdb0d08de1 35 }
pirottealex 0:96bdb0d08de1 36 else if( statut==2)
pirottealex 0:96bdb0d08de1 37 {
pirottealex 0:96bdb0d08de1 38 pc.printf("Statut = 2 : ERROR");
pirottealex 0:96bdb0d08de1 39 }
pirottealex 0:96bdb0d08de1 40 pc.printf("\n\r");
pirottealex 0:96bdb0d08de1 41 for(int j=0; j<=50;j++)
pirottealex 0:96bdb0d08de1 42 {
pirottealex 0:96bdb0d08de1 43 msg[i]=0;
pirottealex 0:96bdb0d08de1 44 }
pirottealex 0:96bdb0d08de1 45 i=0;
pirottealex 0:96bdb0d08de1 46 }
pirottealex 0:96bdb0d08de1 47
pirottealex 0:96bdb0d08de1 48
pirottealex 0:96bdb0d08de1 49 void get_info(void)
pirottealex 0:96bdb0d08de1 50 {
pirottealex 0:96bdb0d08de1 51 Lidar.putc(SYNC);
pirottealex 0:96bdb0d08de1 52 Lidar.putc(GET_INFO);
pirottealex 0:96bdb0d08de1 53 wait(0.1);
pirottealex 0:96bdb0d08de1 54 pc.printf("LIDAR MODEL : %d, LIDAR FIRMWARE : %d.%d, NUMERO DE SERIE : ",msg[BOFF],msg[BOFF+2],msg[BOFF+1]);
pirottealex 0:96bdb0d08de1 55 for(int j=BOFF+3; j<=16+BOFF+3;j++)
pirottealex 0:96bdb0d08de1 56 {
pirottealex 0:96bdb0d08de1 57 pc.printf("%x",msg[j]);
pirottealex 0:96bdb0d08de1 58 }
pirottealex 0:96bdb0d08de1 59 pc.printf("\n\r");
pirottealex 0:96bdb0d08de1 60 for(int j=0; j<=50;j++)
pirottealex 0:96bdb0d08de1 61 {
pirottealex 0:96bdb0d08de1 62 msg[i]=0;
pirottealex 0:96bdb0d08de1 63 }
pirottealex 0:96bdb0d08de1 64 i=0;
pirottealex 0:96bdb0d08de1 65 }
pirottealex 0:96bdb0d08de1 66
pirottealex 0:96bdb0d08de1 67 void reception (void)
pirottealex 0:96bdb0d08de1 68 {
pirottealex 0:96bdb0d08de1 69 msg[i] =Lidar.getc();
pirottealex 0:96bdb0d08de1 70 /*if((flag==0)&&(i==6))
pirottealex 0:96bdb0d08de1 71 {
pirottealex 0:96bdb0d08de1 72 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]);
pirottealex 0:96bdb0d08de1 73 flag=1;
pirottealex 0:96bdb0d08de1 74 i=-1;
pirottealex 0:96bdb0d08de1 75 for(int j=0; j<=50;j++)
pirottealex 0:96bdb0d08de1 76 {
pirottealex 0:96bdb0d08de1 77 msg[i]=0;
pirottealex 0:96bdb0d08de1 78 }
pirottealex 0:96bdb0d08de1 79 }
pirottealex 0:96bdb0d08de1 80 //pc.printf("%d",c);
pirottealex 0:96bdb0d08de1 81
pirottealex 0:96bdb0d08de1 82 if((i==4) && (flag==1))
pirottealex 0:96bdb0d08de1 83 {
pirottealex 0:96bdb0d08de1 84 if(msg[1] & 0xb1)
pirottealex 0:96bdb0d08de1 85 int qualite= msg[0]>>2;
pirottealex 0:96bdb0d08de1 86 if(qualite>=2)
pirottealex 0:96bdb0d08de1 87 {
pirottealex 0:96bdb0d08de1 88 int angle=((msg[1] >> 1) + (msg[2] << 7)) / 64.0;
pirottealex 0:96bdb0d08de1 89 int dist=((msg[3]) + ((msg[4]) << 8)) / 4.0;
pirottealex 0:96bdb0d08de1 90 pc.printf("%d ,%d ,%d\n\r",qualite,angle,dist);
pirottealex 0:96bdb0d08de1 91 }
pirottealex 0:96bdb0d08de1 92 //pc.printf("%d",i);
pirottealex 0:96bdb0d08de1 93 i=-1;
pirottealex 0:96bdb0d08de1 94 for(int j=0; j<=50;j++)
pirottealex 0:96bdb0d08de1 95 {
pirottealex 0:96bdb0d08de1 96 msg[i]=0;
pirottealex 0:96bdb0d08de1 97 }
pirottealex 0:96bdb0d08de1 98 }*/
pirottealex 0:96bdb0d08de1 99
pirottealex 0:96bdb0d08de1 100 i++;
pirottealex 0:96bdb0d08de1 101
pirottealex 0:96bdb0d08de1 102 }