Diff: main.cpp

Revision:

8:efe5a5b1f10a

Parent:

7:13dd93a0efe8

Child:

11:2228e8931266

diff -r 13dd93a0efe8 -r efe5a5b1f10a main.cpp
--- a/main.cpp	Thu Jun 27 04:58:29 2019 +0000
+++ b/main.cpp	Wed Jul 10 03:51:11 2019 +0000
@@ -2,23 +2,27 @@
 #include "motor.h"
 #include <ros.h>
 #include <std_msgs/Float64.h>
-#include <std_msgs/Float32.h>
 #include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
 
 std_msgs::Float64 cum_dist;
-std_msgs::Float32 rela_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);
+//Serial pc(USBTX,USBRX,115200);
 
 char rx_buffer[10];
 int index=0;
@@ -28,35 +32,35 @@
     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);
-
-}
+//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()
 {
@@ -64,20 +68,25 @@
     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));
+//    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");
+//        pc.printf("Enter the value for speed [-78,78]\r\n");
         while(flag!=1) {
             rpm = motor1.getRPM();
             cumdistance = motor1.CalculateCumDis(); //  누적거리
@@ -92,13 +101,24 @@
             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);
+            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();
+//        set_speed();
     }
 
-}
+}
\ No newline at end of file