Diff: main.cpp

Revision:

3:35244995ff2e

Parent:

2:626a20dd7987

--- a/main.cpp	Wed Aug 25 08:56:53 2021 +0000
+++ b/main.cpp	Wed Aug 25 12:12:06 2021 +0000
@@ -44,7 +44,7 @@
 {
     while (true) {
         commander.receive_pc_process();
-        ThisThread::sleep_for(53);
+        ThisThread::sleep_for(103);
     }
 }
 
@@ -52,7 +52,7 @@
 {
     while (true) {
         commander.write_robot_state();
-        ThisThread::sleep_for(101);
+        ThisThread::sleep_for(211);
     }
 }
 
@@ -68,9 +68,7 @@
     
     ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0));
     // theta...
-    //WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), Pose2D(0.8, 0, 0), Pose2D(0.01, 0.01, 0.01), 5.0);
     WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), Pose2D(0.8, 0, 0), 10.0);
-    //int i_way = 0;
     Timer t_way;
     t_way.start();
     
@@ -80,6 +78,7 @@
     // to do waypoint生成とmove_command.enableの処理
     bool is_way_generated = false;
     bool is_stoped = false;
+    bool new_goal = false;
     
     while(1)
     {
@@ -97,32 +96,24 @@
             continue;
         }
         
-        // waypoint存在していれば移動
-        if(is_way_generated)
-        {
-            /*if(way_generator.GetPose(i_way).time_stamp < t_way.read())
-            {
-                i_way++;
-            }
-            ususama.ControlPosition( way_generator.GetPose(i_way) );*/
-            float t = t_way.read();
-            ususama.ControlPosition( way_generator.GetPose( t ) );
-        }
-        // waypoint存在していなければ生成
-        else
+        if( commander.is_ref_pose_changed() || !is_way_generated)
         {
             way_generator = WaypointGenerator<Pose2D>(
                 ususama.GetPose(), 
                 ref_pose, 
-                //Pose2D(0.05, 0.05, 0.1), 
                 10.0
             );
             is_way_generated = true;
-            //i_way = 0;
+            commander.notify_use_ref_pose();
             t_way.reset();
             t_way.start();
         }
-        
+        // waypoint存在していれば移動
+        else
+        {
+            float t = t_way.read();
+            ususama.ControlPosition( way_generator.GetPose( t ) );
+        }
         // stopコマンド確認
         if( commander.is_stop_called() )
         {