File content as of revision 13:3ac8d2472417:

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

BusOut bus(PA_11,PA_12);
MotorCtl motor1(PB_13,bus,PB_14,PB_15);
InterruptIn tachoINT1(PB_14);
InterruptIn tachoINT2(PB_15);


std_msgs::Float64 cum_dist;
std_msgs::Float64 rela_dist;
std_msgs::Float64 flaw_loca;
std_msgs::Float64 ultra_reflection;
std_msgs::Float64 curr_vel;
std_msgs::Int32 curr_rpm;


geometry_msgs::Point flaw_info;

void goalVelCb(const std_msgs::Float64& msg){
    motor1.setTarget(msg.data);
}


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 curr_vel_pub("curr_vel", &curr_vel);
ros::Subscriber<std_msgs::Float64> goal_vel_sub("goal_vel", &goalVelCb);


int index=0;
volatile int flag;

void update_current(void)
{
    motor1.UpdateCurrentPosition();  
}


int main()
{
    
    tachoINT1.fall(&update_current);
    tachoINT1.rise(&update_current);
    tachoINT2.fall(&update_current);
    tachoINT2.rise(&update_current);
    
    wait(1);
    motor1.setTarget(60);
    

    int rpm;
    float velocity;
    float reladistance; //m단위
    float cumdistance;
    float ultra_reflect = 0;
    float flaw_location = 0;
    
    nh.initNode();
    
    nh.advertise(cum_dist_pub);
    nh.advertise(rela_dist_pub);
    nh.advertise(rpm_pub);
    nh.subscribe(goal_vel_sub);
    
    
    
    while(1) {
            
            rpm = motor1.getRPM();
            cumdistance = motor1.CalculateCumDis(); //  누적거리
            reladistance = motor1.CalculateRelaDis(); // 상대거리
            velocity = motor1.CalculateVelocity();

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

            nh.spinOnce();
            wait(1);
        
    }

}