Mecanum robot firmware for WRS2020 Ususama

Dependencies:   UsusamaSerial MecanumInterface PMSU_100 PIDController IMUInterface WaypointManager i2cmaster MecanumController

Files at this revision

API Documentation at this revision

Comitter:
sgrsn
Date:
Wed Aug 25 12:12:06 2021 +0000
Parent:
2:626a20dd7987
Commit message:
Update

Changed in this revision

MecanumController.lib Show annotated file Show diff for this revision Revisions of this file
UsusamaSerial.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/MecanumController.lib	Wed Aug 25 08:56:53 2021 +0000
+++ b/MecanumController.lib	Wed Aug 25 12:12:06 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/sgrsn/code/MecanumController/#26c16fb42252
+https://os.mbed.com/users/sgrsn/code/MecanumController/#bc2e5034dda7
--- a/UsusamaSerial.lib	Wed Aug 25 08:56:53 2021 +0000
+++ b/UsusamaSerial.lib	Wed Aug 25 12:12:06 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/sgrsn/code/UsusamaSerial/#3625406774ec
+https://os.mbed.com/users/sgrsn/code/UsusamaSerial/#fd646595de66
--- 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() )
         {