Diff: main.cpp

Revision:

7:35767da0302c

Parent:

6:bca0839e8295

--- a/main.cpp	Sat Dec 10 08:28:06 2016 +0000
+++ b/main.cpp	Mon Jul 08 01:24:01 2019 +0000
@@ -1,31 +1,184 @@
 #include "mbed.h"
 #include "HCSR04.h"
-#include "AutomationElements.h"
+//#include "AutomationElements.h"
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Float32.h>
+ros::NodeHandle  nh;
+DigitalOut led1(LED1);
+std_msgs::Float32 str_msg1;
+std_msgs::Float32 str_msg2;
+std_msgs::Float32 str_msg3;
+std_msgs::Float32 str_msg4;
+std_msgs::Float32 str_msg5;
+std_msgs::Float32 str_msg6;
+
+ros::Publisher sonar1("sonar1", &str_msg1);
+ros::Publisher sonar2("sonar2", &str_msg2);
+ros::Publisher sonar3("sonar3", &str_msg3);
+ros::Publisher sonar4("sonar4", &str_msg4);
+ros::Publisher sonar5("sonar5", &str_msg5);
+ros::Publisher sonar6("sonar6", &str_msg6);
+
+
+float distance1=0;
+float distance2=0;
+float distance3=0;
+float distance4=0;
+float distance5=0;
+float distance6=0;
+//Serial pc(USBTX, USBRX);
+/*
+DigitalOut          CAN_T(D14);//PB_9
+DigitalOut          CAN_R(D15);//PB_8
+DigitalOut          DO_0(PC_5);
+DigitalOut          DO_1(PC_6);
+DigitalOut          DO_2(PC_8);
+DigitalOut          DO_3(PC_9);
+DigitalOut          DO_4(PA_12);//main relay
+DigitalOut          DO_locate_sensor(PB_11);
 
-Serial pc(USBTX, USBRX);
-HCSR04 sensor(p5, p7);
-float sampleTime = 0.5;
-PT1 filter(1, 2, sampleTime);
-Ticker ticker;
-float distance;
+DigitalIn           DI_0(PB_13);
+DigitalIn           DI_locate_sensor(PB_14);
+DigitalIn           Demo_mode(PB_15);
+*/
+//t,e
+HCSR04 sensor1(PC_10, PC_11);
+HCSR04 sensor2(PC_12,PD_2);
+HCSR04 sensor3(PA_4,PB_0);//(A2, A3);
+
+HCSR04 sensor4(PC_1, PC_0);
+HCSR04 sensor5(PC_14,PC_15);
+HCSR04 sensor6(PC_2,PC_3);//(A2, A3);
+
+
+
+float sampleTime = 0.1;
+//PT1 filter(1, 2, sampleTime);
+Timer t1;
 float filteredDistance; 
+int count;
+void calc1() {           
 
-void calc() {
-    sensor.startMeasurement();
+    distance1 = sensor1.getCm();//*0.2+str_msg1.data*0.8;    
+    
+    if (distance1>100)
+    distance1 = 100;
+    
+    if (distance1<0)
+    distance1 = 0;
+    
+    str_msg1.data  = distance1;
+}
+void calc2() {       
+    distance2 = sensor2.getCm();//*0.2+str_msg1.data*0.8;    
+    
+    if (distance2>100)
+    distance2 = 100;
+    
+    if (distance2<0)
+    distance2 = 0;
+    
+    str_msg2.data  = distance2;
+}
+void calc3() {           
+    distance3 = sensor3.getCm();//*0.2+str_msg1.data*0.8;    
+    
+    if (distance3>100)
+    distance3 = 100;
+    
+    if (distance3<0)
+    distance3 = 0;
+    
+    str_msg3.data  = distance3;
 }
 
+
+void calc4() {           
+    distance4 = sensor4.getCm();//*0.2+str_msg1.data*0.8;    
+    
+    if (distance4>100)
+    distance4 = 100;
+    
+    if (distance4<0)
+    distance4 = 0;
+    
+    str_msg4.data  = distance4;
+}
+
+void calc5() {           
+    distance5 = sensor5.getCm();//*0.2+str_msg1.data*0.8;    
+    
+    if (distance5>100)
+    distance5 = 100;
+    
+    if (distance5<0)
+    distance5 = 0;
+    
+    str_msg5.data  = distance5;
+}
+
+void calc6() {           
+    distance6 = sensor6.getCm();//*0.2+str_msg1.data*0.8;    
+    
+    if (distance6>100)
+    distance6 = 100;
+    
+    if (distance6<0)
+    distance6 = 0;
+    
+    str_msg6.data  = distance6;
+}
 int main() {
-    sensor.setRanges(2, 400);
-    pc.printf("Minimum sensor range = %g cm\n\rMaximum sensor range = %g cm\n\r", sensor.getMinRange(), sensor.getMaxRange());
-    pc.printf("Sensor:       Filtered:\n\r");
-    ticker.attach(&calc, sampleTime);
-    while(true) {
-        while(!sensor.isNewDataReady()) {
-        // wait for new data
+    nh.initNode();
+    nh.advertise(sonar1);
+    nh.advertise(sonar2);
+    nh.advertise(sonar3);
+    nh.advertise(sonar4);
+    nh.advertise(sonar5);
+    nh.advertise(sonar6);
+//    ticker1.attach(&calc1, sampleTime);
+//    ticker2.attach(&calc2, sampleTime);
+//    ticker3.attach(&calc3, sampleTime);
+    t1.start();
+    while(true) {             
+        if (t1.read_us()>10000){
+            t1.reset();
+            t1.start();
+            str_msg1.data = sensor1.getCm()*0.8 + str_msg1.data*0.2;
+            sonar1.publish( &str_msg1 );
+            wait_ms(1);        
+            nh.spinOnce();        
+            
+            str_msg2.data = sensor2.getCm()*0.8 + str_msg2.data*0.2;
+            sonar2.publish( &str_msg2 );
+            wait_ms(1);        
+            nh.spinOnce();        
+            
+            str_msg3.data = sensor3.getCm()*0.8 + str_msg3.data*0.2;
+            sonar3.publish( &str_msg3 );
+            wait_ms(1);        
+            nh.spinOnce();        
+            
+            str_msg4.data = sensor4.getCm()*0.8 + str_msg4.data*0.2;
+            sonar4.publish( &str_msg4 );
+            wait_ms(1);        
+            nh.spinOnce();        
+            
+            str_msg5.data = sensor5.getCm()*0.8 + str_msg5.data*0.2;
+            sonar5.publish( &str_msg5 );
+            wait_ms(1);        
+            nh.spinOnce();        
+            
+            str_msg6.data = sensor6.getCm()*0.8 + str_msg6.data*0.2;
+            sonar6.publish( &str_msg6 );
+            wait_ms(1);        
+            nh.spinOnce();        
+            
+            if( (str_msg1.data<30)||(str_msg2.data<30)||(str_msg3.data<30)||(str_msg4.data<30)||(str_msg5.data<30)||(str_msg6.data<30) )
+            led1 = 1;
+            else
+            led1 = 0;
         }
-        distance = sensor.getDistance_cm();
-        filter.in(distance);
-        filteredDistance = filter.out();
-        pc.printf("%7.1f cm  %7.1f cm\n\r", distance, filteredDistance);
     }
 }
\ No newline at end of file