Quentin Tual / Mbed 2 deprecated Nucleo_LIDAR_PRJ_2018-19

Dependencies:   PwmIn mbed

Committer:
Nilox
Date:
Fri May 17 13:33:37 2019 +0000
Revision:
1:19d43c4e741f
Parent:
0:3b2f23672f26
;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nilox 0:3b2f23672f26 1 #include "mbed.h"
Nilox 0:3b2f23672f26 2 #include <PwmIn.h>
Nilox 0:3b2f23672f26 3 #include "math.h"
Nilox 0:3b2f23672f26 4
Nilox 0:3b2f23672f26 5 // Initialize a pins to perform analog and digital output fucntions
Nilox 0:3b2f23672f26 6 //Serial pc(USBTX,USBRX);
Nilox 0:3b2f23672f26 7 Serial BT(PA_11,PA_12 ,9600);
Nilox 0:3b2f23672f26 8
Nilox 0:3b2f23672f26 9
Nilox 0:3b2f23672f26 10 DigitalOut trig(PA_10); // D2
Nilox 0:3b2f23672f26 11 PwmOut servo(PB_5); // D4
Nilox 0:3b2f23672f26 12 PwmIn LidarI(PB_3); // D3
Nilox 0:3b2f23672f26 13
Nilox 0:3b2f23672f26 14 InterruptIn bouton(USER_BUTTON);
Nilox 0:3b2f23672f26 15
Nilox 0:3b2f23672f26 16 /////////////////////////////////////////////////
Nilox 0:3b2f23672f26 17 void arret(){
Nilox 0:3b2f23672f26 18 sleep();
Nilox 0:3b2f23672f26 19 }
Nilox 0:3b2f23672f26 20 //////////////////////////////////////////////
Nilox 0:3b2f23672f26 21
Nilox 0:3b2f23672f26 22 ////////////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 23 void Mesure(float *pmes){
Nilox 1:19d43c4e741f 24 float moy=0.0f,tmp = 0.0f;
Nilox 1:19d43c4e741f 25 int i=0;
Nilox 1:19d43c4e741f 26 for(i=0;i<=9;i++){
Nilox 1:19d43c4e741f 27 tmp=LidarI.pulsewidth() / 10; //Moyenne de dix mesures stocké dans "moy" pour le calcul
Nilox 1:19d43c4e741f 28 moy+=tmp;
Nilox 1:19d43c4e741f 29 }
Nilox 0:3b2f23672f26 30 wait_ms(40); //Laisser le moteur arriver à sa position
Nilox 0:3b2f23672f26 31 trig = 1;
Nilox 0:3b2f23672f26 32 trig=0; // Déclenchement
Nilox 0:3b2f23672f26 33 wait_ms(10); //Attendre que le signal du LIDAR soit clair pour le microcontroleur et pas parasité par le front descendant du déclenchement
Nilox 1:19d43c4e741f 34 *pmes = moy/10; //Lecture du signal et enregistrement dans la variable "mes" (en µs)
Nilox 0:3b2f23672f26 35 trig = 1; //Stop de la prise de mesure
Nilox 0:3b2f23672f26 36 // pc.printf("Mesure prise : %f \n",*pmes); //Affichage de la mesure prise (mesure de la distance en cm)
Nilox 0:3b2f23672f26 37
Nilox 0:3b2f23672f26 38 }
Nilox 0:3b2f23672f26 39 //////////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 40
Nilox 0:3b2f23672f26 41 // Cette fonction permet de convertir des degrés d'angles en radians afin d'utiliser une valeur avec la fonction cosinus
Nilox 0:3b2f23672f26 42 ///////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 43 float conversion_deg_rad (int in){
Nilox 0:3b2f23672f26 44 return in*0.017453292519943f;
Nilox 0:3b2f23672f26 45 }
Nilox 0:3b2f23672f26 46 //////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 47
Nilox 0:3b2f23672f26 48 ////////////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 49 int aireSegment (float t[],int angle,int i){
Nilox 0:3b2f23672f26 50 float a=t[i],b=t[i-1],c=0.0f;
Nilox 0:3b2f23672f26 51 float tmp=0.0f,ret=0.0f,s=0.0f;
Nilox 0:3b2f23672f26 52
Nilox 0:3b2f23672f26 53
Nilox 0:3b2f23672f26 54 tmp =(a*a)+(b*b)-(2.0f*a*b*cos(conversion_deg_rad(angle)));
Nilox 0:3b2f23672f26 55 c = sqrt(tmp);
Nilox 0:3b2f23672f26 56 s = (a+b+c)/2; // Shift vers la droite de 1 pour la division par 2
Nilox 0:3b2f23672f26 57 // Autre formule intéressante : s=1/4 sqrt((a+(b+c))*(c-(a-b))*(c+(a-b))*(a+(c-b)))
Nilox 0:3b2f23672f26 58 // Cette formule est plus stable lorsque l'un des cotes est très petit
Nilox 0:3b2f23672f26 59 // Utiliser de telle manière que a>b>c
Nilox 0:3b2f23672f26 60 ret = sqrt(s*(s-a)*(s-b)*(s-c));
Nilox 0:3b2f23672f26 61
Nilox 0:3b2f23672f26 62 //pc.printf("aire segment : %f \n", ret); //TEST!!!
Nilox 0:3b2f23672f26 63
Nilox 0:3b2f23672f26 64
Nilox 0:3b2f23672f26 65 return ret;
Nilox 0:3b2f23672f26 66 }
Nilox 0:3b2f23672f26 67 ////////////////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 68
Nilox 0:3b2f23672f26 69 /////////////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 70 void programme(){
Nilox 0:3b2f23672f26 71
Nilox 0:3b2f23672f26 72 // Déclarations des variables
Nilox 0:3b2f23672f26 73 float mes = 0; //Variable qui contient les mesures
Nilox 0:3b2f23672f26 74 float *pmes = &mes; //Pointeur qui permettra d'accéder à la variable mesure en dehors du sous programme principale "main"
Nilox 0:3b2f23672f26 75 float aireTotale = 0, angle=0.01f,A = 0.8f; // "aireTotale" permet de récupérer la valeur recherchée, on y ajoute l'aire de chaque segments à chaque calcul de ceux-ci
Nilox 0:3b2f23672f26 76 // "Angle" de chaque rotation du moteur entre chaque prises de mesure (en degrés d'angle !!!)
Nilox 0:3b2f23672f26 77 // "A" un flottant qui permet de controler le servomoteur (en millisecondes !!!)
Nilox 0:3b2f23672f26 78 // ( 1.0 fonctionnel mais ne va pas jusqu'a 0° pour le moteur
Nilox 0:3b2f23672f26 79 // 0.8 ne marche pas avec les autres fonctions mais permet de positionner le moteur à son maximum)
Nilox 0:3b2f23672f26 80 // "angle_parcouru" indique en degré d'angle l'angle parcouru par le moteur depuis le lancement du programme
Nilox 0:3b2f23672f26 81
Nilox 0:3b2f23672f26 82
Nilox 0:3b2f23672f26 83 int i = 0,nbMes = 1.4f/angle, angle_degree = 1, pourcent = 0; // "i" sert d'index dans le tableau dans lequel on stock les mesures
Nilox 0:3b2f23672f26 84 // "nbMes" correspond au nombre de mesure qui seront effectuées
Nilox 0:3b2f23672f26 85
Nilox 0:3b2f23672f26 86
Nilox 0:3b2f23672f26 87 float tabM[nbMes]; // "tabM" Un tableau dans lequel sera enregistré les mesures effectuées
Nilox 0:3b2f23672f26 88 bouton.rise(&arret); // Pour arreter le programme en appuyant sur le bouton bleu
Nilox 0:3b2f23672f26 89
Nilox 0:3b2f23672f26 90
Nilox 0:3b2f23672f26 91 //pc.printf("\nLancement du programme...\n");
Nilox 0:3b2f23672f26 92 BT.printf("*A\n\nLancement du programme...\n*");
Nilox 0:3b2f23672f26 93
Nilox 0:3b2f23672f26 94
Nilox 0:3b2f23672f26 95 trig = 1; // "trig" à 1 --> LIDAR attend que "trig" passe à 0 pour prendre des mesures en somme il est en "standby"
Nilox 0:3b2f23672f26 96 servo.pulsewidth_us(A*1000); // On fixe le servomoteur à son angle minimum
Nilox 0:3b2f23672f26 97 Mesure(pmes); // Lancement de la fonction qui prend une mesure et qui inscrit dans "mes" lavaleur de la mesure effectuée
Nilox 0:3b2f23672f26 98 tabM[i]=mes; // La mesure est enregistrée dans le tableau des mesures
Nilox 0:3b2f23672f26 99 i++; // L'index du tableau augment donc d'un
Nilox 0:3b2f23672f26 100 wait(1);
Nilox 0:3b2f23672f26 101
Nilox 0:3b2f23672f26 102
Nilox 0:3b2f23672f26 103 for(A = 0.8f; A <= 2.2f ; A += angle){
Nilox 0:3b2f23672f26 104 pourcent = (int) (i*100)/nbMes;
Nilox 0:3b2f23672f26 105 BT.printf("*T%d*",pourcent);
Nilox 0:3b2f23672f26 106 //if(pourcent==0){ // Mettre les couleurs dans les printf
Nilox 0:3b2f23672f26 107 // BT.printf("*L*");
Nilox 0:3b2f23672f26 108 //} else if (){
Nilox 0:3b2f23672f26 109 // BT.printf("*L*");
Nilox 0:3b2f23672f26 110 //} else if (){
Nilox 0:3b2f23672f26 111 // BT.printf("*L*");
Nilox 0:3b2f23672f26 112 //}
Nilox 0:3b2f23672f26 113 servo.pulsewidth_us(A*1000); // Positionnement du Moteur à sa position minimum
Nilox 0:3b2f23672f26 114 Mesure(pmes); // Effectue une mesure et la stocke dans la variable "mes"
Nilox 0:3b2f23672f26 115 tabM[i]=mes; // Stock la valeur contenu dans la varaible "mes" dans le tableau
Nilox 0:3b2f23672f26 116 aireTotale += aireSegment(tabM, angle_degree, i); //Calcul de l'aire de chaque segment et de l'aire totale
Nilox 0:3b2f23672f26 117 i++; // incrémentation de l'index du tableau des mesures
Nilox 0:3b2f23672f26 118 wait(0.01);
Nilox 0:3b2f23672f26 119 }
Nilox 0:3b2f23672f26 120
Nilox 0:3b2f23672f26 121 servo.pulsewidth(0);
Nilox 0:3b2f23672f26 122 i=0; // Reinitialisation de i pour pouvoir le reutiliser
Nilox 0:3b2f23672f26 123 while(i<nbMes){ // Affichage des mesures
Nilox 0:3b2f23672f26 124 //pc.printf("M(%d):[%f]\n",i,tabM[i]);
Nilox 1:19d43c4e741f 125 //BT.printf("*AM(%d):[%f]\n*",i ,tabM[i]);
Nilox 0:3b2f23672f26 126
Nilox 0:3b2f23672f26 127 i=i++;
Nilox 0:3b2f23672f26 128 }
Nilox 0:3b2f23672f26 129
Nilox 0:3b2f23672f26 130 //pc.printf("\n Aire totale : %f \n", aireTotale);
Nilox 0:3b2f23672f26 131 BT.printf("*A\n Aire totale : %f \n*", aireTotale);
Nilox 0:3b2f23672f26 132
Nilox 0:3b2f23672f26 133 //pc.printf("\n Pour une piece rectangle : %f \n", aireTotale*1.285);
Nilox 0:3b2f23672f26 134 //BT.printf("*A\n Pour une piece rectangle : %f \n*", aireTotale*1.285);
Nilox 0:3b2f23672f26 135
Nilox 0:3b2f23672f26 136
Nilox 0:3b2f23672f26 137 // pc.printf("\nArret du programme...");
Nilox 0:3b2f23672f26 138 BT.printf("*A\nArret du programme...*");
Nilox 0:3b2f23672f26 139 }
Nilox 0:3b2f23672f26 140 //////////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 141
Nilox 0:3b2f23672f26 142 /////////////////////////////////////////////////////////////////////////////////
Nilox 0:3b2f23672f26 143 int main(void)
Nilox 0:3b2f23672f26 144 {
Nilox 0:3b2f23672f26 145 char m;
Nilox 0:3b2f23672f26 146
Nilox 0:3b2f23672f26 147 while(1) {
Nilox 0:3b2f23672f26 148
Nilox 0:3b2f23672f26 149 if(BT.readable()){
Nilox 0:3b2f23672f26 150
Nilox 0:3b2f23672f26 151 m = BT.getc();
Nilox 0:3b2f23672f26 152 if(m == '*'){
Nilox 0:3b2f23672f26 153
Nilox 0:3b2f23672f26 154 m = BT.getc();
Nilox 0:3b2f23672f26 155 if(m == 'l'){
Nilox 0:3b2f23672f26 156 //pc.printf("Un paquet lancement\n");
Nilox 0:3b2f23672f26 157 //BT.printf("*AUn paquet lancement\n*");
Nilox 0:3b2f23672f26 158 programme();
Nilox 0:3b2f23672f26 159 }
Nilox 0:3b2f23672f26 160
Nilox 0:3b2f23672f26 161 }
Nilox 0:3b2f23672f26 162
Nilox 0:3b2f23672f26 163 }
Nilox 0:3b2f23672f26 164
Nilox 0:3b2f23672f26 165 }
Nilox 0:3b2f23672f26 166
Nilox 0:3b2f23672f26 167 }
Nilox 0:3b2f23672f26 168 //////////////////////////////////////////////////////////////////////////////////////