Diff: main.cpp

Revision:

4:cf1a4e503974

Parent:

3:a45557a0dcb8

Child:

6:20a32baeff79

--- a/main.cpp	Tue Dec 11 17:51:47 2018 +0000
+++ b/main.cpp	Wed Dec 12 03:35:52 2018 +0000
@@ -3,16 +3,16 @@
 
 #include "myRos.h"
 #include "myCan.h"
+#include "myOled.h"
 #include <ros.h>
 #include <ros/time.h>
 #include <std_msgs/Empty.h>
+#include <geometry_msgs/Point.h>
 #include "odom.h"
 
-//prototype
-void odom_update_callback();
-
 //Create subscliber
 ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb);
+ros::Subscriber<geometry_msgs::Point> drift_sub("nucleo/drift", &drift_sub_cb);
 
 //Initialize GPIO
 DigitalOut myled(LED1);
@@ -38,14 +38,21 @@
     //Initialize ROS
     My_Ros my_ros(&odom);
     my_ros.set_subscliber(ack_sub);
+    my_ros.set_subscliber(drift_sub);
     
+    //Initialize CAN
     My_Can my_can(&odom);
     my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl);
     my_can.led_toggle_attach(&led1_toggle);
     
+    //Initialize OLED
+    I2C i2c(PB_4, PA_8);
+    My_Oled my_oled(&odom, i2c);
+    
     std::vector<Odom_Abstract*> odom_container;
     odom_container.push_back(&my_ros);
     odom_container.push_back(&my_can);
+    odom_container.push_back(&my_oled);
 
     while(1) {
         //For debug
@@ -62,6 +69,7 @@
             }
         }
         
+        //Debug
         if(mysw == 0){
             while(mysw == 0);
             my_can.test();