File content as of revision 7:13dd93a0efe8:

#include "mbed.h"
#include "motor.h"
#include <ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32.h>

std_msgs::Float64 cum_dist;
std_msgs::Float32 rela_dist;
std_msgs::Int32 curr_rpm;


ros::NodeHandle nh ;
ros::Publisher cum_dist_pub("cum_dist", &cum_dist);
ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
ros::Publisher rpm_pub("rpm", &curr_rpm);

MotorCtl motor1(D3,D12,D4,D5);
InterruptIn tachoINT1(D4);
InterruptIn tachoINT2(D5);
Serial pc(USBTX,USBRX,115200);

char rx_buffer[10];
int index=0;
volatile int flag;
void update_current(void)
{
    motor1.UpdateCurrentPosition();
    //  pc.printf("Update Position\r\n");
}
void rx_cb(void)
{
    char ch;
    ch = pc.getc();
    pc.putc(ch);
    rx_buffer[index++]=ch;
    if(ch==0x0D) {
        pc.putc(0x0A);
        rx_buffer[--index]='\0';
        index=0;
        flag=1;
    }
}

void set_speed(void)
{
    int speed;
    speed = atoi((const char*)rx_buffer);
    if(speed>78) {
        speed=78;
    }
    if(speed<-78) {
        speed=-78;
    }
    motor1.setTarget(speed);
    //  motor1.setTarget(60);
    pc.printf(" Set speed = %d\r\n", speed);

}

int main()
{
    nh.initNode();
    nh.advertise(cum_dist_pub);
    nh.advertise(rela_dist_pub);
    nh.advertise(rpm_pub);
    
    tachoINT1.fall(&update_current);
    tachoINT1.rise(&update_current);
    tachoINT2.fall(&update_current);
    tachoINT2.rise(&update_current);
    pc.attach(callback(rx_cb));
    int rpm;
    float reladistance; //m단위
    float cumdistance;

    while(1) {
        flag=0;

        pc.printf("Enter the value for speed [-78,78]\r\n");
        while(flag!=1) {
            rpm = motor1.getRPM();
            cumdistance = motor1.CalculateCumDis(); //  누적거리
            reladistance = motor1.CalculateRelaDis(); // 상대거리
    
            cum_dist.data = cumdistance;
            cum_dist_pub.publish(&cum_dist);
            
            rela_dist.data = reladistance;
            rela_dist_pub.publish(&rela_dist);
            
            curr_rpm.data = rpm;
            rpm_pub.publish(&curr_rpm);            
            
            printf("Rpm: %f\r\n",rpm);
            printf("cumdistance: %f\r\n",cumdistance);
            printf("reladistance: %f\r\n",reladistance);
            wait(1);
        }
//        motor1.setTarget(60);
        set_speed();
    }

}