Serial library for Ususama
Dependents: ususama_serial_demo WRS2021_mecanum_driver
ususama_controller.hpp@3:3625406774ec, 2021-08-25 (annotated)
- Committer:
- sgrsn
- Date:
- Wed Aug 25 08:53:43 2021 +0000
- Revision:
- 3:3625406774ec
- Parent:
- 2:92e2ae1b1886
- Child:
- 4:fd646595de66
Update
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sgrsn | 1:33d6c6f43306 | 1 | #ifndef USUSAMA_CONTROLLER_H |
sgrsn | 1:33d6c6f43306 | 2 | #define USUSAMA_CONTROLLER_H |
sgrsn | 1:33d6c6f43306 | 3 | |
sgrsn | 1:33d6c6f43306 | 4 | #include "ususama_protocol.hpp" |
sgrsn | 1:33d6c6f43306 | 5 | #include "ususama_serial.h" |
sgrsn | 1:33d6c6f43306 | 6 | |
sgrsn | 1:33d6c6f43306 | 7 | class UsusamaController |
sgrsn | 1:33d6c6f43306 | 8 | { |
sgrsn | 1:33d6c6f43306 | 9 | public: |
sgrsn | 1:33d6c6f43306 | 10 | |
sgrsn | 1:33d6c6f43306 | 11 | struct UsusamaProtocol::MoveCommand_t move_command; |
sgrsn | 1:33d6c6f43306 | 12 | struct UsusamaProtocol::StopCommand_t stop_command; |
sgrsn | 3:3625406774ec | 13 | struct UsusamaProtocol::MoveReply_t move_reply; |
sgrsn | 1:33d6c6f43306 | 14 | |
sgrsn | 1:33d6c6f43306 | 15 | UsusamaController(UsusamaSerial *pc) |
sgrsn | 1:33d6c6f43306 | 16 | { |
sgrsn | 1:33d6c6f43306 | 17 | pc_ = pc; |
sgrsn | 3:3625406774ec | 18 | move_command.enable = false; |
sgrsn | 3:3625406774ec | 19 | stop_command.stop = false; |
sgrsn | 1:33d6c6f43306 | 20 | } |
sgrsn | 1:33d6c6f43306 | 21 | |
sgrsn | 3:3625406774ec | 22 | void update(float x, float y, float theta) |
sgrsn | 1:33d6c6f43306 | 23 | { |
sgrsn | 1:33d6c6f43306 | 24 | // stopコマンドをチェック |
sgrsn | 3:3625406774ec | 25 | if(stop_command.stop) |
sgrsn | 1:33d6c6f43306 | 26 | { |
sgrsn | 1:33d6c6f43306 | 27 | move_command.enable = false; |
sgrsn | 1:33d6c6f43306 | 28 | } |
sgrsn | 3:3625406774ec | 29 | move_reply.x = x; |
sgrsn | 3:3625406774ec | 30 | move_reply.y = y; |
sgrsn | 3:3625406774ec | 31 | move_reply.theta = theta; |
sgrsn | 1:33d6c6f43306 | 32 | } |
sgrsn | 1:33d6c6f43306 | 33 | |
sgrsn | 3:3625406774ec | 34 | void write_robot_state() |
sgrsn | 1:33d6c6f43306 | 35 | { |
sgrsn | 1:33d6c6f43306 | 36 | // 送られてきたコマンドによって何を送るか決める |
sgrsn | 1:33d6c6f43306 | 37 | switch(last_command_) |
sgrsn | 1:33d6c6f43306 | 38 | { |
sgrsn | 1:33d6c6f43306 | 39 | case UsusamaProtocol::COMMAND_MOVE: |
sgrsn | 3:3625406774ec | 40 | reply_current_pose(); |
sgrsn | 1:33d6c6f43306 | 41 | break; |
sgrsn | 1:33d6c6f43306 | 42 | |
sgrsn | 1:33d6c6f43306 | 43 | case UsusamaProtocol::COMMAND_POSE_X: |
sgrsn | 1:33d6c6f43306 | 44 | case UsusamaProtocol::COMMAND_POSE_Y: |
sgrsn | 1:33d6c6f43306 | 45 | case UsusamaProtocol::COMMAND_POSE_THETA: |
sgrsn | 1:33d6c6f43306 | 46 | reply_commad_pose(); |
sgrsn | 1:33d6c6f43306 | 47 | break; |
sgrsn | 1:33d6c6f43306 | 48 | |
sgrsn | 1:33d6c6f43306 | 49 | case UsusamaProtocol::COMMAND_STOP: |
sgrsn | 3:3625406774ec | 50 | reply_current_pose(); |
sgrsn | 1:33d6c6f43306 | 51 | break; |
sgrsn | 1:33d6c6f43306 | 52 | } |
sgrsn | 1:33d6c6f43306 | 53 | |
sgrsn | 3:3625406774ec | 54 | //pc_ -> writeData(last_command_, UsusamaProtocol::DEBUG_CONSOLE); |
sgrsn | 3:3625406774ec | 55 | |
sgrsn | 1:33d6c6f43306 | 56 | } |
sgrsn | 1:33d6c6f43306 | 57 | |
sgrsn | 1:33d6c6f43306 | 58 | void reply_commad_pose() |
sgrsn | 3:3625406774ec | 59 | { |
sgrsn | 3:3625406774ec | 60 | // 受け取った値をそのまま返す |
sgrsn | 2:92e2ae1b1886 | 61 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_X], UsusamaProtocol::REPLY_COMMAND_X); |
sgrsn | 2:92e2ae1b1886 | 62 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_Y], UsusamaProtocol::REPLY_COMMAND_Y); |
sgrsn | 2:92e2ae1b1886 | 63 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA], UsusamaProtocol::REPLY_COMMAND_THETA); |
sgrsn | 1:33d6c6f43306 | 64 | } |
sgrsn | 1:33d6c6f43306 | 65 | |
sgrsn | 3:3625406774ec | 66 | void reply_current_pose() |
sgrsn | 1:33d6c6f43306 | 67 | { |
sgrsn | 3:3625406774ec | 68 | pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.reached), UsusamaProtocol::REPLY_MOVE); |
sgrsn | 3:3625406774ec | 69 | pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.x), UsusamaProtocol::REPLY_STATE_X); |
sgrsn | 3:3625406774ec | 70 | pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.y), UsusamaProtocol::REPLY_STATE_Y); |
sgrsn | 3:3625406774ec | 71 | pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.theta), UsusamaProtocol::REPLY_STATE_THETA); |
sgrsn | 1:33d6c6f43306 | 72 | } |
sgrsn | 1:33d6c6f43306 | 73 | |
sgrsn | 1:33d6c6f43306 | 74 | bool is_enable_to_move() |
sgrsn | 1:33d6c6f43306 | 75 | { |
sgrsn | 1:33d6c6f43306 | 76 | return move_command.enable; |
sgrsn | 1:33d6c6f43306 | 77 | } |
sgrsn | 3:3625406774ec | 78 | bool disable_to_move() |
sgrsn | 3:3625406774ec | 79 | { |
sgrsn | 3:3625406774ec | 80 | move_command.enable = false; |
sgrsn | 3:3625406774ec | 81 | return move_command.enable; |
sgrsn | 3:3625406774ec | 82 | } |
sgrsn | 1:33d6c6f43306 | 83 | |
sgrsn | 3:3625406774ec | 84 | bool is_stop_called() |
sgrsn | 3:3625406774ec | 85 | { |
sgrsn | 3:3625406774ec | 86 | return stop_command.stop; |
sgrsn | 3:3625406774ec | 87 | } |
sgrsn | 3:3625406774ec | 88 | |
sgrsn | 3:3625406774ec | 89 | UsusamaProtocol::MoveCommand_t getMoveCommand() |
sgrsn | 1:33d6c6f43306 | 90 | { |
sgrsn | 1:33d6c6f43306 | 91 | return move_command; |
sgrsn | 1:33d6c6f43306 | 92 | } |
sgrsn | 1:33d6c6f43306 | 93 | |
sgrsn | 3:3625406774ec | 94 | void setReachedGoal(bool reached) |
sgrsn | 3:3625406774ec | 95 | { |
sgrsn | 3:3625406774ec | 96 | move_reply.reached = reached; |
sgrsn | 3:3625406774ec | 97 | } |
sgrsn | 1:33d6c6f43306 | 98 | |
sgrsn | 1:33d6c6f43306 | 99 | void receive_pc_process() |
sgrsn | 1:33d6c6f43306 | 100 | { |
sgrsn | 1:33d6c6f43306 | 101 | uint8_t reg = pc_ -> readData(); |
sgrsn | 1:33d6c6f43306 | 102 | |
sgrsn | 3:3625406774ec | 103 | if(reg == 0) return; |
sgrsn | 3:3625406774ec | 104 | |
sgrsn | 1:33d6c6f43306 | 105 | switch(reg) |
sgrsn | 1:33d6c6f43306 | 106 | { |
sgrsn | 1:33d6c6f43306 | 107 | case UsusamaProtocol::COMMAND_MOVE: |
sgrsn | 1:33d6c6f43306 | 108 | move_command.enable = pc_->_register[UsusamaProtocol::COMMAND_MOVE]; |
sgrsn | 1:33d6c6f43306 | 109 | break; |
sgrsn | 1:33d6c6f43306 | 110 | |
sgrsn | 1:33d6c6f43306 | 111 | case UsusamaProtocol::COMMAND_POSE_X: |
sgrsn | 2:92e2ae1b1886 | 112 | move_command.x = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_X] ); |
sgrsn | 1:33d6c6f43306 | 113 | break; |
sgrsn | 1:33d6c6f43306 | 114 | |
sgrsn | 1:33d6c6f43306 | 115 | case UsusamaProtocol::COMMAND_POSE_Y: |
sgrsn | 2:92e2ae1b1886 | 116 | move_command.y = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_Y] ); |
sgrsn | 1:33d6c6f43306 | 117 | break; |
sgrsn | 1:33d6c6f43306 | 118 | |
sgrsn | 1:33d6c6f43306 | 119 | case UsusamaProtocol::COMMAND_POSE_THETA: |
sgrsn | 2:92e2ae1b1886 | 120 | move_command.theta = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA] ); |
sgrsn | 1:33d6c6f43306 | 121 | break; |
sgrsn | 1:33d6c6f43306 | 122 | |
sgrsn | 1:33d6c6f43306 | 123 | case UsusamaProtocol::COMMAND_STOP: |
sgrsn | 1:33d6c6f43306 | 124 | stop_command.stop = pc_->_register[UsusamaProtocol::COMMAND_STOP]; |
sgrsn | 1:33d6c6f43306 | 125 | break; |
sgrsn | 1:33d6c6f43306 | 126 | } |
sgrsn | 1:33d6c6f43306 | 127 | last_command_ = reg; |
sgrsn | 1:33d6c6f43306 | 128 | } |
sgrsn | 1:33d6c6f43306 | 129 | |
sgrsn | 1:33d6c6f43306 | 130 | private: |
sgrsn | 1:33d6c6f43306 | 131 | UsusamaSerial *pc_; |
sgrsn | 1:33d6c6f43306 | 132 | uint8_t last_command_ = 0; |
sgrsn | 3:3625406774ec | 133 | bool cmd_pose_changed_ = false; |
sgrsn | 1:33d6c6f43306 | 134 | }; |
sgrsn | 1:33d6c6f43306 | 135 | |
sgrsn | 1:33d6c6f43306 | 136 | #endif |