toss tongbai
mitsui
Dependencies: PwmIn mbed ros_lib_melodic
Diff: main.cpp
- Revision:
- 0:d2a396fcd7dd
--- /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();
+ }
+
+}