File content as of revision 2:92e2ae1b1886:

#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_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)
    {
        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 = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_X] );
                break;
                
            case UsusamaProtocol::COMMAND_POSE_Y:
                move_command.y = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_Y] );
                break;
                
            case UsusamaProtocol::COMMAND_POSE_THETA:
                move_command.theta = UsusamaProtocol::decode_int2float( 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