Diff: main.cpp

Revision:

3:e33c80165703

Parent:

2:24b2b10d3bc2

--- a/main.cpp	Wed Aug 02 15:34:50 2017 +0300
+++ b/main.cpp	Wed Aug 09 18:39:52 2017 +0300
@@ -3,6 +3,7 @@
 #include "MQTTEthernet.h"
 #include "rtos.h"
 #include "k64f.h"
+#include "ultrasonic.h"
 
 // connect options for MQTT broker
 #define BROKER "192.168.1.133"//"broker.mqttdashboard.com"   // MQTT broker URL
@@ -14,6 +15,20 @@
 
 Queue<uint32_t, 6> messageQ;
 
+Timer timer;
+
+DigitalOut trigger1(PTC5);
+DigitalIn echo1(PTC7);
+
+DigitalOut trigger2(PTC9);
+DigitalIn echo2(PTC8);
+
+DigitalOut trigger3(PTC0);
+DigitalIn echo3(PTC1);
+
+DigitalOut trigger4(PTB19);
+DigitalIn echo4(PTB18);
+
 // LED color control function
 void controlLED(color_t led_color) {
     switch(led_color) {
@@ -65,11 +80,41 @@
             controlLED(off);
     }        
 }
+void dist1(int distance){
+			pc.printf("| %d mm\t",distance);	
+}
+void dist2(int distance){
+			pc.printf("| %d mm\t",distance);	
+}
+void dist3(int distance){
+			pc.printf("| %d mm\t",distance);	
+}
+void dist4(int distance){
+			pc.printf("| %d mm\t |\n",distance);	
+}
+//ultrasonic mu1(PTC5,PTC7,0.1,0.5,&dist1);
+//ultrasonic mu2(PTC9,PTC8,0.1,.5,&dist1);
+//ultrasonic mu3(PTC0,PTC1,0.1,.5,&dist1);
+//ultrasonic mu4(PTC19,PTC18,0.1,.5,&dist1);
+int main() {
+		int distance;
+		int corr,res;
 
-int main() {
     // turn off LED  
     controlLED(off);
-    
+//	 mu1.startUpdates();
+//	 mu2.startUpdates();
+//	 mu3.startUpdates();
+//	 mu4.startUpdates();
+	
+		timer.reset();
+		timer.start();
+		while(echo1==2){};
+			timer.stop();
+			corr =timer.read_us();
+
+	
+	
     // set SW2 and SW3 to generate interrupt on falling edge 
     switch2.fall(&sw2_ISR);
     switch3.fall(&sw3_ISR);
@@ -117,6 +162,7 @@
         pc.printf("success\r\n");
         
     char* topic = TOPIC;
+		int lenth1;
     
     // subscribe to MQTT topic
     pc.printf("Subscribing to MQTT topic %s: ", topic);
@@ -128,13 +174,68 @@
         
     MQTT::Message message;
     char buf[100];
-    message.qos = MQTT::QOS0;
+    message.qos = MQTT::QOS1;
     message.retained = false;
     message.dup = false;
     message.payload = (void*)buf;
     message.payloadlen = strlen(buf)+1;
     
     while(true) {
+//				mu1.checkDistance();
+//				mu2.checkDistance();
+//      	mu3.checkDistance();
+//				mu4.checkDistance();
+			
+					trigger1=1;
+					timer.reset();
+					wait_us(10);
+					trigger1=0;
+					while(echo1==0){};
+						timer.start();
+					while(echo1==1){};
+						timer.stop();
+						res=timer.read_us();
+						distance =(res-corr)/58.0;
+					pc.printf("| %d\t",distance);
+
+					trigger2=1;
+					timer.reset();
+					wait_us(10);
+					trigger2=0;
+					while(echo2==0){};
+						timer.start();
+					while(echo2==1){};
+						timer.stop();
+						res=timer.read_us();
+						distance =(res-corr)/58.0;
+					pc.printf("| %d\t",distance);
+						
+					trigger3=1;
+					timer.reset();
+					wait_us(10);
+					trigger3=0;
+					while(echo3==0){};
+						timer.start();
+					while(echo3==1){};
+						timer.stop();
+						res=timer.read_us();
+						distance =(res-corr)/58.0;
+					pc.printf("| %d\t",distance);
+						
+					trigger4=1;
+					timer.reset();
+					wait_us(10);
+					trigger4=0;
+					while(echo4==0){};
+						timer.start();
+					while(echo4==1){};
+						timer.stop();
+						res=timer.read_us();
+						distance =(res-corr)/58.0;
+					pc.printf("| %d |\n",distance);
+			
+			
+			
         osEvent switchEvent = messageQ.get(100);
         
         if (switchEvent.value.v == 22 || switchEvent.value.v == 33) {