mm
Dependencies: CMPS03 SRF05 mbed pixy
Diff: fct.cpp
- Revision:
- 0:6c5fac591b01
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fct.cpp Thu Feb 08 19:35:15 2018 +0000 @@ -0,0 +1,72 @@ +#include "mbed.h" +#include "fct.h" +#include "SRF05.h" + +void vitmoteur(float VitG, float VitD) +{ + if(VitG<0) { + VitG=-1*VitG; + cmdI2C=cmdI2C&0xfe; //passe le moteur gauche en marche arriere 00000001 + } else { + cmdI2C=cmdI2C|0x01; // marche avant 11110111 mot gauche + } + if(VitD<0) { + VitD=-1*VitD; + cmdI2C=cmdI2C&0xfd; //passe le moteur gauche en marche arriere 00000100 + } else { + cmdI2C=cmdI2C|0x02;//marche avant 11111011 mot droit + } + monI2C.write(ADR_PCF,&cmdI2C,1); + MotG.pulsewidth(((100-VitG)/100.0)*PERIOD); + MotD.pulsewidth(((100-VitD)/100.0)*PERIOD); +} +void lecture_blanc(void) +{ + if(C1.read()>0.5) { + captL1=0; + } else { + captL1=1; + } + if(C3.read()>0.5) { + captL3=0; + } else { + captL3=1; + } +} + +void lecture_an(void) +{ + capt_av=SD_2.read()*3.3; + capt_av=(10/(capt_av-0.5))-0.42; + if(capt_av<0) + { + capt_av=80; + } + + distance_av=a/(3.3*LD_2.read()-b); + if(distance_av>120 || distance_av<20) + { + distance_av=0; + } + +} + + + +void lecture_us(void) +{ + us_arriere=us_arr.read(); +} + +void init(void) +{ + bp.mode(PullUp); + MotG.period(PERIOD); + MotD.period(PERIOD); + //vitmoteur(0,0); +} + +void lecture_boussole(void) +{ + gBoussole=Boussole.readBearing() / 10.0; +} \ No newline at end of file