File content as of revision 7:35767da0302c:

#include "mbed.h"
#include "HCSR04.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);

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() {           

    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() {
    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;
        }
    }
}