File content as of revision 8:efe5a5b1f10a:

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

std_msgs::Float64 cum_dist;
std_msgs::Float64 rela_dist;
std_msgs::Float64 flaw_loca;
std_msgs::Int32 curr_rpm;
//std_msgs::String flaw_state;


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);
ros::Publisher flaw_loca_pub("flaw_loca", &flaw_loca);
//ros::Publisher flaw_state_pub("flaw_state", &flaw_state);

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);
    nh.advertise(flaw_loca_pub);
//    nh.advertise(flaw_state_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;
    float flaw_location = 0;
//    char flaw_curr_state[10] = "stable";
    
    motor1.setTarget(60);
    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);            
            
            flaw_loca.data = flaw_location;
            flaw_loca_pub.publish(&flaw_loca);
            
            flaw_location+= 0.05;
            if(flaw_location > 1){
                flaw_location = 0;
            } 
//            flaw_state.data = flaw_curr_state;
//            flaw_state_pub.publish(&flaw_state);            
            
//            printf("Rpm: %f\r\n",rpm);
//            printf("cumdistance: %f\r\n",cumdistance);
//            printf("reladistance: %f\r\n",reladistance);
            nh.spinOnce();
            wait(1);
        }
//        motor1.setTarget(60);
//        set_speed();
    }

}