toss tongbai / Mbed 2 deprecated test_ultrasonic

mitsui

Dependencies:   PwmIn mbed ros_lib_melodic

main.cpp@0:d2a396fcd7dd, 2021-11-01 (annotated)

Committer:
tossapon2944
Date:
Mon Nov 01 10:48:47 2021 +0000
Revision:
0:d2a396fcd7dd
dd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tossapon2944 0:d2a396fcd7dd 1 #include "mbed.h"
tossapon2944 0:d2a396fcd7dd 2 #include <ros.h>
tossapon2944 0:d2a396fcd7dd 3 #include <ros/time.h>
tossapon2944 0:d2a396fcd7dd 4 #include "PwmIn.h"
tossapon2944 0:d2a396fcd7dd 5 #include <std_msgs/Float32.h>
tossapon2944 0:d2a396fcd7dd 6 #include <geometry_msgs/Point.h>
tossapon2944 0:d2a396fcd7dd 7 #include <std_msgs/String.h>
tossapon2944 0:d2a396fcd7dd 8 #include <geometry_msgs/Vector3.h>
tossapon2944 0:d2a396fcd7dd 9 #include <std_msgs/Float32MultiArray.h>
tossapon2944 0:d2a396fcd7dd 10 #include <std_msgs/Int32MultiArray.h>
tossapon2944 0:d2a396fcd7dd 11 #include <std_msgs/Int16MultiArray.h>
tossapon2944 0:d2a396fcd7dd 12 #include <std_msgs/Int8.h>
tossapon2944 0:d2a396fcd7dd 13 #include <geometry_msgs/Twist.h>
tossapon2944 0:d2a396fcd7dd 14 #include <actionlib_msgs/GoalStatusArray.h>
tossapon2944 0:d2a396fcd7dd 15
tossapon2944 0:d2a396fcd7dd 16 // Blinking rate in milliseconds
tossapon2944 0:d2a396fcd7dd 17 #define BLINKING_RATE_MS 500
tossapon2944 0:d2a396fcd7dd 18
tossapon2944 0:d2a396fcd7dd 19 ros::NodeHandle nh;
tossapon2944 0:d2a396fcd7dd 20
tossapon2944 0:d2a396fcd7dd 21 DigitalIn button01(PF_9);
tossapon2944 0:d2a396fcd7dd 22 DigitalIn button02(PF_7);
tossapon2944 0:d2a396fcd7dd 23 DigitalIn button03(PF_8);
tossapon2944 0:d2a396fcd7dd 24 DigitalIn button04(PE_3);
tossapon2944 0:d2a396fcd7dd 25 DigitalIn button05(PE_6);
tossapon2944 0:d2a396fcd7dd 26 DigitalIn button06(PE_5);
tossapon2944 0:d2a396fcd7dd 27 DigitalIn button07(PE_4);
tossapon2944 0:d2a396fcd7dd 28 DigitalIn button08(PE_2);
tossapon2944 0:d2a396fcd7dd 29 DigitalIn button09(PD_7);
tossapon2944 0:d2a396fcd7dd 30 DigitalIn button10(PG_2);
tossapon2944 0:d2a396fcd7dd 31 DigitalIn button11(PG_3);
tossapon2944 0:d2a396fcd7dd 32 DigitalIn button12(PD_2);
tossapon2944 0:d2a396fcd7dd 33 DigitalIn button13(PF_3);
tossapon2944 0:d2a396fcd7dd 34
tossapon2944 0:d2a396fcd7dd 35 DigitalOut out01(PF_15);
tossapon2944 0:d2a396fcd7dd 36 DigitalOut out02(PF_14);
tossapon2944 0:d2a396fcd7dd 37 DigitalOut out03(PF_13);
tossapon2944 0:d2a396fcd7dd 38 DigitalOut out04(PE_10);
tossapon2944 0:d2a396fcd7dd 39 DigitalOut out05(PB_2);
tossapon2944 0:d2a396fcd7dd 40 DigitalOut out06(PD_13);
tossapon2944 0:d2a396fcd7dd 41 DigitalOut out07(PD_12);
tossapon2944 0:d2a396fcd7dd 42 DigitalOut out08(PE_15);
tossapon2944 0:d2a396fcd7dd 43 DigitalOut out09(PE_7);
tossapon2944 0:d2a396fcd7dd 44 DigitalOut out10(PE_8);
tossapon2944 0:d2a396fcd7dd 45 DigitalOut out11(PB_15);
tossapon2944 0:d2a396fcd7dd 46 DigitalOut out12(PB_8);
tossapon2944 0:d2a396fcd7dd 47 DigitalOut out13(PB_9);
tossapon2944 0:d2a396fcd7dd 48 DigitalOut out14(PD_14);
tossapon2944 0:d2a396fcd7dd 49 DigitalOut out15(PB_3);
tossapon2944 0:d2a396fcd7dd 50 DigitalOut out16(PB_13);
tossapon2944 0:d2a396fcd7dd 51 DigitalOut out17(PB_4);
tossapon2944 0:d2a396fcd7dd 52 //DigitalOut out18(PF_4);
tossapon2944 0:d2a396fcd7dd 53 //DigitalOut out19(PF_3);
tossapon2944 0:d2a396fcd7dd 54
tossapon2944 0:d2a396fcd7dd 55 //PIN OF ULTRASONIC
tossapon2944 0:d2a396fcd7dd 56 DigitalOut trigPin(PC_6);
tossapon2944 0:d2a396fcd7dd 57 PwmIn echo01(PB_10);
tossapon2944 0:d2a396fcd7dd 58 PwmIn echo02(PE_12);
tossapon2944 0:d2a396fcd7dd 59 PwmIn echo03(PE_14);
tossapon2944 0:d2a396fcd7dd 60 PwmIn echo04(PD_15);
tossapon2944 0:d2a396fcd7dd 61 PwmIn echo05(PE_13);
tossapon2944 0:d2a396fcd7dd 62 PwmIn echo06(PE_11);
tossapon2944 0:d2a396fcd7dd 63
tossapon2944 0:d2a396fcd7dd 64 bool press =false;
tossapon2944 0:d2a396fcd7dd 65 bool msgs = false;
tossapon2944 0:d2a396fcd7dd 66
tossapon2944 0:d2a396fcd7dd 67 float current_time;
tossapon2944 0:d2a396fcd7dd 68 float previous_time;
tossapon2944 0:d2a396fcd7dd 69
tossapon2944 0:d2a396fcd7dd 70 int st_app = 99;
tossapon2944 0:d2a396fcd7dd 71 int st_alm;
tossapon2944 0:d2a396fcd7dd 72 int st_mov;
tossapon2944 0:d2a396fcd7dd 73 int st_bty;
tossapon2944 0:d2a396fcd7dd 74 int st_drp;
tossapon2944 0:d2a396fcd7dd 75 int st_brk;
tossapon2944 0:d2a396fcd7dd 76 int pre_button = 0;
tossapon2944 0:d2a396fcd7dd 77 //int led[10];
tossapon2944 0:d2a396fcd7dd 78
tossapon2944 0:d2a396fcd7dd 79 std_msgs::Int8 btnros;
tossapon2944 0:d2a396fcd7dd 80 std_msgs::Float32MultiArray distance1;
tossapon2944 0:d2a396fcd7dd 81
tossapon2944 0:d2a396fcd7dd 82 void st_callback(const std_msgs::Int16MultiArray& st_data)
tossapon2944 0:d2a396fcd7dd 83 {
tossapon2944 0:d2a396fcd7dd 84 st_app = st_data.data[0];
tossapon2944 0:d2a396fcd7dd 85 st_alm = st_data.data[1];
tossapon2944 0:d2a396fcd7dd 86 st_mov = st_data.data[2];
tossapon2944 0:d2a396fcd7dd 87 st_bty = st_data.data[3];
tossapon2944 0:d2a396fcd7dd 88 st_brk = st_data.data[4];
tossapon2944 0:d2a396fcd7dd 89
tossapon2944 0:d2a396fcd7dd 90 //nh.loginfo("st_msg received ");
tossapon2944 0:d2a396fcd7dd 91 }
tossapon2944 0:d2a396fcd7dd 92
tossapon2944 0:d2a396fcd7dd 93 ros::Subscriber<std_msgs::Int16MultiArray> st_sub("sub_state",&st_callback);
tossapon2944 0:d2a396fcd7dd 94 ros::Publisher btn_pub("/pub_button",&btnros);
tossapon2944 0:d2a396fcd7dd 95 ros::Publisher ultra_pub("/ultra_raw",&distance1);
tossapon2944 0:d2a396fcd7dd 96
tossapon2944 0:d2a396fcd7dd 97 Timer t_main;
tossapon2944 0:d2a396fcd7dd 98 Timer t_pub;
tossapon2944 0:d2a396fcd7dd 99 Timer t_press;
tossapon2944 0:d2a396fcd7dd 100 Timer t_botton;
tossapon2944 0:d2a396fcd7dd 101 int alarm =0;
tossapon2944 0:d2a396fcd7dd 102
tossapon2944 0:d2a396fcd7dd 103 void onbutton()
tossapon2944 0:d2a396fcd7dd 104 {
tossapon2944 0:d2a396fcd7dd 105 if(!button01)
tossapon2944 0:d2a396fcd7dd 106 {
tossapon2944 0:d2a396fcd7dd 107 btnros.data = 1;
tossapon2944 0:d2a396fcd7dd 108 alarm = 1;
tossapon2944 0:d2a396fcd7dd 109 press = true;
tossapon2944 0:d2a396fcd7dd 110 }
tossapon2944 0:d2a396fcd7dd 111 else if(!button02)
tossapon2944 0:d2a396fcd7dd 112 {
tossapon2944 0:d2a396fcd7dd 113 btnros.data =2;
tossapon2944 0:d2a396fcd7dd 114 alarm = 2;
tossapon2944 0:d2a396fcd7dd 115 press = true;
tossapon2944 0:d2a396fcd7dd 116 }
tossapon2944 0:d2a396fcd7dd 117 else if(!button03)
tossapon2944 0:d2a396fcd7dd 118 {
tossapon2944 0:d2a396fcd7dd 119 btnros.data =3;
tossapon2944 0:d2a396fcd7dd 120 alarm = 3;
tossapon2944 0:d2a396fcd7dd 121 press = true;
tossapon2944 0:d2a396fcd7dd 122 }
tossapon2944 0:d2a396fcd7dd 123 else if(!button04)
tossapon2944 0:d2a396fcd7dd 124 {
tossapon2944 0:d2a396fcd7dd 125 btnros.data =4;
tossapon2944 0:d2a396fcd7dd 126 alarm = 4;
tossapon2944 0:d2a396fcd7dd 127 press = true;
tossapon2944 0:d2a396fcd7dd 128 }
tossapon2944 0:d2a396fcd7dd 129 else if(!button05)
tossapon2944 0:d2a396fcd7dd 130 {
tossapon2944 0:d2a396fcd7dd 131 btnros.data =5;
tossapon2944 0:d2a396fcd7dd 132 alarm = 5;
tossapon2944 0:d2a396fcd7dd 133 press = true;
tossapon2944 0:d2a396fcd7dd 134 }
tossapon2944 0:d2a396fcd7dd 135 else if(!button06)
tossapon2944 0:d2a396fcd7dd 136 {
tossapon2944 0:d2a396fcd7dd 137 btnros.data =6;
tossapon2944 0:d2a396fcd7dd 138 alarm = 6;
tossapon2944 0:d2a396fcd7dd 139 press = true;
tossapon2944 0:d2a396fcd7dd 140 }
tossapon2944 0:d2a396fcd7dd 141 else if(!button07)
tossapon2944 0:d2a396fcd7dd 142 {
tossapon2944 0:d2a396fcd7dd 143 btnros.data =7;
tossapon2944 0:d2a396fcd7dd 144 alarm = 7;
tossapon2944 0:d2a396fcd7dd 145 press = true;
tossapon2944 0:d2a396fcd7dd 146 }
tossapon2944 0:d2a396fcd7dd 147 else if(!button08)
tossapon2944 0:d2a396fcd7dd 148 {
tossapon2944 0:d2a396fcd7dd 149 btnros.data =8;
tossapon2944 0:d2a396fcd7dd 150 alarm = 8;
tossapon2944 0:d2a396fcd7dd 151 press = true;
tossapon2944 0:d2a396fcd7dd 152 }
tossapon2944 0:d2a396fcd7dd 153 else if(!button09)
tossapon2944 0:d2a396fcd7dd 154 {
tossapon2944 0:d2a396fcd7dd 155 btnros.data =9;
tossapon2944 0:d2a396fcd7dd 156 alarm = 9;
tossapon2944 0:d2a396fcd7dd 157 press = true;
tossapon2944 0:d2a396fcd7dd 158 }
tossapon2944 0:d2a396fcd7dd 159 else if(!button10)
tossapon2944 0:d2a396fcd7dd 160 {
tossapon2944 0:d2a396fcd7dd 161 btnros.data =10;
tossapon2944 0:d2a396fcd7dd 162 alarm = 10;
tossapon2944 0:d2a396fcd7dd 163 press = true;
tossapon2944 0:d2a396fcd7dd 164 }
tossapon2944 0:d2a396fcd7dd 165 else if(!button11)
tossapon2944 0:d2a396fcd7dd 166 {
tossapon2944 0:d2a396fcd7dd 167 btnros.data=11;
tossapon2944 0:d2a396fcd7dd 168 alarm = 11;
tossapon2944 0:d2a396fcd7dd 169 press = true;
tossapon2944 0:d2a396fcd7dd 170 }
tossapon2944 0:d2a396fcd7dd 171 else if(!button12)
tossapon2944 0:d2a396fcd7dd 172 {
tossapon2944 0:d2a396fcd7dd 173 btnros.data=12;// reset alam
tossapon2944 0:d2a396fcd7dd 174 alarm = 12;
tossapon2944 0:d2a396fcd7dd 175 press = true;
tossapon2944 0:d2a396fcd7dd 176
tossapon2944 0:d2a396fcd7dd 177 }
tossapon2944 0:d2a396fcd7dd 178 else if(!button13)
tossapon2944 0:d2a396fcd7dd 179 {
tossapon2944 0:d2a396fcd7dd 180 btnros.data=13;// reset alam
tossapon2944 0:d2a396fcd7dd 181 press = true;
tossapon2944 0:d2a396fcd7dd 182 }
tossapon2944 0:d2a396fcd7dd 183
tossapon2944 0:d2a396fcd7dd 184 //nh.loginfo("in press btn");
tossapon2944 0:d2a396fcd7dd 185 pre_button = btnros.data;
tossapon2944 0:d2a396fcd7dd 186
tossapon2944 0:d2a396fcd7dd 187 }
tossapon2944 0:d2a396fcd7dd 188
tossapon2944 0:d2a396fcd7dd 189 void zero()
tossapon2944 0:d2a396fcd7dd 190 {
tossapon2944 0:d2a396fcd7dd 191 out01 = 1; //station 1
tossapon2944 0:d2a396fcd7dd 192 out02 = 1; //station 2
tossapon2944 0:d2a396fcd7dd 193 out03 = 1; //station 3
tossapon2944 0:d2a396fcd7dd 194 out04 = 1; //station 4
tossapon2944 0:d2a396fcd7dd 195 out05 = 1; //station 5
tossapon2944 0:d2a396fcd7dd 196 out06 = 1; //station 6
tossapon2944 0:d2a396fcd7dd 197 out07 = 1; //station 7
tossapon2944 0:d2a396fcd7dd 198 out08 = 1; //station 8
tossapon2944 0:d2a396fcd7dd 199 out09 = 1; //station home
tossapon2944 0:d2a396fcd7dd 200 out10 = 1; //station change
tossapon2944 0:d2a396fcd7dd 201 //out11 = 1; //station drop point
tossapon2944 0:d2a396fcd7dd 202 out16 = 1; //station drop point
tossapon2944 0:d2a396fcd7dd 203 }
tossapon2944 0:d2a396fcd7dd 204
tossapon2944 0:d2a396fcd7dd 205 void nucleo_to_PLC()
tossapon2944 0:d2a396fcd7dd 206 {
tossapon2944 0:d2a396fcd7dd 207 if(st_app == 99) //zero
tossapon2944 0:d2a396fcd7dd 208 {
tossapon2944 0:d2a396fcd7dd 209 zero();
tossapon2944 0:d2a396fcd7dd 210 }
tossapon2944 0:d2a396fcd7dd 211 else if(st_app == 0) //home
tossapon2944 0:d2a396fcd7dd 212 {
tossapon2944 0:d2a396fcd7dd 213 zero();
tossapon2944 0:d2a396fcd7dd 214 out09 = 0;
tossapon2944 0:d2a396fcd7dd 215 }
tossapon2944 0:d2a396fcd7dd 216 else if(st_app == 1) //station 1
tossapon2944 0:d2a396fcd7dd 217 {
tossapon2944 0:d2a396fcd7dd 218 zero();
tossapon2944 0:d2a396fcd7dd 219 out01 = 0;
tossapon2944 0:d2a396fcd7dd 220 }
tossapon2944 0:d2a396fcd7dd 221 else if(st_app == 2) //station 2
tossapon2944 0:d2a396fcd7dd 222 {
tossapon2944 0:d2a396fcd7dd 223 zero();
tossapon2944 0:d2a396fcd7dd 224 out02 = 0;
tossapon2944 0:d2a396fcd7dd 225 }
tossapon2944 0:d2a396fcd7dd 226 else if(st_app == 3) //station 3
tossapon2944 0:d2a396fcd7dd 227 {
tossapon2944 0:d2a396fcd7dd 228 zero();
tossapon2944 0:d2a396fcd7dd 229 out03 = 0;
tossapon2944 0:d2a396fcd7dd 230 }
tossapon2944 0:d2a396fcd7dd 231 else if(st_app == 4) //station 4
tossapon2944 0:d2a396fcd7dd 232 {
tossapon2944 0:d2a396fcd7dd 233 zero();
tossapon2944 0:d2a396fcd7dd 234 out04 = 0;
tossapon2944 0:d2a396fcd7dd 235 }
tossapon2944 0:d2a396fcd7dd 236 else if(st_app == 5) //station 5
tossapon2944 0:d2a396fcd7dd 237 {
tossapon2944 0:d2a396fcd7dd 238 zero();
tossapon2944 0:d2a396fcd7dd 239 out05 = 0;
tossapon2944 0:d2a396fcd7dd 240 }
tossapon2944 0:d2a396fcd7dd 241 else if(st_app == 6) //station 6
tossapon2944 0:d2a396fcd7dd 242 {
tossapon2944 0:d2a396fcd7dd 243 zero();
tossapon2944 0:d2a396fcd7dd 244 out06 = 0;
tossapon2944 0:d2a396fcd7dd 245 }
tossapon2944 0:d2a396fcd7dd 246 else if(st_app == 7) //station 7
tossapon2944 0:d2a396fcd7dd 247 {
tossapon2944 0:d2a396fcd7dd 248 zero();
tossapon2944 0:d2a396fcd7dd 249 out07 = 0;
tossapon2944 0:d2a396fcd7dd 250 }
tossapon2944 0:d2a396fcd7dd 251 else if(st_app == 8) //station 8
tossapon2944 0:d2a396fcd7dd 252 {
tossapon2944 0:d2a396fcd7dd 253 zero();
tossapon2944 0:d2a396fcd7dd 254 out08 = 0;
tossapon2944 0:d2a396fcd7dd 255 }
tossapon2944 0:d2a396fcd7dd 256 else if(st_app == 9) //change station
tossapon2944 0:d2a396fcd7dd 257 {
tossapon2944 0:d2a396fcd7dd 258 zero();
tossapon2944 0:d2a396fcd7dd 259 out10 = 0;
tossapon2944 0:d2a396fcd7dd 260 }
tossapon2944 0:d2a396fcd7dd 261 else if(st_app == 10) //drop point
tossapon2944 0:d2a396fcd7dd 262 {
tossapon2944 0:d2a396fcd7dd 263 zero();
tossapon2944 0:d2a396fcd7dd 264 out16 = 0;
tossapon2944 0:d2a396fcd7dd 265 }
tossapon2944 0:d2a396fcd7dd 266 /////////////////////////////////
tossapon2944 0:d2a396fcd7dd 267 if(st_alm == 0 && alarm == 12)
tossapon2944 0:d2a396fcd7dd 268 {
tossapon2944 0:d2a396fcd7dd 269 out11 = 1;
tossapon2944 0:d2a396fcd7dd 270 out14 = 1;
tossapon2944 0:d2a396fcd7dd 271 }
tossapon2944 0:d2a396fcd7dd 272 else if(st_alm == 0 || st_alm == 3 && alarm != 12)
tossapon2944 0:d2a396fcd7dd 273 {
tossapon2944 0:d2a396fcd7dd 274 out11 = 0; //robot ready
tossapon2944 0:d2a396fcd7dd 275 out14 = 1;
tossapon2944 0:d2a396fcd7dd 276 }
tossapon2944 0:d2a396fcd7dd 277 else if(st_alm == 99)
tossapon2944 0:d2a396fcd7dd 278 {
tossapon2944 0:d2a396fcd7dd 279 out11= 1;
tossapon2944 0:d2a396fcd7dd 280 out14 = 0; //robot alarm
tossapon2944 0:d2a396fcd7dd 281 }
tossapon2944 0:d2a396fcd7dd 282 //////////////////////////////////
tossapon2944 0:d2a396fcd7dd 283 if(st_mov == 0)
tossapon2944 0:d2a396fcd7dd 284 {
tossapon2944 0:d2a396fcd7dd 285 out12 = 1;
tossapon2944 0:d2a396fcd7dd 286 out13 = 1;
tossapon2944 0:d2a396fcd7dd 287 }
tossapon2944 0:d2a396fcd7dd 288 else if(st_mov == 1)
tossapon2944 0:d2a396fcd7dd 289 {
tossapon2944 0:d2a396fcd7dd 290 out12 = 0; //robot forword
tossapon2944 0:d2a396fcd7dd 291 out13 = 1;
tossapon2944 0:d2a396fcd7dd 292 }
tossapon2944 0:d2a396fcd7dd 293 else if(st_mov == 2)
tossapon2944 0:d2a396fcd7dd 294 {
tossapon2944 0:d2a396fcd7dd 295 out12 = 1;
tossapon2944 0:d2a396fcd7dd 296 out13 = 0; //robot backword
tossapon2944 0:d2a396fcd7dd 297 }
tossapon2944 0:d2a396fcd7dd 298 //////////////////////////////////
tossapon2944 0:d2a396fcd7dd 299 if(st_bty == 0)
tossapon2944 0:d2a396fcd7dd 300 {
tossapon2944 0:d2a396fcd7dd 301 out15 = 1; //battery status
tossapon2944 0:d2a396fcd7dd 302 }
tossapon2944 0:d2a396fcd7dd 303 else if(st_bty == 1)
tossapon2944 0:d2a396fcd7dd 304 {
tossapon2944 0:d2a396fcd7dd 305 out15 = 0; //battery status (low)
tossapon2944 0:d2a396fcd7dd 306 }
tossapon2944 0:d2a396fcd7dd 307 else if(st_bty == 2)
tossapon2944 0:d2a396fcd7dd 308 {
tossapon2944 0:d2a396fcd7dd 309 out15 = 1; //battery status (full)
tossapon2944 0:d2a396fcd7dd 310 }
tossapon2944 0:d2a396fcd7dd 311 //////////////////////////////////
tossapon2944 0:d2a396fcd7dd 312 if(st_brk == 0)
tossapon2944 0:d2a396fcd7dd 313 {
tossapon2944 0:d2a396fcd7dd 314 out17 = 1; //normal break
tossapon2944 0:d2a396fcd7dd 315 //out18 = 1;
tossapon2944 0:d2a396fcd7dd 316 }
tossapon2944 0:d2a396fcd7dd 317 else if(st_brk == 1)
tossapon2944 0:d2a396fcd7dd 318 {
tossapon2944 0:d2a396fcd7dd 319 out17 = 1; //not break
tossapon2944 0:d2a396fcd7dd 320 }
tossapon2944 0:d2a396fcd7dd 321 else if(st_brk == 3)
tossapon2944 0:d2a396fcd7dd 322 {
tossapon2944 0:d2a396fcd7dd 323 out17 = 0; //break
tossapon2944 0:d2a396fcd7dd 324 }
tossapon2944 0:d2a396fcd7dd 325 }
tossapon2944 0:d2a396fcd7dd 326
tossapon2944 0:d2a396fcd7dd 327 void ultrasonic()
tossapon2944 0:d2a396fcd7dd 328 {
tossapon2944 0:d2a396fcd7dd 329 float duration[6];
tossapon2944 0:d2a396fcd7dd 330 float points[6];
tossapon2944 0:d2a396fcd7dd 331 trigPin = false; // trig PIN
tossapon2944 0:d2a396fcd7dd 332 wait_us(2);
tossapon2944 0:d2a396fcd7dd 333 trigPin = true;
tossapon2944 0:d2a396fcd7dd 334 wait_us(10);
tossapon2944 0:d2a396fcd7dd 335 trigPin = false;
tossapon2944 0:d2a396fcd7dd 336
tossapon2944 0:d2a396fcd7dd 337 duration[0] = echo01.pulsewidth();
tossapon2944 0:d2a396fcd7dd 338 //duration[1] = echo02.pulsewidth();
tossapon2944 0:d2a396fcd7dd 339 //duration[2] = echo03.pulsewidth();
tossapon2944 0:d2a396fcd7dd 340 duration[3] = echo04.pulsewidth();
tossapon2944 0:d2a396fcd7dd 341 //duration[4] = echo05.pulsewidth();
tossapon2944 0:d2a396fcd7dd 342 duration[5] = echo06.pulsewidth();
tossapon2944 0:d2a396fcd7dd 343
tossapon2944 0:d2a396fcd7dd 344 points[0] = duration[0]/ 0.00560f;
tossapon2944 0:d2a396fcd7dd 345 //points[1] = duration[1]/ 0.00560f;
tossapon2944 0:d2a396fcd7dd 346 //points[2] = duration[2]/ 0.00560f;
tossapon2944 0:d2a396fcd7dd 347 points[3] = duration[3]/ 0.00560f;
tossapon2944 0:d2a396fcd7dd 348 //points[4] = duration[4]/ 0.00560f;
tossapon2944 0:d2a396fcd7dd 349 points[5] = duration[5]/ 0.00560f;
tossapon2944 0:d2a396fcd7dd 350
tossapon2944 0:d2a396fcd7dd 351 distance1.data[0]= points[0];
tossapon2944 0:d2a396fcd7dd 352 //distance1.data[1]= points[1];
tossapon2944 0:d2a396fcd7dd 353 //distance1.data[2]= points[2];
tossapon2944 0:d2a396fcd7dd 354 distance1.data[1]= points[3];
tossapon2944 0:d2a396fcd7dd 355 //distance1.data[4]= points[4];
tossapon2944 0:d2a396fcd7dd 356 distance1.data[2]= points[5];
tossapon2944 0:d2a396fcd7dd 357 }
tossapon2944 0:d2a396fcd7dd 358
tossapon2944 0:d2a396fcd7dd 359 //void st_robot()
tossapon2944 0:d2a396fcd7dd 360 //{
tossapon2944 0:d2a396fcd7dd 361 //}
tossapon2944 0:d2a396fcd7dd 362
tossapon2944 0:d2a396fcd7dd 363 int main()
tossapon2944 0:d2a396fcd7dd 364 {
tossapon2944 0:d2a396fcd7dd 365 // Initialise the digital pin LED1 as an output
tossapon2944 0:d2a396fcd7dd 366 nh.initNode();
tossapon2944 0:d2a396fcd7dd 367 //nh.subscribe(btn_sub);
tossapon2944 0:d2a396fcd7dd 368 nh.subscribe(st_sub);
tossapon2944 0:d2a396fcd7dd 369 nh.advertise(btn_pub);
tossapon2944 0:d2a396fcd7dd 370 nh.advertise(ultra_pub);
tossapon2944 0:d2a396fcd7dd 371 nh.getHardware()->setBaud(57600);
tossapon2944 0:d2a396fcd7dd 372
tossapon2944 0:d2a396fcd7dd 373 //int current_data = 0;
tossapon2944 0:d2a396fcd7dd 374 //int last_data = 0;
tossapon2944 0:d2a396fcd7dd 375
tossapon2944 0:d2a396fcd7dd 376 button01.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 377 button02.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 378 button03.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 379 button04.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 380 button05.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 381 button06.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 382 button07.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 383 button08.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 384 button09.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 385 button10.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 386 button11.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 387 button12.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 388 button13.mode(PullUp);
tossapon2944 0:d2a396fcd7dd 389
tossapon2944 0:d2a396fcd7dd 390 out01 = 1;
tossapon2944 0:d2a396fcd7dd 391 out02 = 1;
tossapon2944 0:d2a396fcd7dd 392 out03 = 1;
tossapon2944 0:d2a396fcd7dd 393 out04 = 1;
tossapon2944 0:d2a396fcd7dd 394 out05 = 1;
tossapon2944 0:d2a396fcd7dd 395 out06 = 1;
tossapon2944 0:d2a396fcd7dd 396 out07 = 1;
tossapon2944 0:d2a396fcd7dd 397 out08 = 1;
tossapon2944 0:d2a396fcd7dd 398 out09 = 1;
tossapon2944 0:d2a396fcd7dd 399 out10 = 1;
tossapon2944 0:d2a396fcd7dd 400 out11 = 1;
tossapon2944 0:d2a396fcd7dd 401 out12 = 1;
tossapon2944 0:d2a396fcd7dd 402 out13 = 1;
tossapon2944 0:d2a396fcd7dd 403 out14 = 1;
tossapon2944 0:d2a396fcd7dd 404 out15 = 1;
tossapon2944 0:d2a396fcd7dd 405 out16 = 1;
tossapon2944 0:d2a396fcd7dd 406 //out17=0;
tossapon2944 0:d2a396fcd7dd 407 //out18=0;
tossapon2944 0:d2a396fcd7dd 408 //out19=0;
tossapon2944 0:d2a396fcd7dd 409 distance1.layout.dim = (std_msgs::MultiArrayDimension*)
tossapon2944 0:d2a396fcd7dd 410 malloc(sizeof(std_msgs::MultiArrayDimension)*2);
tossapon2944 0:d2a396fcd7dd 411 distance1.layout.dim[0].label = "height1";
tossapon2944 0:d2a396fcd7dd 412 distance1.layout.dim[0].size =4;
tossapon2944 0:d2a396fcd7dd 413 distance1.layout.dim[0].stride = 1;
tossapon2944 0:d2a396fcd7dd 414 distance1.layout.data_offset = 0;
tossapon2944 0:d2a396fcd7dd 415 distance1.data = (float *)malloc(sizeof(float)*8);
tossapon2944 0:d2a396fcd7dd 416 distance1.data_length = 3;
tossapon2944 0:d2a396fcd7dd 417
tossapon2944 0:d2a396fcd7dd 418 t_main.start();
tossapon2944 0:d2a396fcd7dd 419 t_pub.start();
tossapon2944 0:d2a396fcd7dd 420 t_press.start();
tossapon2944 0:d2a396fcd7dd 421
tossapon2944 0:d2a396fcd7dd 422 while (true)
tossapon2944 0:d2a396fcd7dd 423 {
tossapon2944 0:d2a396fcd7dd 424 if(t_main.read() >= 0.05f)
tossapon2944 0:d2a396fcd7dd 425 {
tossapon2944 0:d2a396fcd7dd 426 onbutton();
tossapon2944 0:d2a396fcd7dd 427 ultrasonic();
tossapon2944 0:d2a396fcd7dd 428 //btn_pub.publish(&btnros);
tossapon2944 0:d2a396fcd7dd 429 ultra_pub.publish(&distance1);
tossapon2944 0:d2a396fcd7dd 430 nucleo_to_PLC();
tossapon2944 0:d2a396fcd7dd 431 t_main.reset();
tossapon2944 0:d2a396fcd7dd 432 }
tossapon2944 0:d2a396fcd7dd 433
tossapon2944 0:d2a396fcd7dd 434 if(press)
tossapon2944 0:d2a396fcd7dd 435 {
tossapon2944 0:d2a396fcd7dd 436 if(t_pub.read() >= 0.5f){
tossapon2944 0:d2a396fcd7dd 437 btn_pub.publish(&btnros);
tossapon2944 0:d2a396fcd7dd 438 press = false;
tossapon2944 0:d2a396fcd7dd 439 t_pub.reset();
tossapon2944 0:d2a396fcd7dd 440 }
tossapon2944 0:d2a396fcd7dd 441 }
tossapon2944 0:d2a396fcd7dd 442
tossapon2944 0:d2a396fcd7dd 443 //nucleo_to_PLC();
tossapon2944 0:d2a396fcd7dd 444 nh.spinOnce();
tossapon2944 0:d2a396fcd7dd 445 }
tossapon2944 0:d2a396fcd7dd 446
tossapon2944 0:d2a396fcd7dd 447 }