Diff: main.cpp

Revision:

0:8637a4213254

Child:

2:64a1dd5f6dc7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 18 12:03:09 2018 +0000
@@ -0,0 +1,118 @@
+#include "mbed.h"
+#include "SG90.h"
+#include "VL53L0X.h"
+#define USE_XSHUT   0
+
+DigitalIn Btn1(PC_13);//  USER_BUTTON
+SG90 sg(D9);
+
+//  Constructor ----------------------------------------------------------------
+DigitalOut  myled(LED1);
+Serial      pc(USBTX, USBRX);
+VL53L0X     sensor(I2C_SDA,I2C_SCL,NC);        // XSHUT = NC
+Timer       t;
+
+//  ROM / Constant data --------------------------------------------------------
+char *const msg0="VL53L0X is running correctly!!\r\n";
+char *const msg1="VL53L0X -> something is wrong!!\r\n";
+char *const msg2="#,";
+char *const msg3="d[mm]=,";
+char *const msg4="d[mm]=,error,";
+char *const msg5="VL53[mS]=, ";
+char *const msg6="all[mS]=, ";
+
+//L298n connections
+DigitalOut pinI1(D5);
+DigitalOut pinI2(D6);
+DigitalOut pinI3(D10);
+DigitalOut pinI4(D11);
+
+int main(){
+    int status=VL53L0X_ERROR_NONE;
+    uint32_t data;
+    uint32_t count=0;
+    uint32_t tm_sensor;
+    uint32_t tm_all_work;
+ 	
+ 	#if USE_XSHUT
+    status = sensor.init_sensor(0x33);  // set new I2C address
+	#else
+    // no control XSHUT then set default address (no other way)
+    status = sensor.init_sensor(VL53L0X_DEFAULT_ADDRESS);
+	#endif
+
+    if (status == VL53L0X_ERROR_NONE)
+        pc.printf(msg0);
+    else{
+        pc.printf(msg1);
+        pc.printf("%d\n",status);
+        }
+    //status = sensor.set_mode(range_long_distance_33ms_200cm);
+    status = sensor.set_mode(range_hi_accurate_200ms_120cm);
+    //status = sensor.set_mode(range_hi_speed_20ms_120cm);
+    if (status == VL53L0X_ERROR_NONE)
+        pc.printf(msg0);
+    else{
+        pc.printf(msg1);
+        pc.printf("%d\n",status);
+        }
+    while(true){
+        t.reset();
+        t.start();
+        //myled = !myled;
+        status = sensor.get_distance(&data);
+        tm_sensor = t.read_ms();
+        if (status == VL53L0X_ERROR_NONE)
+            pc.printf("%s%5d,%s%5d,", msg2, count++, msg3, data);
+        else
+            pc.printf("%s%5d,%s", msg2, count++, msg4);
+        pc.printf("%s%d,%s%d\r\n", msg5, tm_sensor, msg6, tm_all_work);
+        tm_all_work = t.read_ms();
+        if (tm_all_work < 99)
+            wait_ms(100 - tm_all_work);
+
+	/*sg.SetAngle(80);
+	wait_ms(1000);
+	sg.SetAngle(-80);
+	wait_ms(1000);
+	forward();
+	wait_ms(3000);
+	left();
+	wait_ms(1000);
+	right();
+	wait_ms(1000);
+	stop();*/
+	}
+}
+
+void reverse(){
+    pinI1=1;
+    pinI2=0;
+    pinI3=1;
+    pinI4=0;
+}
+
+void forward(){
+    pinI1=0;
+    pinI2=1;
+    pinI3=0;
+    pinI4=1;
+}
+void left(){
+    pinI1=0;
+    pinI2=1;
+    pinI3=0;
+    pinI4=0;
+	}
+void right(){
+    pinI1=0;
+    pinI2=0;
+    pinI3=0;
+    pinI4=1;
+	}
+void stop(){
+    pinI1=0;
+    pinI2=0;
+    pinI3=0;
+    pinI4=0;
+	}