Weber Yang / Mbed 2 deprecated mbed_sonar

Delta for AGV

Dependencies:   mbed HC-SR04 ros_lib_kinetic

main.cpp@7:35767da0302c, 2019-07-08 (annotated)

Committer:
WeberYang
Date:
Mon Jul 08 01:24:01 2019 +0000
Revision:
7:35767da0302c
Parent:
6:bca0839e8295 
Delta for AGV ultrasonic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tbjazic 0:ebee649c5b1b 1 #include "mbed.h"
tbjazic 2:f86b1e3609b3 2 #include "HCSR04.h"
WeberYang 7:35767da0302c 3 //#include "AutomationElements.h"
WeberYang 7:35767da0302c 4 #include <ros.h>
WeberYang 7:35767da0302c 5 #include <std_msgs/String.h>
WeberYang 7:35767da0302c 6 #include <std_msgs/Float32.h>
WeberYang 7:35767da0302c 7 ros::NodeHandle nh;
WeberYang 7:35767da0302c 8 DigitalOut led1(LED1);
WeberYang 7:35767da0302c 9 std_msgs::Float32 str_msg1;
WeberYang 7:35767da0302c 10 std_msgs::Float32 str_msg2;
WeberYang 7:35767da0302c 11 std_msgs::Float32 str_msg3;
WeberYang 7:35767da0302c 12 std_msgs::Float32 str_msg4;
WeberYang 7:35767da0302c 13 std_msgs::Float32 str_msg5;
WeberYang 7:35767da0302c 14 std_msgs::Float32 str_msg6;
WeberYang 7:35767da0302c 15
WeberYang 7:35767da0302c 16 ros::Publisher sonar1("sonar1", &str_msg1);
WeberYang 7:35767da0302c 17 ros::Publisher sonar2("sonar2", &str_msg2);
WeberYang 7:35767da0302c 18 ros::Publisher sonar3("sonar3", &str_msg3);
WeberYang 7:35767da0302c 19 ros::Publisher sonar4("sonar4", &str_msg4);
WeberYang 7:35767da0302c 20 ros::Publisher sonar5("sonar5", &str_msg5);
WeberYang 7:35767da0302c 21 ros::Publisher sonar6("sonar6", &str_msg6);
WeberYang 7:35767da0302c 22
WeberYang 7:35767da0302c 23
WeberYang 7:35767da0302c 24 float distance1=0;
WeberYang 7:35767da0302c 25 float distance2=0;
WeberYang 7:35767da0302c 26 float distance3=0;
WeberYang 7:35767da0302c 27 float distance4=0;
WeberYang 7:35767da0302c 28 float distance5=0;
WeberYang 7:35767da0302c 29 float distance6=0;
WeberYang 7:35767da0302c 30 //Serial pc(USBTX, USBRX);
WeberYang 7:35767da0302c 31 /*
WeberYang 7:35767da0302c 32 DigitalOut CAN_T(D14);//PB_9
WeberYang 7:35767da0302c 33 DigitalOut CAN_R(D15);//PB_8
WeberYang 7:35767da0302c 34 DigitalOut DO_0(PC_5);
WeberYang 7:35767da0302c 35 DigitalOut DO_1(PC_6);
WeberYang 7:35767da0302c 36 DigitalOut DO_2(PC_8);
WeberYang 7:35767da0302c 37 DigitalOut DO_3(PC_9);
WeberYang 7:35767da0302c 38 DigitalOut DO_4(PA_12);//main relay
WeberYang 7:35767da0302c 39 DigitalOut DO_locate_sensor(PB_11);
tbjazic 0:ebee649c5b1b 40
WeberYang 7:35767da0302c 41 DigitalIn DI_0(PB_13);
WeberYang 7:35767da0302c 42 DigitalIn DI_locate_sensor(PB_14);
WeberYang 7:35767da0302c 43 DigitalIn Demo_mode(PB_15);
WeberYang 7:35767da0302c 44 */
WeberYang 7:35767da0302c 45 //t,e
WeberYang 7:35767da0302c 46 HCSR04 sensor1(PC_10, PC_11);
WeberYang 7:35767da0302c 47 HCSR04 sensor2(PC_12,PD_2);
WeberYang 7:35767da0302c 48 HCSR04 sensor3(PA_4,PB_0);//(A2, A3);
WeberYang 7:35767da0302c 49
WeberYang 7:35767da0302c 50 HCSR04 sensor4(PC_1, PC_0);
WeberYang 7:35767da0302c 51 HCSR04 sensor5(PC_14,PC_15);
WeberYang 7:35767da0302c 52 HCSR04 sensor6(PC_2,PC_3);//(A2, A3);
WeberYang 7:35767da0302c 53
WeberYang 7:35767da0302c 54
WeberYang 7:35767da0302c 55
WeberYang 7:35767da0302c 56 float sampleTime = 0.1;
WeberYang 7:35767da0302c 57 //PT1 filter(1, 2, sampleTime);
WeberYang 7:35767da0302c 58 Timer t1;
tbjazic 4:052ac3f5c938 59 float filteredDistance;
WeberYang 7:35767da0302c 60 int count;
WeberYang 7:35767da0302c 61 void calc1() {
tbjazic 4:052ac3f5c938 62
WeberYang 7:35767da0302c 63 distance1 = sensor1.getCm();//*0.2+str_msg1.data*0.8;
WeberYang 7:35767da0302c 64
WeberYang 7:35767da0302c 65 if (distance1>100)
WeberYang 7:35767da0302c 66 distance1 = 100;
WeberYang 7:35767da0302c 67
WeberYang 7:35767da0302c 68 if (distance1<0)
WeberYang 7:35767da0302c 69 distance1 = 0;
WeberYang 7:35767da0302c 70
WeberYang 7:35767da0302c 71 str_msg1.data = distance1;
WeberYang 7:35767da0302c 72 }
WeberYang 7:35767da0302c 73 void calc2() {
WeberYang 7:35767da0302c 74 distance2 = sensor2.getCm();//*0.2+str_msg1.data*0.8;
WeberYang 7:35767da0302c 75
WeberYang 7:35767da0302c 76 if (distance2>100)
WeberYang 7:35767da0302c 77 distance2 = 100;
WeberYang 7:35767da0302c 78
WeberYang 7:35767da0302c 79 if (distance2<0)
WeberYang 7:35767da0302c 80 distance2 = 0;
WeberYang 7:35767da0302c 81
WeberYang 7:35767da0302c 82 str_msg2.data = distance2;
WeberYang 7:35767da0302c 83 }
WeberYang 7:35767da0302c 84 void calc3() {
WeberYang 7:35767da0302c 85 distance3 = sensor3.getCm();//*0.2+str_msg1.data*0.8;
WeberYang 7:35767da0302c 86
WeberYang 7:35767da0302c 87 if (distance3>100)
WeberYang 7:35767da0302c 88 distance3 = 100;
WeberYang 7:35767da0302c 89
WeberYang 7:35767da0302c 90 if (distance3<0)
WeberYang 7:35767da0302c 91 distance3 = 0;
WeberYang 7:35767da0302c 92
WeberYang 7:35767da0302c 93 str_msg3.data = distance3;
tbjazic 4:052ac3f5c938 94 }
tbjazic 0:ebee649c5b1b 95
WeberYang 7:35767da0302c 96
WeberYang 7:35767da0302c 97 void calc4() {
WeberYang 7:35767da0302c 98 distance4 = sensor4.getCm();//*0.2+str_msg1.data*0.8;
WeberYang 7:35767da0302c 99
WeberYang 7:35767da0302c 100 if (distance4>100)
WeberYang 7:35767da0302c 101 distance4 = 100;
WeberYang 7:35767da0302c 102
WeberYang 7:35767da0302c 103 if (distance4<0)
WeberYang 7:35767da0302c 104 distance4 = 0;
WeberYang 7:35767da0302c 105
WeberYang 7:35767da0302c 106 str_msg4.data = distance4;
WeberYang 7:35767da0302c 107 }
WeberYang 7:35767da0302c 108
WeberYang 7:35767da0302c 109 void calc5() {
WeberYang 7:35767da0302c 110 distance5 = sensor5.getCm();//*0.2+str_msg1.data*0.8;
WeberYang 7:35767da0302c 111
WeberYang 7:35767da0302c 112 if (distance5>100)
WeberYang 7:35767da0302c 113 distance5 = 100;
WeberYang 7:35767da0302c 114
WeberYang 7:35767da0302c 115 if (distance5<0)
WeberYang 7:35767da0302c 116 distance5 = 0;
WeberYang 7:35767da0302c 117
WeberYang 7:35767da0302c 118 str_msg5.data = distance5;
WeberYang 7:35767da0302c 119 }
WeberYang 7:35767da0302c 120
WeberYang 7:35767da0302c 121 void calc6() {
WeberYang 7:35767da0302c 122 distance6 = sensor6.getCm();//*0.2+str_msg1.data*0.8;
WeberYang 7:35767da0302c 123
WeberYang 7:35767da0302c 124 if (distance6>100)
WeberYang 7:35767da0302c 125 distance6 = 100;
WeberYang 7:35767da0302c 126
WeberYang 7:35767da0302c 127 if (distance6<0)
WeberYang 7:35767da0302c 128 distance6 = 0;
WeberYang 7:35767da0302c 129
WeberYang 7:35767da0302c 130 str_msg6.data = distance6;
WeberYang 7:35767da0302c 131 }
tbjazic 0:ebee649c5b1b 132 int main() {
WeberYang 7:35767da0302c 133 nh.initNode();
WeberYang 7:35767da0302c 134 nh.advertise(sonar1);
WeberYang 7:35767da0302c 135 nh.advertise(sonar2);
WeberYang 7:35767da0302c 136 nh.advertise(sonar3);
WeberYang 7:35767da0302c 137 nh.advertise(sonar4);
WeberYang 7:35767da0302c 138 nh.advertise(sonar5);
WeberYang 7:35767da0302c 139 nh.advertise(sonar6);
WeberYang 7:35767da0302c 140 // ticker1.attach(&calc1, sampleTime);
WeberYang 7:35767da0302c 141 // ticker2.attach(&calc2, sampleTime);
WeberYang 7:35767da0302c 142 // ticker3.attach(&calc3, sampleTime);
WeberYang 7:35767da0302c 143 t1.start();
WeberYang 7:35767da0302c 144 while(true) {
WeberYang 7:35767da0302c 145 if (t1.read_us()>10000){
WeberYang 7:35767da0302c 146 t1.reset();
WeberYang 7:35767da0302c 147 t1.start();
WeberYang 7:35767da0302c 148 str_msg1.data = sensor1.getCm()*0.8 + str_msg1.data*0.2;
WeberYang 7:35767da0302c 149 sonar1.publish( &str_msg1 );
WeberYang 7:35767da0302c 150 wait_ms(1);
WeberYang 7:35767da0302c 151 nh.spinOnce();
WeberYang 7:35767da0302c 152
WeberYang 7:35767da0302c 153 str_msg2.data = sensor2.getCm()*0.8 + str_msg2.data*0.2;
WeberYang 7:35767da0302c 154 sonar2.publish( &str_msg2 );
WeberYang 7:35767da0302c 155 wait_ms(1);
WeberYang 7:35767da0302c 156 nh.spinOnce();
WeberYang 7:35767da0302c 157
WeberYang 7:35767da0302c 158 str_msg3.data = sensor3.getCm()*0.8 + str_msg3.data*0.2;
WeberYang 7:35767da0302c 159 sonar3.publish( &str_msg3 );
WeberYang 7:35767da0302c 160 wait_ms(1);
WeberYang 7:35767da0302c 161 nh.spinOnce();
WeberYang 7:35767da0302c 162
WeberYang 7:35767da0302c 163 str_msg4.data = sensor4.getCm()*0.8 + str_msg4.data*0.2;
WeberYang 7:35767da0302c 164 sonar4.publish( &str_msg4 );
WeberYang 7:35767da0302c 165 wait_ms(1);
WeberYang 7:35767da0302c 166 nh.spinOnce();
WeberYang 7:35767da0302c 167
WeberYang 7:35767da0302c 168 str_msg5.data = sensor5.getCm()*0.8 + str_msg5.data*0.2;
WeberYang 7:35767da0302c 169 sonar5.publish( &str_msg5 );
WeberYang 7:35767da0302c 170 wait_ms(1);
WeberYang 7:35767da0302c 171 nh.spinOnce();
WeberYang 7:35767da0302c 172
WeberYang 7:35767da0302c 173 str_msg6.data = sensor6.getCm()*0.8 + str_msg6.data*0.2;
WeberYang 7:35767da0302c 174 sonar6.publish( &str_msg6 );
WeberYang 7:35767da0302c 175 wait_ms(1);
WeberYang 7:35767da0302c 176 nh.spinOnce();
WeberYang 7:35767da0302c 177
WeberYang 7:35767da0302c 178 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) )
WeberYang 7:35767da0302c 179 led1 = 1;
WeberYang 7:35767da0302c 180 else
WeberYang 7:35767da0302c 181 led1 = 0;
tbjazic 6:bca0839e8295 182 }
tbjazic 0:ebee649c5b1b 183 }
tbjazic 0:ebee649c5b1b 184 }