mm
Dependencies: CMPS03 SRF05 mbed pixy
fct.cpp@0:6c5fac591b01, 2018-02-08 (annotated)
- Committer:
- pirottealex
- Date:
- Thu Feb 08 19:35:15 2018 +0000
- Revision:
- 0:6c5fac591b01
mm
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pirottealex | 0:6c5fac591b01 | 1 | #include "mbed.h" |
pirottealex | 0:6c5fac591b01 | 2 | #include "fct.h" |
pirottealex | 0:6c5fac591b01 | 3 | #include "SRF05.h" |
pirottealex | 0:6c5fac591b01 | 4 | |
pirottealex | 0:6c5fac591b01 | 5 | void vitmoteur(float VitG, float VitD) |
pirottealex | 0:6c5fac591b01 | 6 | { |
pirottealex | 0:6c5fac591b01 | 7 | if(VitG<0) { |
pirottealex | 0:6c5fac591b01 | 8 | VitG=-1*VitG; |
pirottealex | 0:6c5fac591b01 | 9 | cmdI2C=cmdI2C&0xfe; //passe le moteur gauche en marche arriere 00000001 |
pirottealex | 0:6c5fac591b01 | 10 | } else { |
pirottealex | 0:6c5fac591b01 | 11 | cmdI2C=cmdI2C|0x01; // marche avant 11110111 mot gauche |
pirottealex | 0:6c5fac591b01 | 12 | } |
pirottealex | 0:6c5fac591b01 | 13 | if(VitD<0) { |
pirottealex | 0:6c5fac591b01 | 14 | VitD=-1*VitD; |
pirottealex | 0:6c5fac591b01 | 15 | cmdI2C=cmdI2C&0xfd; //passe le moteur gauche en marche arriere 00000100 |
pirottealex | 0:6c5fac591b01 | 16 | } else { |
pirottealex | 0:6c5fac591b01 | 17 | cmdI2C=cmdI2C|0x02;//marche avant 11111011 mot droit |
pirottealex | 0:6c5fac591b01 | 18 | } |
pirottealex | 0:6c5fac591b01 | 19 | monI2C.write(ADR_PCF,&cmdI2C,1); |
pirottealex | 0:6c5fac591b01 | 20 | MotG.pulsewidth(((100-VitG)/100.0)*PERIOD); |
pirottealex | 0:6c5fac591b01 | 21 | MotD.pulsewidth(((100-VitD)/100.0)*PERIOD); |
pirottealex | 0:6c5fac591b01 | 22 | } |
pirottealex | 0:6c5fac591b01 | 23 | void lecture_blanc(void) |
pirottealex | 0:6c5fac591b01 | 24 | { |
pirottealex | 0:6c5fac591b01 | 25 | if(C1.read()>0.5) { |
pirottealex | 0:6c5fac591b01 | 26 | captL1=0; |
pirottealex | 0:6c5fac591b01 | 27 | } else { |
pirottealex | 0:6c5fac591b01 | 28 | captL1=1; |
pirottealex | 0:6c5fac591b01 | 29 | } |
pirottealex | 0:6c5fac591b01 | 30 | if(C3.read()>0.5) { |
pirottealex | 0:6c5fac591b01 | 31 | captL3=0; |
pirottealex | 0:6c5fac591b01 | 32 | } else { |
pirottealex | 0:6c5fac591b01 | 33 | captL3=1; |
pirottealex | 0:6c5fac591b01 | 34 | } |
pirottealex | 0:6c5fac591b01 | 35 | } |
pirottealex | 0:6c5fac591b01 | 36 | |
pirottealex | 0:6c5fac591b01 | 37 | void lecture_an(void) |
pirottealex | 0:6c5fac591b01 | 38 | { |
pirottealex | 0:6c5fac591b01 | 39 | capt_av=SD_2.read()*3.3; |
pirottealex | 0:6c5fac591b01 | 40 | capt_av=(10/(capt_av-0.5))-0.42; |
pirottealex | 0:6c5fac591b01 | 41 | if(capt_av<0) |
pirottealex | 0:6c5fac591b01 | 42 | { |
pirottealex | 0:6c5fac591b01 | 43 | capt_av=80; |
pirottealex | 0:6c5fac591b01 | 44 | } |
pirottealex | 0:6c5fac591b01 | 45 | |
pirottealex | 0:6c5fac591b01 | 46 | distance_av=a/(3.3*LD_2.read()-b); |
pirottealex | 0:6c5fac591b01 | 47 | if(distance_av>120 || distance_av<20) |
pirottealex | 0:6c5fac591b01 | 48 | { |
pirottealex | 0:6c5fac591b01 | 49 | distance_av=0; |
pirottealex | 0:6c5fac591b01 | 50 | } |
pirottealex | 0:6c5fac591b01 | 51 | |
pirottealex | 0:6c5fac591b01 | 52 | } |
pirottealex | 0:6c5fac591b01 | 53 | |
pirottealex | 0:6c5fac591b01 | 54 | |
pirottealex | 0:6c5fac591b01 | 55 | |
pirottealex | 0:6c5fac591b01 | 56 | void lecture_us(void) |
pirottealex | 0:6c5fac591b01 | 57 | { |
pirottealex | 0:6c5fac591b01 | 58 | us_arriere=us_arr.read(); |
pirottealex | 0:6c5fac591b01 | 59 | } |
pirottealex | 0:6c5fac591b01 | 60 | |
pirottealex | 0:6c5fac591b01 | 61 | void init(void) |
pirottealex | 0:6c5fac591b01 | 62 | { |
pirottealex | 0:6c5fac591b01 | 63 | bp.mode(PullUp); |
pirottealex | 0:6c5fac591b01 | 64 | MotG.period(PERIOD); |
pirottealex | 0:6c5fac591b01 | 65 | MotD.period(PERIOD); |
pirottealex | 0:6c5fac591b01 | 66 | //vitmoteur(0,0); |
pirottealex | 0:6c5fac591b01 | 67 | } |
pirottealex | 0:6c5fac591b01 | 68 | |
pirottealex | 0:6c5fac591b01 | 69 | void lecture_boussole(void) |
pirottealex | 0:6c5fac591b01 | 70 | { |
pirottealex | 0:6c5fac591b01 | 71 | gBoussole=Boussole.readBearing() / 10.0; |
pirottealex | 0:6c5fac591b01 | 72 | } |