File content as of revision 11:2228e8931266:

#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>

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;


ros::NodeHandle nh;

ros::Publisher rela_dist_pub("rela_dist", &rela_dist);
ros::Publisher rpm_pub("rpm", &curr_rpm);
ros::Publisher ultra_reflection_pub("ultra_reflection", &ultra_reflection);
ros::Publisher flaw_info_pub("flaw_info", &flaw_info);
ros::Publisher curr_vel_pub("curr_vel", &curr_vel);

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(rela_dist_pub);
    nh.advertise(rpm_pub);
    nh.advertise(ultra_reflection_pub);
    nh.advertise(flaw_info_pub);
    nh.advertise(curr_vel_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 velocity;
    float reladistance; //m단위
    float cumdistance;
    float ultra_reflect = 0;
    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(); // 상대거리
            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);   
            
            curr_vel.data = velocity;
            curr_vel_pub.publish(&curr_vel);         
            
            ultra_reflection.data = ultra_reflect;
            ultra_reflection_pub.publish(&ultra_reflection);

            flaw_info.x = cumdistance;
            flaw_info.y = flaw_location;
            flaw_info_pub.publish(&flaw_info);
            
            flaw_location += 0.05;
            if(flaw_location > 1){
                flaw_location = 0;
            } 
            
            ultra_reflect += 0.1;
            if(ultra_reflect > 0.5){
                ultra_reflect = 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();
    }

}