Revision 10:ca4e4062701a, committed 2019-11-26

Comitter:

Jeonghoon

Date:

Tue Nov 26 15:30:27 2019 +0000

Parent:

9:ccd662244071

Commit message:

capstone finish

Changed in this revision

--- a/MbedHardware.h	Thu Nov 21 10:36:11 2019 +0000
+++ b/MbedHardware.h	Tue Nov 26 15:30:27 2019 +0000
@@ -14,15 +14,15 @@
 
 class MbedHardware {
   public:
-    MbedHardware(PinName tx, PinName rx, long baud = 1843200)
+    MbedHardware(PinName tx, PinName rx, long baud)
       :iostream(tx, rx){
       baud_ = baud;
       t.start();
     }
 
     MbedHardware()
-      :iostream(USBTX, USBRX) {
-        baud_ = 1843200;
+      :iostream(USBTX, USBRX) { //PA_11, PA_12 //USBTX, USBRX
+        baud_ = 115200;
         t.start();
     }
 

--- a/main.cpp	Thu Nov 21 10:36:11 2019 +0000
+++ b/main.cpp	Tue Nov 26 15:30:27 2019 +0000
@@ -3,19 +3,51 @@
 //
 #include "mbed.h"
 #include "ov7670.h"
-#include <ros.h>
+#include "motor.h"
+#include "ros.h"
 #include <ros/time.h>
 #include <std_msgs/UInt8MultiArray.h>
 #include <std_msgs/Byte.h>
 #include <std_msgs/Bool.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/String.h>
+
+
+BusOut bus(PC_0,PC_1);
+MotorCtl motor1(PB_13,bus,PB_14,PB_15);
+InterruptIn tachoINT1(PB_14);
+InterruptIn tachoINT2(PB_15);
+
+
+void goalVelCb(const std_msgs::Float64& msg){
+    motor1.setTarget(msg.data);
+}
+
+
+
+
+std_msgs::Float64 cum_dist;
+std_msgs::Float64 rela_dist;
+std_msgs::Float64 curr_vel;
+std_msgs::Int32 curr_rpm;
 
 std_msgs::UInt8MultiArray img;
 //std_msgs::Byte img;
 std_msgs::Bool sync;
-ros::NodeHandle nh;
+
+ros::NodeHandle nh_motor;
+ros::NodeHandle_cam nh_cam;
 ros::Publisher img_pub("image_data", &img);
 ros::Publisher sync_pub("sync", &sync);
 
+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 curr_vel_pub("curr_vel", &curr_vel);
+ros::Subscriber<std_msgs::Float64> goal_vel_sub("goal_vel", &goalVelCb);
+
+
 
 
 OV7670 camera(
@@ -45,21 +77,48 @@
 uint8_t fdata[SIZEX*SIZEY];
 uint8_t fdata_ros[NUMBYTE];
 int loop_num;
+size_t size = SIZEX*SIZEY;
+
+int index=0;
+volatile int flag;
+
+void update_current(void)
+{
+    motor1.UpdateCurrentPosition();  
+}
 
 
-size_t size = SIZEX*SIZEY;
 
 int main()
 {   
-    nh.initNode();
+
+    int rpm;
+    float velocity;
+    float reladistance; //m단위
+    float cumdistance;
+    
+    tachoINT1.fall(&update_current);
+    tachoINT1.rise(&update_current);
+    tachoINT2.fall(&update_current);
+    tachoINT2.rise(&update_current);
+    
+    wait(1);
+    motor1.setTarget(10);
+    
+    nh_cam.initNode();
+    nh_motor.initNode();
     loop_num = size/NUMBYTE;
     sync.data=true;
     wait_ms(1);
 //    pc.baud(115200) ;
     camera.Reset() ;
-    nh.advertise(img_pub);
-    nh.advertise(sync_pub);
-
+    nh_cam.advertise(img_pub);
+    nh_cam.advertise(sync_pub);
+    
+    nh_motor.advertise(cum_dist_pub);
+    nh_motor.advertise(rela_dist_pub);
+    nh_motor.advertise(rpm_pub);
+    nh_motor.subscribe(goal_vel_sub);
 #ifdef QQVGA
 //    camera.InitQQVGA565(true,false) ;
     camera.InitQQVGAYUV(false,false);
@@ -80,6 +139,12 @@
 
         camera.ReadStart() ;
         
+        //motor
+        rpm = motor1.getRPM();
+        cumdistance = motor1.CalculateCumDis(); //  누적거리
+        reladistance = motor1.CalculateRelaDis(); // 상대거리
+        velocity = motor1.CalculateVelocity();        
+        //motor end
         sync.data=true;
         
         for (int y = 0; y < SIZEX*SIZEY ; y++) {
@@ -100,11 +165,23 @@
             img.data_length = NUMBYTE;
             img.data = &fdata_ros[0];
             img_pub.publish(&img);
-            nh.spinOnce();
+            
+            //motor
+            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);               
+            //motor end
+            
+            nh_cam.spinOnce();
+            nh_motor.spinOnce();
             sync.data=false;
-            wait_us(3125); // 25000us 12500
-            
+            wait_us(4687.5); // 3125 us , 1843200 baudrate for camera
+                           // 950 ~ 1000 us for motor
         }
 
        

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.lib	Tue Nov 26 15:30:27 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/kangmingyo/code/motor_sn7544/#b1c5a0cf4a9b

--- a/ros.h	Thu Nov 21 10:36:11 2019 +0000
+++ b/ros.h	Tue Nov 26 15:30:27 2019 +0000
@@ -38,9 +38,17 @@
 #include "ros/node_handle.h"
 #include "MbedHardware.h"
 
+class MbedHardware_cam : public MbedHardware
+{
+    public:
+        MbedHardware_cam(): MbedHardware(PA_11, PA_12, 1228800){};
+};
+
 namespace ros
 {
+  
   typedef NodeHandle_<MbedHardware> NodeHandle;
+  typedef NodeHandle_<MbedHardware_cam> NodeHandle_cam;
 }
 
 #endif

--- a/ros/node_handle.h	Thu Nov 21 10:36:11 2019 +0000
+++ b/ros/node_handle.h	Tue Nov 26 15:30:27 2019 +0000
@@ -69,6 +69,8 @@
 #define MSG_TIMEOUT 20  //20 milliseconds to recieve all of message data
 
 #include "ros/msg.h"
+#include "MbedHardware.h"
+#include "ros.h"
 
 namespace ros {