mitsui
Dependencies: PwmIn mbed ros_lib_melodic
main.cpp
- Committer:
- tossapon2944
- Date:
- 2021-11-01
- Revision:
- 0:d2a396fcd7dd
File content as of revision 0:d2a396fcd7dd:
#include "mbed.h" #include <ros.h> #include <ros/time.h> #include "PwmIn.h" #include <std_msgs/Float32.h> #include <geometry_msgs/Point.h> #include <std_msgs/String.h> #include <geometry_msgs/Vector3.h> #include <std_msgs/Float32MultiArray.h> #include <std_msgs/Int32MultiArray.h> #include <std_msgs/Int16MultiArray.h> #include <std_msgs/Int8.h> #include <geometry_msgs/Twist.h> #include <actionlib_msgs/GoalStatusArray.h> // Blinking rate in milliseconds #define BLINKING_RATE_MS 500 ros::NodeHandle nh; DigitalIn button01(PF_9); DigitalIn button02(PF_7); DigitalIn button03(PF_8); DigitalIn button04(PE_3); DigitalIn button05(PE_6); DigitalIn button06(PE_5); DigitalIn button07(PE_4); DigitalIn button08(PE_2); DigitalIn button09(PD_7); DigitalIn button10(PG_2); DigitalIn button11(PG_3); DigitalIn button12(PD_2); DigitalIn button13(PF_3); DigitalOut out01(PF_15); DigitalOut out02(PF_14); DigitalOut out03(PF_13); DigitalOut out04(PE_10); DigitalOut out05(PB_2); DigitalOut out06(PD_13); DigitalOut out07(PD_12); DigitalOut out08(PE_15); DigitalOut out09(PE_7); DigitalOut out10(PE_8); DigitalOut out11(PB_15); DigitalOut out12(PB_8); DigitalOut out13(PB_9); DigitalOut out14(PD_14); DigitalOut out15(PB_3); DigitalOut out16(PB_13); DigitalOut out17(PB_4); //DigitalOut out18(PF_4); //DigitalOut out19(PF_3); //PIN OF ULTRASONIC DigitalOut trigPin(PC_6); PwmIn echo01(PB_10); PwmIn echo02(PE_12); PwmIn echo03(PE_14); PwmIn echo04(PD_15); PwmIn echo05(PE_13); PwmIn echo06(PE_11); bool press =false; bool msgs = false; float current_time; float previous_time; int st_app = 99; int st_alm; int st_mov; int st_bty; int st_drp; int st_brk; int pre_button = 0; //int led[10]; std_msgs::Int8 btnros; std_msgs::Float32MultiArray distance1; void st_callback(const std_msgs::Int16MultiArray& st_data) { st_app = st_data.data[0]; st_alm = st_data.data[1]; st_mov = st_data.data[2]; st_bty = st_data.data[3]; st_brk = st_data.data[4]; //nh.loginfo("st_msg received "); } ros::Subscriber<std_msgs::Int16MultiArray> st_sub("sub_state",&st_callback); ros::Publisher btn_pub("/pub_button",&btnros); ros::Publisher ultra_pub("/ultra_raw",&distance1); Timer t_main; Timer t_pub; Timer t_press; Timer t_botton; int alarm =0; void onbutton() { if(!button01) { btnros.data = 1; alarm = 1; press = true; } else if(!button02) { btnros.data =2; alarm = 2; press = true; } else if(!button03) { btnros.data =3; alarm = 3; press = true; } else if(!button04) { btnros.data =4; alarm = 4; press = true; } else if(!button05) { btnros.data =5; alarm = 5; press = true; } else if(!button06) { btnros.data =6; alarm = 6; press = true; } else if(!button07) { btnros.data =7; alarm = 7; press = true; } else if(!button08) { btnros.data =8; alarm = 8; press = true; } else if(!button09) { btnros.data =9; alarm = 9; press = true; } else if(!button10) { btnros.data =10; alarm = 10; press = true; } else if(!button11) { btnros.data=11; alarm = 11; press = true; } else if(!button12) { btnros.data=12;// reset alam alarm = 12; press = true; } else if(!button13) { btnros.data=13;// reset alam press = true; } //nh.loginfo("in press btn"); pre_button = btnros.data; } void zero() { out01 = 1; //station 1 out02 = 1; //station 2 out03 = 1; //station 3 out04 = 1; //station 4 out05 = 1; //station 5 out06 = 1; //station 6 out07 = 1; //station 7 out08 = 1; //station 8 out09 = 1; //station home out10 = 1; //station change //out11 = 1; //station drop point out16 = 1; //station drop point } void nucleo_to_PLC() { if(st_app == 99) //zero { zero(); } else if(st_app == 0) //home { zero(); out09 = 0; } else if(st_app == 1) //station 1 { zero(); out01 = 0; } else if(st_app == 2) //station 2 { zero(); out02 = 0; } else if(st_app == 3) //station 3 { zero(); out03 = 0; } else if(st_app == 4) //station 4 { zero(); out04 = 0; } else if(st_app == 5) //station 5 { zero(); out05 = 0; } else if(st_app == 6) //station 6 { zero(); out06 = 0; } else if(st_app == 7) //station 7 { zero(); out07 = 0; } else if(st_app == 8) //station 8 { zero(); out08 = 0; } else if(st_app == 9) //change station { zero(); out10 = 0; } else if(st_app == 10) //drop point { zero(); out16 = 0; } ///////////////////////////////// if(st_alm == 0 && alarm == 12) { out11 = 1; out14 = 1; } else if(st_alm == 0 || st_alm == 3 && alarm != 12) { out11 = 0; //robot ready out14 = 1; } else if(st_alm == 99) { out11= 1; out14 = 0; //robot alarm } ////////////////////////////////// if(st_mov == 0) { out12 = 1; out13 = 1; } else if(st_mov == 1) { out12 = 0; //robot forword out13 = 1; } else if(st_mov == 2) { out12 = 1; out13 = 0; //robot backword } ////////////////////////////////// if(st_bty == 0) { out15 = 1; //battery status } else if(st_bty == 1) { out15 = 0; //battery status (low) } else if(st_bty == 2) { out15 = 1; //battery status (full) } ////////////////////////////////// if(st_brk == 0) { out17 = 1; //normal break //out18 = 1; } else if(st_brk == 1) { out17 = 1; //not break } else if(st_brk == 3) { out17 = 0; //break } } void ultrasonic() { float duration[6]; float points[6]; trigPin = false; // trig PIN wait_us(2); trigPin = true; wait_us(10); trigPin = false; duration[0] = echo01.pulsewidth(); //duration[1] = echo02.pulsewidth(); //duration[2] = echo03.pulsewidth(); duration[3] = echo04.pulsewidth(); //duration[4] = echo05.pulsewidth(); duration[5] = echo06.pulsewidth(); points[0] = duration[0]/ 0.00560f; //points[1] = duration[1]/ 0.00560f; //points[2] = duration[2]/ 0.00560f; points[3] = duration[3]/ 0.00560f; //points[4] = duration[4]/ 0.00560f; points[5] = duration[5]/ 0.00560f; distance1.data[0]= points[0]; //distance1.data[1]= points[1]; //distance1.data[2]= points[2]; distance1.data[1]= points[3]; //distance1.data[4]= points[4]; distance1.data[2]= points[5]; } //void st_robot() //{ //} int main() { // Initialise the digital pin LED1 as an output nh.initNode(); //nh.subscribe(btn_sub); nh.subscribe(st_sub); nh.advertise(btn_pub); nh.advertise(ultra_pub); nh.getHardware()->setBaud(57600); //int current_data = 0; //int last_data = 0; button01.mode(PullUp); button02.mode(PullUp); button03.mode(PullUp); button04.mode(PullUp); button05.mode(PullUp); button06.mode(PullUp); button07.mode(PullUp); button08.mode(PullUp); button09.mode(PullUp); button10.mode(PullUp); button11.mode(PullUp); button12.mode(PullUp); button13.mode(PullUp); out01 = 1; out02 = 1; out03 = 1; out04 = 1; out05 = 1; out06 = 1; out07 = 1; out08 = 1; out09 = 1; out10 = 1; out11 = 1; out12 = 1; out13 = 1; out14 = 1; out15 = 1; out16 = 1; //out17=0; //out18=0; //out19=0; distance1.layout.dim = (std_msgs::MultiArrayDimension*) malloc(sizeof(std_msgs::MultiArrayDimension)*2); distance1.layout.dim[0].label = "height1"; distance1.layout.dim[0].size =4; distance1.layout.dim[0].stride = 1; distance1.layout.data_offset = 0; distance1.data = (float *)malloc(sizeof(float)*8); distance1.data_length = 3; t_main.start(); t_pub.start(); t_press.start(); while (true) { if(t_main.read() >= 0.05f) { onbutton(); ultrasonic(); //btn_pub.publish(&btnros); ultra_pub.publish(&distance1); nucleo_to_PLC(); t_main.reset(); } if(press) { if(t_pub.read() >= 0.5f){ btn_pub.publish(&btnros); press = false; t_pub.reset(); } } //nucleo_to_PLC(); nh.spinOnce(); } }