Diff: main.cpp

Revision:

3:a45557a0dcb8

Parent:

2:086272a2da1c

Child:

4:cf1a4e503974

--- a/main.cpp	Fri Dec 07 22:34:31 2018 +0000
+++ b/main.cpp	Tue Dec 11 17:51:47 2018 +0000
@@ -1,10 +1,11 @@
-#include "mbed.h"
+#include <mbed.h>
+#include <vector>  
 
 #include "myRos.h"
+#include "myCan.h"
 #include <ros.h>
 #include <ros/time.h>
 #include <std_msgs/Empty.h>
-
 #include "odom.h"
 
 //prototype
@@ -20,7 +21,11 @@
 DigitalOut  led2(PB_0);
 DigitalIn   mysw(PC_13);
 
-//Initialize ODOM
+void led0_toggle(void) { led0 = !led0; }
+void led1_toggle(void) { led1 = !led1; }
+void led2_toggle(void) { led2 = !led2; }
+
+//Create Odom instance
 Odom odom;
     
 int main() {
@@ -30,33 +35,39 @@
     uint32_t debug_ts = 0;
     uint32_t ros_loop_ts = 0;
     
-    //Set TIM IT Callback
-    Ticker odom_tick;
-    odom_tick.attach(&odom_update_callback, 0.02);
-        
     //Initialize ROS
-    My_Ros my_ros;
+    My_Ros my_ros(&odom);
     my_ros.set_subscliber(ack_sub);
+    
+    My_Can my_can(&odom);
+    my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl);
+    my_can.led_toggle_attach(&led1_toggle);
+    
+    std::vector<Odom_Abstract*> odom_container;
+    odom_container.push_back(&my_ros);
+    odom_container.push_back(&my_can);
 
     while(1) {
         //For debug
         if(debug_ts <= GetTick.read_ms()){
-            debug_ts = GetTick.read_ms() + 20;
-            led0 = !led0;
+            debug_ts = GetTick.read_ms() + 100;
+            led0_toggle();
         }
 
-        //For myRos loop
+        //For odom loop
         if(ros_loop_ts <= GetTick.read_ms()){        
             ros_loop_ts = GetTick.read_ms() + 10;
-            my_ros.loop();
+            for(std::vector<Odom_Abstract*>::iterator it = odom_container.begin(); it != odom_container.end(); ++it){
+                (*it)->loop();
+            }
         }
         
+        if(mysw == 0){
+            while(mysw == 0);
+            my_can.test();
+        }
         
         //角度計算用:ポーリングに置いとく
         odom.update_angle();
     }
-}
-
-void odom_update_callback(){
-    odom.update_it_cb();
 }
\ No newline at end of file