fct.cpp

00001 #include "mbed.h"
00002 #include "fct.h"
00003 #include "SRF05.h"
00004 
00005 void vitmoteur(float VitG, float VitD)
00006 {
00007     if(VitG<0) {
00008         VitG=-1*VitG;
00009         cmdI2C=cmdI2C&0xfe; //passe le moteur gauche en marche arriere 00000001
00010     } else {
00011         cmdI2C=cmdI2C|0x01; // marche avant 11110111 mot gauche
00012     }
00013     if(VitD<0) {
00014         VitD=-1*VitD;
00015         cmdI2C=cmdI2C&0xfd; //passe le moteur gauche en marche arriere 00000100
00016     } else {
00017         cmdI2C=cmdI2C|0x02;//marche avant 11111011 mot droit
00018     }
00019     monI2C.write(ADR_PCF,&cmdI2C,1);
00020     MotG.pulsewidth(((100-VitG)/100.0)*PERIOD);
00021     MotD.pulsewidth(((100-VitD)/100.0)*PERIOD);
00022 }
00023 void lecture_blanc(void)
00024 {
00025     if(C1.read()>0.5) {
00026         captL1=0;
00027     } else {
00028         captL1=1;
00029     }
00030     if(C3.read()>0.5) {
00031         captL3=0;
00032     } else {
00033         captL3=1;
00034     }
00035 }
00036 
00037 void lecture_us(void)
00038 {
00039     us_arriere=us_arr.read();
00040 }
00041 
00042 void init(void)
00043 {
00044     bp.mode(PullUp);
00045     MotG.period(PERIOD);
00046     MotD.period(PERIOD);
00047     vitmoteur(0,0);
00048 }
00049 
00050 void lecture_boussole(void)
00051 {
00052     gBoussole=Boussole.readBearing() / 10.0;
00053 }