Diff: main.cpp

Revision:

1:f4ae12a86f80

Parent:

0:31a72c321609

Child:

2:3f2d37164a10

--- a/main.cpp	Thu Feb 02 15:37:06 2017 +0000
+++ b/main.cpp	Fri Feb 03 10:34:43 2017 +0000
@@ -1,8 +1,6 @@
 #include "mbed.h"
 #include "hcsr04.h"
 
-//I2C to_main(dp4,dp5);
-//I2CSlave to_main(dp4,dp5);
 Serial to_main(dp4,dp5);
 //Serial pc(USBTX,USBRX);
 
@@ -11,25 +9,33 @@
 HCSR04 right(dp20,dp21);
 HCSR04 left(dp18,dp19);
 
-//DigitalOut led(dp12);
 
-//const int addr=0x0A;
 
 int main() {
     char dist[4]={0};
     //to_main.baud(19200);
-    //to_main.address(addr);
     while(1) {
         front.start();
         back.start();
         right.start();
         left.start();
-        dist[0]=front.get_dist_cm();
-        dist[1]=back.get_dist_cm();
-        dist[2]=right.get_dist_cm();
-        dist[3]=left.get_dist_cm();
-        //led=to_main.write(addr,dist,4);
-        //led=to_main.write(dist, 4);
+        if(front.get_dist_cm()>255)
+            dist[0]=255;
+        else
+            dist[0]=front.get_dist_cm();
+        if(back.get_dist_cm()>255)
+            dist[1]=255;
+        else
+            dist[1]=back.get_dist_cm();
+        if(right.get_dist_cm()>255)
+            dist[2]=255;
+        else
+            dist[2]=right.get_dist_cm();
+        if(left.get_dist_cm()>255)
+            dist[3]=255;
+        else
+            dist[3]=left.get_dist_cm();
+
         to_main.putc(1);
         for(int i=0;i<4;i++){
             to_main.putc(dist[i]);