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();
    }
    
}