Revision 5:8eba0e0f29e3, committed 2017-01-30

Comitter:

EmbeddedSam

Date:

Mon Jan 30 11:29:12 2017 +0000

Parent:

4:dd88ad16a3f2

Commit message:

WORKING WITHOUT SENSORS

Changed in this revision

--- a/main.cpp	Mon Oct 24 13:01:59 2016 +0000
+++ b/main.cpp	Mon Jan 30 11:29:12 2017 +0000
@@ -30,7 +30,16 @@
 #include "MQTTEthernet.h"
 #include "MQTTClient.h"
 
-HCSR04 UltrasonicSensor(D4,D5); //Trigger,Echo 
+HCSR04 UltrasonicSensor_FrontLeft(D2,D3); //Trigger,Echo 
+HCSR04 UltrasonicSensor_FrontCentre(D4,D5); //Trigger,Echo 
+HCSR04 UltrasonicSensor_FrontRight(D6,D7); //Trigger,Echo 
+
+
+HCSR04 UltrasonicSensor_BackLeft(PG_5,PG_6); //Trigger,Echo 
+HCSR04 UltrasonicSensor_BackCentre(PE_0,PG_8); //Trigger,Echo 
+HCSR04 UltrasonicSensor_BackRight(PF_15,PF_11); //Trigger,Echo 
+Serial pc(USBTX, USBRX);
+
 float distance;
  
 EthernetInterface eth;
@@ -54,15 +63,11 @@
     printf("Starting to connect to Ethernet\r\n");
     
     MQTTEthernet ipstack = MQTTEthernet();
-    float version = 0.5;
-    char* topic = "ultrasound_front";
-    
-    printf("HelloMQTT: version is %f\n", version);
               
     MQTT::Client<MQTTEthernet, Countdown> client = MQTT::Client<MQTTEthernet, Countdown>(ipstack);
     
     //This is just my MQTT broker, this could be anything//
-    char* hostname = "10.42.0.54";
+    char* hostname = "192.168.1.2";
     int port = 1883;
     printf("Connecting to %s:%d\n", hostname, port);
     int rc = ipstack.connect(hostname, port);
@@ -82,10 +87,6 @@
     if ((rc = client.connect(data)) != 0)
         printf("rc from MQTT connect is %d\n", rc);
     
-    //Subscribe to topic
-     printf("Subscribing to topic\r\n");
-    if ((rc = client.subscribe(topic, MQTT::QOS2, messageArrived)) != 0)
-        printf("rc from MQTT subscribe is %d\n", rc);
 
     //Start building Message
     MQTT::Message message;
@@ -99,20 +100,107 @@
     while(1)
     {
         //I have not fully benchmarked this loop but by eye it seems to be publishing sensor data about 10Hz TODO:Speed it up as much as possible and properly characterise sensor update rates
-        distance = UltrasonicSensor.distance();
-        wait_ms(50);
+        distance = UltrasonicSensor_FrontLeft.distance();
+        wait_ms(10);
+        if(distance == 17182){
+            //this is just 999999 error code ran through distance calc, i should change this but 
+            printf("\n\rOut of range ->  echo timed out");
+            distance = 99999;
+        }
+        else{
+            printf("\n\rDistance is: %0.2f", distance);
+        }
+        
+        sprintf(buf, "%d", int(distance));
+        message.payload = (void*)buf;
+        message.payloadlen = strlen(buf)+1;
+        rc = client.publish("RobotSensors/Ultrasound/FrontLeft", message);
+        
+        //Read Front Centre
+        distance = UltrasonicSensor_FrontCentre.distance();
+        wait_ms(10);
         if(distance == 17182){
-            //this is just 999999 error code ran through distance calc
+            //this is just 999999 error code ran through distance calc, i should change this but 
             printf("\n\rOut of range ->  echo timed out");
+            distance = 99999;
+        }
+        else{
+            printf("\n\rDistance is: %0.2f", distance);
+        }
+        
+        sprintf(buf, "%d", int(distance));
+        message.payload = (void*)buf;
+        message.payloadlen = strlen(buf)+1;
+        rc = client.publish("RobotSensors/Ultrasound/FrontCentre", message);
+        
+        //Read Front Right
+        distance = UltrasonicSensor_FrontRight.distance();
+        wait_ms(10);
+        if(distance == 17182){
+            //this is just 999999 error code ran through distance calc, i should change this but 
+            printf("\n\rOut of range ->  echo timed out");
+            distance = 99999;
         }
         else{
             printf("\n\rDistance is: %0.2f", distance);
         }
         
-        sprintf(buf, " %f\n", distance);
+        sprintf(buf, "%d", int(distance));
+        message.payload = (void*)buf;
+        message.payloadlen = strlen(buf)+1;
+        rc = client.publish("RobotSensors/Ultrasound/FrontRight", message);
+        
+        
+        
+        distance = UltrasonicSensor_BackLeft.distance();
+        wait_ms(10);
+        if(distance == 17182){
+            //this is just 999999 error code ran through distance calc, i should change this but 
+            printf("\n\rOut of range ->  echo timed out");
+            distance = 99999;
+        }
+        else{
+            printf("\n\rDistance is: %0.2f", distance);
+        }
+        
+        sprintf(buf, "%d", int(distance));
         message.payload = (void*)buf;
         message.payloadlen = strlen(buf)+1;
-        rc = client.publish(topic, message);
+        rc = client.publish("RobotSensors/Ultrasound/BackLeft", message);
+        
+        //Read Front Centre
+        distance = UltrasonicSensor_BackCentre.distance();
+        wait_ms(10);
+        if(distance == 17182){
+            //this is just 999999 error code ran through distance calc, i should change this but 
+            printf("\n\rOut of range ->  echo timed out");
+            distance = 99999;
+        }
+        else{
+            printf("\n\rDistance is: %0.2f", distance);
+        }
+        
+        sprintf(buf, "%d", int(distance));
+        message.payload = (void*)buf;
+        message.payloadlen = strlen(buf)+1;
+        rc = client.publish("RobotSensors/Ultrasound/BackCentre", message);
+        
+        //Read Front Right
+        distance = UltrasonicSensor_BackRight.distance();
+        wait_ms(10);
+        if(distance == 17182){
+            //this is just 999999 error code ran through distance calc, i should change this but 
+            printf("\n\rOut of range ->  echo timed out");
+            distance = 99999;
+        }
+        else{
+            printf("\n\rDistance is: %0.2f", distance);
+        }
+        
+        sprintf(buf, "%d", int(distance));
+        message.payload = (void*)buf;
+        message.payloadlen = strlen(buf)+1;
+        rc = client.publish("RobotSensors/Ultrasound/BackRight", message);
     }
     //client.yield(100);              
 //    if ((rc = client.unsubscribe(topic)) != 0)