Diff: main.cpp

Revision:

2:626a20dd7987

Parent:

1:2720d0e8b2f1

Child:

3:35244995ff2e

--- a/main.cpp	Tue Aug 24 07:06:27 2021 +0000
+++ b/main.cpp	Wed Aug 25 08:56:53 2021 +0000
@@ -27,7 +27,7 @@
 
 void mecanum_test()
 {
-    ususama.ControllVelocity(Vel2D(0.1, 0.0, 0.0));
+    ususama.ControllVelocity(Vel2D(1.0, 0.0, 0.0));
     thread_sleep_for(1000);
     ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0));
 }
@@ -44,75 +44,112 @@
 {
     while (true) {
         commander.receive_pc_process();
-        leds = commander.getReferencePose().x;
         ThisThread::sleep_for(53);
     }
 }
 
 void write_pc_thread()
 {
-    Pose2D current_pose(0,0,0);
     while (true) {
-        current_pose = ususama.GetPose();
-        commander.write_robot_state(current_pose.x, current_pose.y, current_pose.theta);
-        ThisThread::sleep_for(47);
+        commander.write_robot_state();
+        ThisThread::sleep_for(101);
     }
 }
 
 
 int main()
 {
-    std::list<std::unique_ptr<Pose2D>> ref_pose_list;
-    ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.8, 0.0));
-    ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, 0.0));
+    //std::list<std::unique_ptr<Pose2D>> ref_pose_list;
+    //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.8, 0.0));
+    //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, 0.0));
     //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.5, 0));
     //ref_pose_list.push_back(std::make_unique<Pose2D>(0.0, 0.0, -1.57));
-    auto ref_pose_itr = ref_pose_list.begin();
+    //auto ref_pose_itr = ref_pose_list.begin();
     
+    ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0));
     // theta...
-    WaypointGenerator<Pose2D> way_generator( Pose2D(0, 0, 0), *(*ref_pose_itr), Pose2D(0.01, 0.01, 0.01), 5.0);
-    int i_way = 0;
+    //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();
     
     thread_read2pc.start(read_pc_thread);
     thread_write2pc.start(write_pc_thread);
     
+    // to do waypoint生成とmove_command.enableの処理
+    bool is_way_generated = false;
+    bool is_stoped = false;
+    
     while(1)
     {
-        // todo: read using timer
-        thread_sleep_for(10);
-        
-        
-        UsusamaProtocol::MoveCommand_t ref_pose = commander.getReferencePose();
-        
+        //thread_sleep_for(10);
         
         ususama.ComputePose();
-        ususama.ControlPosition( way_generator.GetPose(i_way) );
+        commander.update(ususama.GetPose().x, ususama.GetPose().y, ususama.GetPose().theta);
+        
+        UsusamaProtocol::MoveCommand_t move_command = commander.getMoveCommand();
+        Pose2D ref_pose(move_command.x, move_command.y, move_command.theta);
         
-        if(way_generator.GetPose(i_way).time_stamp < t_way.read())
+        if(!move_command.enable)
         {
-            i_way++;
+            ususama.ControllVelocity(Vel2D(0.0, 0.0, 0.0));
+            continue;
         }
         
-        if( ususama.IsReferencePose( *(*ref_pose_itr) ) )
+        // waypoint存在していれば移動
+        if(is_way_generated)
         {
-            //printf("goal\r\n");
-            
-            ref_pose_itr++;
-            if(ref_pose_itr == ref_pose_list.end())
+            /*if(way_generator.GetPose(i_way).time_stamp < t_way.read())
             {
-                ref_pose_itr = ref_pose_list.begin();
+                i_way++;
             }
-            
-            // update waypoint
-            //printf("generate new waypoint...");
-            way_generator = WaypointGenerator<Pose2D>( ususama.GetPose(), *(*ref_pose_itr), Pose2D(0.01, 0.01, 0.005), 5.0);
-            //printf("has finished\r\n");
-            
-            i_way = 0;
+            ususama.ControlPosition( way_generator.GetPose(i_way) );*/
+            float t = t_way.read();
+            ususama.ControlPosition( way_generator.GetPose( t ) );
+        }
+        // waypoint存在していなければ生成
+        else
+        {
+            way_generator = WaypointGenerator<Pose2D>(
+                ususama.GetPose(), 
+                ref_pose, 
+                //Pose2D(0.05, 0.05, 0.1), 
+                10.0
+            );
+            is_way_generated = true;
+            //i_way = 0;
             t_way.reset();
             t_way.start();
         }
+        
+        // stopコマンド確認
+        if( commander.is_stop_called() )
+        {
+            is_way_generated = false;
+            commander.disable_to_move();
+            t_way.stop();
+            is_stoped = true;
+            //ususama.ControllVelocity( Vel2D(0, 0, 0) );
+            // ususama.CoastAllMotor();
+            // ususama.ControlPosition( ususama.GetPose() );
+        }
+        else if(is_stoped)
+        {
+            t_way.start();
+            is_stoped = false;
+        }
+        
+
+        if( ususama.IsReferencePose( ref_pose ) )
+        {
+            is_way_generated = false;
+            commander.disable_to_move();
+            commander.setReachedGoal(true);
+        }
+        else
+        {
+            commander.setReachedGoal(false);
+        }
     }
 }
\ No newline at end of file