Revision 4:fd646595de66, committed 2021-08-25

Comitter:

sgrsn

Date:

Wed Aug 25 12:11:35 2021 +0000

Parent:

3:3625406774ec

Commit message:

Update

Changed in this revision

diff -r 3625406774ec -r fd646595de66 ususama_controller.hpp
--- a/ususama_controller.hpp	Wed Aug 25 08:53:43 2021 +0000
+++ b/ususama_controller.hpp	Wed Aug 25 12:11:35 2021 +0000
@@ -26,6 +26,18 @@
         {
             move_command.enable = false;
         }
+        switch(last_command_)
+        {
+            case UsusamaProtocol::COMMAND_POSE_X:
+            case UsusamaProtocol::COMMAND_POSE_Y:
+            case UsusamaProtocol::COMMAND_POSE_THETA:
+                cmd_pose_changed_ = true;
+                break;
+            case UsusamaProtocol::COMMAND_MOVE:
+                //cmd_pose_changed_ = false;
+                break;    
+        }
+            
         move_reply.x = x;
         move_reply.y = y;
         move_reply.theta = theta;
@@ -58,17 +70,41 @@
     void reply_commad_pose()
     {
         // 受け取った値をそのまま返す
-        pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_X], UsusamaProtocol::REPLY_COMMAND_X);
-        pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_Y], UsusamaProtocol::REPLY_COMMAND_Y);
-        pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA], UsusamaProtocol::REPLY_COMMAND_THETA);
+        switch(n_reply_commad_pose)
+        {
+            case 0:
+                pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_X], UsusamaProtocol::REPLY_COMMAND_X);
+                break;
+            case 1:
+                pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_Y], UsusamaProtocol::REPLY_COMMAND_Y);
+                break;
+            case 2:
+                pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA], UsusamaProtocol::REPLY_COMMAND_THETA);
+                break;
+        }
+        n_reply_commad_pose++;
+        if (n_reply_commad_pose == 3) n_reply_commad_pose = 0;
     }
     
     void reply_current_pose()
     {
-        pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.reached), UsusamaProtocol::REPLY_MOVE);
-        pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.x), UsusamaProtocol::REPLY_STATE_X);
-        pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.y), UsusamaProtocol::REPLY_STATE_Y);
-        pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.theta), UsusamaProtocol::REPLY_STATE_THETA);
+        switch(n_reply_current_pose)
+        {
+            case 0:
+                pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.reached), UsusamaProtocol::REPLY_MOVE);
+                break;
+            case 1:
+                pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.x), UsusamaProtocol::REPLY_STATE_X);
+                break;
+            case 2:
+                pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.y), UsusamaProtocol::REPLY_STATE_Y);
+                break;
+            case 3:
+                pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.theta), UsusamaProtocol::REPLY_STATE_THETA);
+                break;
+        }
+        n_reply_current_pose++;
+        if (n_reply_current_pose == 4) n_reply_current_pose = 0;
     }
     
     bool is_enable_to_move()
@@ -80,7 +116,14 @@
         move_command.enable = false;
         return move_command.enable;
     }
-    
+    bool is_ref_pose_changed()
+    {
+        return cmd_pose_changed_;
+    }
+    void notify_use_ref_pose()
+    {
+        cmd_pose_changed_ = false;
+    }
     bool is_stop_called()
     {
         return stop_command.stop;
@@ -131,6 +174,9 @@
     UsusamaSerial *pc_;
     uint8_t last_command_ = 0;
     bool cmd_pose_changed_ = false;
+    
+    int n_reply_current_pose = 0;
+    int n_reply_commad_pose = 0;
 };
 
 #endif
\ No newline at end of file