AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

captUS.cpp@16:4c0b1647e8ae, 2020-10-13 (annotated)

Committer:
g0dd4
Date:
Tue Oct 13 14:50:31 2020 +0000
Revision:
16:4c0b1647e8ae
Parent:
15:43f5bda97488 
Conversion du temps en distance ; Detection d'un obstacle; Changement de base; Test_fonctionnelle ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
g0dd4 16:4c0b1647e8ae 1 #include "captUS.h"
g0dd4 16:4c0b1647e8ae 2 #include "pins.h"
g0dd4 16:4c0b1647e8ae 3 #include "math.h"
Nanaud 1:2fe8c402ee79 4 //Nom du fichier : captUS.cpp
Nanaud 0:dc036b67c87c 5
Nanaud 1:2fe8c402ee79 6 // Variables globales & timers
Nanaud 1:2fe8c402ee79 7 float us_out[6];
g0dd4 16:4c0b1647e8ae 8 float* distance;
Nanaud 1:2fe8c402ee79 9 Timer tps;
Nanaud 1:2fe8c402ee79 10 Ticker ticker_US;
Nanaud 0:dc036b67c87c 11
g0dd4 16:4c0b1647e8ae 12
Nanaud 1:2fe8c402ee79 13 void captUS_init(){
g0dd4 16:4c0b1647e8ae 14 ::distance = new float(6); //équivalent au malloc()
g0dd4 15:43f5bda97488 15 tps.reset();
g0dd4 15:43f5bda97488 16 tps.start();
g0dd4 15:43f5bda97488 17 }
Nanaud 1:2fe8c402ee79 18
g0dd4 16:4c0b1647e8ae 19
Nanaud 1:2fe8c402ee79 20 void captUS_trig(){
Nanaud 0:dc036b67c87c 21 tps.reset();
Nanaud 0:dc036b67c87c 22 trigger=1;
g0dd4 15:43f5bda97488 23 wait_us(20);
Nanaud 0:dc036b67c87c 24 trigger=0;
Nanaud 0:dc036b67c87c 25 }
Nanaud 0:dc036b67c87c 26
Nanaud 0:dc036b67c87c 27 void echoRise1(){us_out[0]=tps.read_us();}
Nanaud 0:dc036b67c87c 28 void echoFall1(){us_out[0]=(tps.read_us()-us_out[0])/2;}
Nanaud 0:dc036b67c87c 29
Nanaud 0:dc036b67c87c 30 void echoRise2(){us_out[1]=tps.read_us();}
Nanaud 0:dc036b67c87c 31 void echoFall2(){us_out[1]=(tps.read_us()-us_out[1])/2;}
Nanaud 0:dc036b67c87c 32
Nanaud 0:dc036b67c87c 33 void echoRise3(){us_out[2]=tps.read_us();}
Nanaud 0:dc036b67c87c 34 void echoFall3(){us_out[2]=(tps.read_us()-us_out[2])/2;}
Nanaud 0:dc036b67c87c 35
Nanaud 0:dc036b67c87c 36 void echoRise4(){us_out[3]=tps.read_us();}
Nanaud 0:dc036b67c87c 37 void echoFall4(){us_out[3]=(tps.read_us()-us_out[3])/2;}
Nanaud 0:dc036b67c87c 38
Nanaud 0:dc036b67c87c 39 void echoRise5(){us_out[4]=tps.read_us();}
Nanaud 0:dc036b67c87c 40 void echoFall5(){us_out[4]=(tps.read_us()-us_out[4])/2;}
Nanaud 0:dc036b67c87c 41
Nanaud 0:dc036b67c87c 42 void echoRise6(){us_out[5]=tps.read_us();}
Nanaud 0:dc036b67c87c 43 void echoFall6(){us_out[5]=(tps.read_us()-us_out[5])/2;}
g0dd4 15:43f5bda97488 44
g0dd4 15:43f5bda97488 45
g0dd4 15:43f5bda97488 46 float* convertToDistance(){
g0dd4 15:43f5bda97488 47 /**************************************
g0dd4 15:43f5bda97488 48 * Nous convertisons grâce au valeur *
g0dd4 15:43f5bda97488 49 * qui sont retournées par echoRiseX *
g0dd4 15:43f5bda97488 50 * et echoFallx *
g0dd4 15:43f5bda97488 51 **************************************/
g0dd4 16:4c0b1647e8ae 52
g0dd4 16:4c0b1647e8ae 53 for(char i = 0; i<6;i++){
g0dd4 16:4c0b1647e8ae 54 ::distance[i] = (us_out[i]*340)/1000;//conversion en distance(mm)
g0dd4 16:4c0b1647e8ae 55 }
g0dd4 16:4c0b1647e8ae 56
g0dd4 16:4c0b1647e8ae 57 /****************************************
g0dd4 16:4c0b1647e8ae 58 * nous retournons l'adresse du tableau *
g0dd4 16:4c0b1647e8ae 59 ****************************************/
g0dd4 16:4c0b1647e8ae 60 return ::distance;
g0dd4 16:4c0b1647e8ae 61 }
g0dd4 16:4c0b1647e8ae 62
g0dd4 16:4c0b1647e8ae 63 bool obstacleSpoted(float dist,double x_robot,double y_robot ,double phi, char I_theta){
g0dd4 16:4c0b1647e8ae 64 /**************************
g0dd4 16:4c0b1647e8ae 65 * convertion de ° en rad *
g0dd4 16:4c0b1647e8ae 66 **************************/
g0dd4 16:4c0b1647e8ae 67 double phiG = ((phi+(I_theta*THETA))*_PI_)/180;
g0dd4 16:4c0b1647e8ae 68
g0dd4 16:4c0b1647e8ae 69 /***********************************
g0dd4 16:4c0b1647e8ae 70 * convertion de la norme grâce à *
g0dd4 16:4c0b1647e8ae 71 * la norme et à l'angle *
g0dd4 16:4c0b1647e8ae 72 ***********************************/
g0dd4 16:4c0b1647e8ae 73 double x_dect = dist * cos(phiG) + x_robot;
g0dd4 16:4c0b1647e8ae 74 double y_dect = dist * sin(phiG) + y_robot;
g0dd4 16:4c0b1647e8ae 75
g0dd4 15:43f5bda97488 76
g0dd4 16:4c0b1647e8ae 77 changementBase(&x_dect,&y_dect);
g0dd4 16:4c0b1647e8ae 78 /*********************************************
g0dd4 16:4c0b1647e8ae 79 * vérification de la position de l'obstacle *
g0dd4 16:4c0b1647e8ae 80 *********************************************/
g0dd4 16:4c0b1647e8ae 81 if(x_dect > LARGEUR_TAB || x_dect < 0||y_dect >LONGUEUR_TAB||y_dect < 0){
g0dd4 16:4c0b1647e8ae 82 return false;
g0dd4 16:4c0b1647e8ae 83 }
g0dd4 16:4c0b1647e8ae 84 else{
g0dd4 16:4c0b1647e8ae 85 return true;
g0dd4 16:4c0b1647e8ae 86 }
g0dd4 16:4c0b1647e8ae 87
g0dd4 15:43f5bda97488 88 }
g0dd4 16:4c0b1647e8ae 89
g0dd4 16:4c0b1647e8ae 90 void changementBase(double* x_detect, double* y_detect){
g0dd4 16:4c0b1647e8ae 91 /*****************************
g0dd4 16:4c0b1647e8ae 92 * l'origine de notres table *
g0dd4 16:4c0b1647e8ae 93 * se situe au coins *
g0dd4 16:4c0b1647e8ae 94 * supérieur *
g0dd4 16:4c0b1647e8ae 95 *****************************/
g0dd4 16:4c0b1647e8ae 96 *y_detect+=505;
g0dd4 16:4c0b1647e8ae 97 *x_detect+=5;
g0dd4 16:4c0b1647e8ae 98 }
g0dd4 16:4c0b1647e8ae 99
g0dd4 16:4c0b1647e8ae 100