![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mitsui
Dependencies: PwmIn mbed ros_lib_melodic
Revision 0:d2a396fcd7dd, committed 2021-11-01
- Comitter:
- tossapon2944
- Date:
- Mon Nov 01 10:48:47 2021 +0000
- Commit message:
- dd
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PwmIn.lib Mon Nov 01 10:48:47 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/PwmIn/#6d68eb9b6bbb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Nov 01 10:48:47 2021 +0000 @@ -0,0 +1,447 @@ +#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(); + } + +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Nov 01 10:48:47 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
Binary file resources/official_armmbed_example_badge.png has changed
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_melodic.lib Mon Nov 01 10:48:47 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_melodic/#da82487f547e