main.cpp

00001 #include "mbed.h"
00002 #include "HCSR04.h"
00003 //#include "AutomationElements.h"
00004 #include <ros.h>
00005 #include <std_msgs/String.h>
00006 #include <std_msgs/Float32.h>
00007 ros::NodeHandle  nh;
00008 DigitalOut led1(LED1);
00009 std_msgs::Float32 str_msg1;
00010 std_msgs::Float32 str_msg2;
00011 std_msgs::Float32 str_msg3;
00012 std_msgs::Float32 str_msg4;
00013 std_msgs::Float32 str_msg5;
00014 std_msgs::Float32 str_msg6;
00015 
00016 ros::Publisher sonar1("sonar1", &str_msg1);
00017 ros::Publisher sonar2("sonar2", &str_msg2);
00018 ros::Publisher sonar3("sonar3", &str_msg3);
00019 ros::Publisher sonar4("sonar4", &str_msg4);
00020 ros::Publisher sonar5("sonar5", &str_msg5);
00021 ros::Publisher sonar6("sonar6", &str_msg6);
00022 
00023 
00024 float distance1=0;
00025 float distance2=0;
00026 float distance3=0;
00027 float distance4=0;
00028 float distance5=0;
00029 float distance6=0;
00030 //Serial pc(USBTX, USBRX);
00031 /*
00032 DigitalOut          CAN_T(D14);//PB_9
00033 DigitalOut          CAN_R(D15);//PB_8
00034 DigitalOut          DO_0(PC_5);
00035 DigitalOut          DO_1(PC_6);
00036 DigitalOut          DO_2(PC_8);
00037 DigitalOut          DO_3(PC_9);
00038 DigitalOut          DO_4(PA_12);//main relay
00039 DigitalOut          DO_locate_sensor(PB_11);
00040 
00041 DigitalIn           DI_0(PB_13);
00042 DigitalIn           DI_locate_sensor(PB_14);
00043 DigitalIn           Demo_mode(PB_15);
00044 */
00045 //t,e
00046 HCSR04 sensor1(PC_10, PC_11);
00047 HCSR04 sensor2(PC_12,PD_2);
00048 HCSR04 sensor3(PA_4,PB_0);//(A2, A3);
00049 
00050 HCSR04 sensor4(PC_1, PC_0);
00051 HCSR04 sensor5(PC_14,PC_15);
00052 HCSR04 sensor6(PC_2,PC_3);//(A2, A3);
00053 
00054 
00055 
00056 float sampleTime = 0.1;
00057 //PT1 filter(1, 2, sampleTime);
00058 Timer t1;
00059 float filteredDistance; 
00060 int count;
00061 void calc1() {           
00062 
00063     distance1 = sensor1.getCm();//*0.2+str_msg1.data*0.8;    
00064     
00065     if (distance1>100)
00066     distance1 = 100;
00067     
00068     if (distance1<0)
00069     distance1 = 0;
00070     
00071     str_msg1.data  = distance1;
00072 }
00073 void calc2() {       
00074     distance2 = sensor2.getCm();//*0.2+str_msg1.data*0.8;    
00075     
00076     if (distance2>100)
00077     distance2 = 100;
00078     
00079     if (distance2<0)
00080     distance2 = 0;
00081     
00082     str_msg2.data  = distance2;
00083 }
00084 void calc3() {           
00085     distance3 = sensor3.getCm();//*0.2+str_msg1.data*0.8;    
00086     
00087     if (distance3>100)
00088     distance3 = 100;
00089     
00090     if (distance3<0)
00091     distance3 = 0;
00092     
00093     str_msg3.data  = distance3;
00094 }
00095 
00096 
00097 void calc4() {           
00098     distance4 = sensor4.getCm();//*0.2+str_msg1.data*0.8;    
00099     
00100     if (distance4>100)
00101     distance4 = 100;
00102     
00103     if (distance4<0)
00104     distance4 = 0;
00105     
00106     str_msg4.data  = distance4;
00107 }
00108 
00109 void calc5() {           
00110     distance5 = sensor5.getCm();//*0.2+str_msg1.data*0.8;    
00111     
00112     if (distance5>100)
00113     distance5 = 100;
00114     
00115     if (distance5<0)
00116     distance5 = 0;
00117     
00118     str_msg5.data  = distance5;
00119 }
00120 
00121 void calc6() {           
00122     distance6 = sensor6.getCm();//*0.2+str_msg1.data*0.8;    
00123     
00124     if (distance6>100)
00125     distance6 = 100;
00126     
00127     if (distance6<0)
00128     distance6 = 0;
00129     
00130     str_msg6.data  = distance6;
00131 }
00132 int main() {
00133     nh.initNode();
00134     nh.advertise(sonar1);
00135     nh.advertise(sonar2);
00136     nh.advertise(sonar3);
00137     nh.advertise(sonar4);
00138     nh.advertise(sonar5);
00139     nh.advertise(sonar6);
00140 //    ticker1.attach(&calc1, sampleTime);
00141 //    ticker2.attach(&calc2, sampleTime);
00142 //    ticker3.attach(&calc3, sampleTime);
00143     t1.start();
00144     while(true) {             
00145         if (t1.read_us()>10000){
00146             t1.reset();
00147             t1.start();
00148             str_msg1.data = sensor1.getCm()*0.8 + str_msg1.data*0.2;
00149             sonar1.publish( &str_msg1 );
00150             wait_ms(1);        
00151             nh.spinOnce();        
00152             
00153             str_msg2.data = sensor2.getCm()*0.8 + str_msg2.data*0.2;
00154             sonar2.publish( &str_msg2 );
00155             wait_ms(1);        
00156             nh.spinOnce();        
00157             
00158             str_msg3.data = sensor3.getCm()*0.8 + str_msg3.data*0.2;
00159             sonar3.publish( &str_msg3 );
00160             wait_ms(1);        
00161             nh.spinOnce();        
00162             
00163             str_msg4.data = sensor4.getCm()*0.8 + str_msg4.data*0.2;
00164             sonar4.publish( &str_msg4 );
00165             wait_ms(1);        
00166             nh.spinOnce();        
00167             
00168             str_msg5.data = sensor5.getCm()*0.8 + str_msg5.data*0.2;
00169             sonar5.publish( &str_msg5 );
00170             wait_ms(1);        
00171             nh.spinOnce();        
00172             
00173             str_msg6.data = sensor6.getCm()*0.8 + str_msg6.data*0.2;
00174             sonar6.publish( &str_msg6 );
00175             wait_ms(1);        
00176             nh.spinOnce();        
00177             
00178             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) )
00179             led1 = 1;
00180             else
00181             led1 = 0;
00182         }
00183     }
00184 }