Serial library for Ususama
Dependents: ususama_serial_demo WRS2021_mecanum_driver
ususama_controller.hpp
- Committer:
- sgrsn
- Date:
- 2021-08-25
- Revision:
- 3:3625406774ec
- Parent:
- 2:92e2ae1b1886
- Child:
- 4:fd646595de66
File content as of revision 3:3625406774ec:
#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; struct UsusamaProtocol::MoveReply_t move_reply; UsusamaController(UsusamaSerial *pc) { pc_ = pc; move_command.enable = false; stop_command.stop = false; } void update(float x, float y, float theta) { // stopコマンドをチェック if(stop_command.stop) { move_command.enable = false; } move_reply.x = x; move_reply.y = y; move_reply.theta = theta; } void write_robot_state() { // 送られてきたコマンドによって何を送るか決める switch(last_command_) { case UsusamaProtocol::COMMAND_MOVE: reply_current_pose(); 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(); 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() { 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; } 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: 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; bool cmd_pose_changed_ = false; }; #endif