Diff: ususama_controller.hpp

Revision:

3:3625406774ec

Parent:

2:92e2ae1b1886

Child:

4:fd646595de66

--- a/ususama_controller.hpp	Tue Aug 24 07:05:55 2021 +0000
+++ b/ususama_controller.hpp	Wed Aug 25 08:53:43 2021 +0000
@@ -10,29 +10,34 @@
     
     struct UsusamaProtocol::MoveCommand_t move_command;
     struct UsusamaProtocol::StopCommand_t stop_command;
+    struct UsusamaProtocol::MoveReply_t move_reply;
     
     UsusamaController(UsusamaSerial *pc)
     {
         pc_ = pc;
+        move_command.enable = false;
+        stop_command.stop = false;
     }
     
-    void update()
+    void update(float x, float y, float theta)
     {
-        
         // stopコマンドをチェック
-        if(stop_command.stop > 0)
+        if(stop_command.stop)
         {
             move_command.enable = false;
         }
+        move_reply.x = x;
+        move_reply.y = y;
+        move_reply.theta = theta;
     }
     
-    void write_robot_state(float state_x, float state_y, float state_theta)
+    void write_robot_state()
     {
         // 送られてきたコマンドによって何を送るか決める
         switch(last_command_)
         {
             case UsusamaProtocol::COMMAND_MOVE:
-                reply_current_pose(state_x, state_y, state_theta);
+                reply_current_pose();
                 break;
             
             case UsusamaProtocol::COMMAND_POSE_X:
@@ -42,45 +47,61 @@
                 break;
                 
             case UsusamaProtocol::COMMAND_STOP:
-                reply_current_pose(state_x, state_y, state_theta);
+                reply_current_pose();
                 break;
         }
         
+        //pc_ -> writeData(last_command_, UsusamaProtocol::DEBUG_CONSOLE);
+        
     }
     
     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);
     }
     
-    void reply_current_pose(float x, float y, float theta)
+    void reply_current_pose()
     {
-        int x_int = UsusamaProtocol::encode_float2int( x );
-        int y_int = UsusamaProtocol::encode_float2int( y );
-        int theta_int = UsusamaProtocol::encode_float2int( theta );
-        
-        pc_ -> writeData(x_int, UsusamaProtocol::REPLY_STATE_X);
-        pc_ -> writeData(y_int, UsusamaProtocol::REPLY_STATE_Y);
-        pc_ -> writeData(theta_int, UsusamaProtocol::REPLY_STATE_THETA);
+        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);
     }
     
     bool is_enable_to_move()
     {
         return move_command.enable;
     }
+    bool disable_to_move()
+    {
+        move_command.enable = false;
+        return move_command.enable;
+    }
     
-    UsusamaProtocol::MoveCommand_t getReferencePose()
+    bool is_stop_called()
+    {
+        return stop_command.stop;
+    }
+    
+    UsusamaProtocol::MoveCommand_t getMoveCommand()
     {
         return move_command;
     }
     
+    void setReachedGoal(bool reached)
+    {
+        move_reply.reached = reached;
+    }
     
     void receive_pc_process()
     {
         uint8_t reg = pc_ -> readData();
         
+        if(reg == 0) return;
+        
         switch(reg)
         {
             case UsusamaProtocol::COMMAND_MOVE:
@@ -103,13 +124,13 @@
                 stop_command.stop = pc_->_register[UsusamaProtocol::COMMAND_STOP];
                 break;
         }
-        
         last_command_ = reg;
     }
     
     private:
     UsusamaSerial *pc_;
     uint8_t last_command_ = 0;
+    bool cmd_pose_changed_ = false;
 };
 
 #endif
\ No newline at end of file