Diff: main.cpp

Revision:

13:681f3d1e9077

Parent:

11:2228e8931266

--- a/main.cpp	Thu Jul 11 13:30:27 2019 +0000
+++ b/main.cpp	Wed Aug 07 13:40:22 2019 +0000
@@ -6,29 +6,13 @@
 #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);
+BusOut bus(D11,D12);
+MotorCtl motor1(D3,bus,D4,D5);
 InterruptIn tachoINT1(D4);
 InterruptIn tachoINT2(D5);
-//Serial pc(USBTX,USBRX,115200);
+RawSerial pc(USBTX,USBRX,115200);
 
 char rx_buffer[10];
 int index=0;
@@ -38,110 +22,56 @@
     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);
+
+    motor1.setTarget(speed);
+
+   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);
-    
+    pc.attach(callback(rx_cb));
+    int rpm;
     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";
+    wait(1);
+    motor1.setTarget(60);
     
-    motor1.setTarget(60);
     while(1) {
         flag=0;
 
-//        pc.printf("Enter the value for speed [-78,78]\r\n");
-        while(flag!=1) {
-            
+        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();
+//            pc.printf("RPM: %d\r\n",rpm);
             wait(1);
         }
 //        motor1.setTarget(60);
-//        set_speed();
+        set_speed();
     }
 
 }
\ No newline at end of file