Delta for AGV
Dependencies: mbed HC-SR04 ros_lib_kinetic
Diff: main.cpp
- Revision:
- 7:35767da0302c
- Parent:
- 6:bca0839e8295
diff -r bca0839e8295 -r 35767da0302c main.cpp --- 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