Delta for AGV
Dependencies: mbed HC-SR04 ros_lib_kinetic
main.cpp
- Committer:
- WeberYang
- Date:
- 2019-07-08
- Revision:
- 7:35767da0302c
- Parent:
- 6:bca0839e8295
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; } } }