Revision 13:3ac8d2472417, committed 2019-11-21

Comitter:

Jeonghoon

Date:

Thu Nov 21 11:39:20 2019 +0000

Parent:

12:da4fb0d541ca

Commit message:

complete motor

Changed in this revision

--- a/MbedHardware.h	Tue Aug 06 08:33:42 2019 +0000
+++ b/MbedHardware.h	Thu Nov 21 11:39:20 2019 +0000
@@ -14,7 +14,7 @@
 
 class MbedHardware {
   public:
-    MbedHardware(PinName tx, PinName rx, long baud = 57600)
+    MbedHardware(PinName tx, PinName rx, long baud = 115200)
       :iostream(tx, rx){
       baud_ = baud;
       t.start();
@@ -22,7 +22,7 @@
 
     MbedHardware()
       :iostream(USBTX, USBRX) {
-        baud_ = 57600;
+        baud_ = 115200;
         t.start();
     }
 

--- a/main.cpp	Tue Aug 06 08:33:42 2019 +0000
+++ b/main.cpp	Thu Nov 21 11:39:20 2019 +0000
@@ -6,6 +6,12 @@
 #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;
@@ -16,133 +22,78 @@
 
 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 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);
+ros::Subscriber<std_msgs::Float64> goal_vel_sub("goal_vel", &goalVelCb);
 
-BusOut bus(D11,D12);
-MotorCtl motor1(D3,bus,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");
+    motor1.UpdateCurrentPosition();  
 }
-//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));
+    
+    wait(1);
+    motor1.setTarget(60);
+    
+
     int rpm;
     float velocity;
     float reladistance; //m단위
     float cumdistance;
     float ultra_reflect = 0;
     float flaw_location = 0;
-//    char flaw_curr_state[10] = "stable";
+    
+    nh.initNode();
     
-    motor1.setTarget(60);
+    nh.advertise(cum_dist_pub);
+    nh.advertise(rela_dist_pub);
+    nh.advertise(rpm_pub);
+    nh.subscribe(goal_vel_sub);
+    
+    
+    
     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);
+            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();
+        
     }
 
 }
\ No newline at end of file

--- a/mbed-os.lib	Tue Aug 06 08:33:42 2019 +0000
+++ b/mbed-os.lib	Thu Nov 21 11:39:20 2019 +0000
@@ -1,1 +1,1 @@
-https://github.com/ARMmbed/mbed-os/#51d55508e8400b60af467005646c4e2164738d48
+https://github.com/ARMmbed/mbed-os/#949cb49ab0a144da0e3b04b6af46db0cd2a20d75