Diff: fct.cpp

Revision:

0:a8cee96c9250

Child:

2:48f7e5f79caa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fct.cpp	Sat Jun 10 04:58:21 2017 +0000
@@ -0,0 +1,53 @@
+#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_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