Diff: ususama_controller.hpp

Revision:

1:33d6c6f43306

Child:

2:92e2ae1b1886

diff -r a0ec74fb2cb0 -r 33d6c6f43306 ususama_controller.hpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ususama_controller.hpp	Mon Aug 23 17:02:24 2021 +0000
@@ -0,0 +1,115 @@
+#ifndef USUSAMA_CONTROLLER_H
+#define USUSAMA_CONTROLLER_H
+
+#include "ususama_protocol.hpp"
+#include "ususama_serial.h"
+
+class UsusamaController
+{
+    public:
+    
+    struct UsusamaProtocol::MoveCommand_t move_command;
+    struct UsusamaProtocol::StopCommand_t stop_command;
+    
+    UsusamaController(UsusamaSerial *pc)
+    {
+        pc_ = pc;
+    }
+    
+    void update()
+    {
+        
+        // stopコマンドをチェック
+        if(stop_command.stop > 0)
+        {
+            move_command.enable = false;
+        }
+    }
+    
+    void write_robot_state(float state_x, float state_y, float state_theta)
+    {
+        // 送られてきたコマンドによって何を送るか決める
+        switch(last_command_)
+        {
+            case UsusamaProtocol::COMMAND_MOVE:
+                reply_current_pose(state_x, state_y, state_theta);
+                break;
+            
+            case UsusamaProtocol::COMMAND_POSE_X:
+            case UsusamaProtocol::COMMAND_POSE_Y:
+            case UsusamaProtocol::COMMAND_POSE_THETA:
+                reply_commad_pose();
+                break;
+                
+            case UsusamaProtocol::COMMAND_STOP:
+                reply_current_pose(state_x, state_y, state_theta);
+                break;
+        }
+        
+    }
+    
+    void reply_commad_pose()
+    {        
+        pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_X], UsusamaProtocol::REPLY_STATE_X);
+        pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_Y], UsusamaProtocol::REPLY_STATE_Y);
+        pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA], UsusamaProtocol::REPLY_STATE_THETA);
+    }
+    
+    void reply_current_pose(float x, float y, float theta)
+    {
+        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);
+    }
+    
+    bool is_enable_to_move()
+    {
+        return move_command.enable;
+    }
+    
+    UsusamaProtocol::MoveCommand_t getReferencePose()
+    {
+        return move_command;
+    }
+    
+    
+    void receive_pc_process()
+    {
+        uint8_t reg = pc_ -> readData();
+        
+        switch(reg)
+        {
+            case UsusamaProtocol::COMMAND_MOVE:
+                move_command.enable = pc_->_register[UsusamaProtocol::COMMAND_MOVE];
+                break;
+                
+            case UsusamaProtocol::COMMAND_POSE_X:
+                move_command.x = pc_->_register[UsusamaProtocol::COMMAND_POSE_X];
+                break;
+                
+            case UsusamaProtocol::COMMAND_POSE_Y:
+                move_command.y = pc_->_register[UsusamaProtocol::COMMAND_POSE_Y];
+                break;
+                
+            case UsusamaProtocol::COMMAND_POSE_THETA:
+                move_command.theta = pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA];
+                break;
+                
+            case UsusamaProtocol::COMMAND_STOP:
+                stop_command.stop = pc_->_register[UsusamaProtocol::COMMAND_STOP];
+                break;
+        }
+        
+        last_command_ = reg;
+    }
+    
+    private:
+    UsusamaSerial *pc_;
+    uint8_t last_command_ = 0;
+};
+
+#endif
\ No newline at end of file