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?
User | Revision | Line number | New 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 | } |