Weber Yang
Delta for AGV
Dependencies: mbed HC-SR04 ros_lib_kinetic
Revision 7:35767da0302c, committed 2019-07-08
- Comitter:
- WeberYang
- Date:
- Mon Jul 08 01:24:01 2019 +0000
- Parent:
- 6:bca0839e8295
- Commit message:
- Delta for AGV ultrasonic
Changed in this revision
--- a/AutomationElements.lib Sat Dec 10 08:28:06 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/teams/TVZ-Mechatronics-Team/code/AutomationElements/#b9e11da0f2eb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HC-SR04.lib Mon Jul 08 01:24:01 2019 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Nestordp/code/HC-SR04/#d1d7bb1c1f6c
--- a/HCSR04.lib Sat Dec 10 08:28:06 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://developer.mbed.org/teams/TVZ-Mechatronics-Team/code/HCSR04/#cf3e4e307d15
--- 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Mon Jul 08 01:24:01 2019 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f