Diff: main.cpp

Revision:

11:2228e8931266

Parent:

8:efe5a5b1f10a

Child:

12:da4fb0d541ca

Child:

13:681f3d1e9077

--- a/main.cpp	Wed Jul 10 04:31:08 2019 +0000
+++ b/main.cpp	Thu Jul 11 13:30:27 2019 +0000
@@ -4,20 +4,26 @@
 #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;
-//std_msgs::String flaw_state;
+
+
+geometry_msgs::Point flaw_info;
 
 
-ros::NodeHandle nh ;
-ros::Publisher cum_dist_pub("cum_dist", &cum_dist);
+ros::NodeHandle nh;
+
 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);
+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);
@@ -65,11 +71,12 @@
 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);
+    nh.advertise(ultra_reflection_pub);
+    nh.advertise(flaw_info_pub);
+    nh.advertise(curr_vel_pub);
     
     tachoINT1.fall(&update_current);
     tachoINT1.rise(&update_current);
@@ -77,8 +84,10 @@
     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";
     
@@ -88,26 +97,40 @@
 
 //        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);
+            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);            
+            rpm_pub.publish(&curr_rpm);   
+            
+            curr_vel.data = velocity;
+            curr_vel_pub.publish(&curr_vel);         
             
-            flaw_loca.data = flaw_location;
-            flaw_loca_pub.publish(&flaw_loca);
+            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;
+            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);