carte esclave petit robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

Capteurs/Capteur.cpp

Committer:
Artiom
Date:
2019-05-16
Revision:
7:553f3f1c2c53
Parent:
0:bc74da1c502f
Child:
8:8aeb718824ea

File content as of revision 7:553f3f1c2c53:

#include "Capteur.h"
#include "mbed.h"
#include "Actionneurs.h"
#define NOMBRE_TELEMETRE 4
#define precision 5
#define CONSTANTE_TIR 0.639f

///////////////////////////////////////Capteurs de Contact///////////////////////
DigitalIn capteur_contact_block(PB_15);
DigitalIn capteur_contact1(PB_4);
DigitalIn capteur_contact2(PB_5);
DigitalIn capteur_contact3(PH_1);

/////////////////////////////////////Lecture Batterie////////////////////////////
AnalogIn Val_batterie(PC_2);


////////////////////////////////////////Télémètres///////////////////////////////
//distance
AnalogIn telemetre1(PB_0);
AnalogIn telemetre2(PC_5);
AnalogIn telemetre3(PC_4);
AnalogIn telemetre4(PA_5);
//seuil logique
DigitalIn telemetre1_isr(PB_1);
DigitalIn telemetre2_isr(PA_7);
DigitalIn telemetre3_isr(PA_6);
DigitalIn telemetre4_isr(PA_4);
///////////////////////////////////Capteurs  multiCouleurs////////////////////////



/////////////////////////////////////Capteurs monocouleur////////////////////////
DigitalIn monocouleur1(PC_15);
DigitalIn monocouleur2(PC_14);
DigitalIn monocouleur3(PB_10);
DigitalIn monocouleur4(PC_3);

/////////////////////////////////////Capteurs industriels////////////////////////
/*AnalogIn Capteur_indus1(PA_7);
AnalogIn Capteur_indus2(PA_6);
AnalogIn Capteur_indus3(PA_5);
AnalogIn Capteur_indus4(PA_4);
AnalogIn Capteur_indus5(PC_1);
AnalogIn Capteur_indus6(PC_0);*/

short distance_moyenne;

short lecture_telemetre(char numero_telemetre){ // DEGUEUX MAIS FONCTIONNEL :')
    //wait(1);
    float telemetre_distance=0;
    short distance;
    long long distance_moyenne_somme=0;
    for (unsigned char i=0; i<precision;i++){
        if(numero_telemetre==1)
        {
            telemetre_distance=telemetre1.read();
            distance=(((telemetre_distance)-0.2)*4671)+45;
            wait_us(100);
            telemetre_distance=telemetre1.read();
            distance=(((telemetre_distance)-0.2)*4671)+50;
            distance+=86;
            distance=(distance*0.98)+16.6;
        }
        else if (numero_telemetre==2)
        {
            telemetre_distance=telemetre2.read();
            distance=(((telemetre_distance)-0.2)*4671)+45;
            wait_us(100);
            telemetre_distance=telemetre2.read();
            distance=(((telemetre_distance)-0.2)*4671)+50;
            distance+=86;
            distance=(distance*0.975)+25.7;
        }
        else if (numero_telemetre==3)
        {
            telemetre_distance=telemetre3.read();
            distance=(((telemetre_distance)-0.2)*4671)+45;
            wait_us(100);
            telemetre_distance=telemetre3.read();
            distance=(((telemetre_distance)-0.2)*4671)+50;
            distance+=86;
            distance=(distance*0.98)+15.6;
        }
        else if (numero_telemetre==4)
        {
            telemetre_distance=telemetre4.read();
            distance=(((telemetre_distance)-0.2)*4671)+45;
            wait_us(100);
            telemetre_distance=telemetre4.read();
            distance=(((telemetre_distance)-0.2)*4671)+50;
            distance+=86;
            distance=(distance*0.98)+11.15;
        }
        distance_moyenne_somme+=distance;
    }
    distance_moyenne=(distance_moyenne_somme/precision);  
    wait_us(100); 
    return distance_moyenne;
}



float Val_puissance(char zone_tir){
    float voltage=Val_batterie.read()*3.3f;
    float puissance_tir=CONSTANTE_TIR/voltage;
    return puissance_tir;
}
int capteur_contact(void){
    if(capteur_contact_block.read()==1){
        return 1;
    }
    else{
        return 0;
    }
}