Serial library for Ususama
Dependents: ususama_serial_demo WRS2021_mecanum_driver
ususama_controller.hpp@4:fd646595de66, 2021-08-25 (annotated)
- Committer:
- sgrsn
- Date:
- Wed Aug 25 12:11:35 2021 +0000
- Revision:
- 4:fd646595de66
- Parent:
- 3:3625406774ec
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 | 4:fd646595de66 | 29 | switch(last_command_) |
sgrsn | 4:fd646595de66 | 30 | { |
sgrsn | 4:fd646595de66 | 31 | case UsusamaProtocol::COMMAND_POSE_X: |
sgrsn | 4:fd646595de66 | 32 | case UsusamaProtocol::COMMAND_POSE_Y: |
sgrsn | 4:fd646595de66 | 33 | case UsusamaProtocol::COMMAND_POSE_THETA: |
sgrsn | 4:fd646595de66 | 34 | cmd_pose_changed_ = true; |
sgrsn | 4:fd646595de66 | 35 | break; |
sgrsn | 4:fd646595de66 | 36 | case UsusamaProtocol::COMMAND_MOVE: |
sgrsn | 4:fd646595de66 | 37 | //cmd_pose_changed_ = false; |
sgrsn | 4:fd646595de66 | 38 | break; |
sgrsn | 4:fd646595de66 | 39 | } |
sgrsn | 4:fd646595de66 | 40 | |
sgrsn | 3:3625406774ec | 41 | move_reply.x = x; |
sgrsn | 3:3625406774ec | 42 | move_reply.y = y; |
sgrsn | 3:3625406774ec | 43 | move_reply.theta = theta; |
sgrsn | 1:33d6c6f43306 | 44 | } |
sgrsn | 1:33d6c6f43306 | 45 | |
sgrsn | 3:3625406774ec | 46 | void write_robot_state() |
sgrsn | 1:33d6c6f43306 | 47 | { |
sgrsn | 1:33d6c6f43306 | 48 | // 送られてきたコマンドによって何を送るか決める |
sgrsn | 1:33d6c6f43306 | 49 | switch(last_command_) |
sgrsn | 1:33d6c6f43306 | 50 | { |
sgrsn | 1:33d6c6f43306 | 51 | case UsusamaProtocol::COMMAND_MOVE: |
sgrsn | 3:3625406774ec | 52 | reply_current_pose(); |
sgrsn | 1:33d6c6f43306 | 53 | break; |
sgrsn | 1:33d6c6f43306 | 54 | |
sgrsn | 1:33d6c6f43306 | 55 | case UsusamaProtocol::COMMAND_POSE_X: |
sgrsn | 1:33d6c6f43306 | 56 | case UsusamaProtocol::COMMAND_POSE_Y: |
sgrsn | 1:33d6c6f43306 | 57 | case UsusamaProtocol::COMMAND_POSE_THETA: |
sgrsn | 1:33d6c6f43306 | 58 | reply_commad_pose(); |
sgrsn | 1:33d6c6f43306 | 59 | break; |
sgrsn | 1:33d6c6f43306 | 60 | |
sgrsn | 1:33d6c6f43306 | 61 | case UsusamaProtocol::COMMAND_STOP: |
sgrsn | 3:3625406774ec | 62 | reply_current_pose(); |
sgrsn | 1:33d6c6f43306 | 63 | break; |
sgrsn | 1:33d6c6f43306 | 64 | } |
sgrsn | 1:33d6c6f43306 | 65 | |
sgrsn | 3:3625406774ec | 66 | //pc_ -> writeData(last_command_, UsusamaProtocol::DEBUG_CONSOLE); |
sgrsn | 3:3625406774ec | 67 | |
sgrsn | 1:33d6c6f43306 | 68 | } |
sgrsn | 1:33d6c6f43306 | 69 | |
sgrsn | 1:33d6c6f43306 | 70 | void reply_commad_pose() |
sgrsn | 3:3625406774ec | 71 | { |
sgrsn | 3:3625406774ec | 72 | // 受け取った値をそのまま返す |
sgrsn | 4:fd646595de66 | 73 | switch(n_reply_commad_pose) |
sgrsn | 4:fd646595de66 | 74 | { |
sgrsn | 4:fd646595de66 | 75 | case 0: |
sgrsn | 4:fd646595de66 | 76 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_X], UsusamaProtocol::REPLY_COMMAND_X); |
sgrsn | 4:fd646595de66 | 77 | break; |
sgrsn | 4:fd646595de66 | 78 | case 1: |
sgrsn | 4:fd646595de66 | 79 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_Y], UsusamaProtocol::REPLY_COMMAND_Y); |
sgrsn | 4:fd646595de66 | 80 | break; |
sgrsn | 4:fd646595de66 | 81 | case 2: |
sgrsn | 4:fd646595de66 | 82 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA], UsusamaProtocol::REPLY_COMMAND_THETA); |
sgrsn | 4:fd646595de66 | 83 | break; |
sgrsn | 4:fd646595de66 | 84 | } |
sgrsn | 4:fd646595de66 | 85 | n_reply_commad_pose++; |
sgrsn | 4:fd646595de66 | 86 | if (n_reply_commad_pose == 3) n_reply_commad_pose = 0; |
sgrsn | 1:33d6c6f43306 | 87 | } |
sgrsn | 1:33d6c6f43306 | 88 | |
sgrsn | 3:3625406774ec | 89 | void reply_current_pose() |
sgrsn | 1:33d6c6f43306 | 90 | { |
sgrsn | 4:fd646595de66 | 91 | switch(n_reply_current_pose) |
sgrsn | 4:fd646595de66 | 92 | { |
sgrsn | 4:fd646595de66 | 93 | case 0: |
sgrsn | 4:fd646595de66 | 94 | pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.reached), UsusamaProtocol::REPLY_MOVE); |
sgrsn | 4:fd646595de66 | 95 | break; |
sgrsn | 4:fd646595de66 | 96 | case 1: |
sgrsn | 4:fd646595de66 | 97 | pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.x), UsusamaProtocol::REPLY_STATE_X); |
sgrsn | 4:fd646595de66 | 98 | break; |
sgrsn | 4:fd646595de66 | 99 | case 2: |
sgrsn | 4:fd646595de66 | 100 | pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.y), UsusamaProtocol::REPLY_STATE_Y); |
sgrsn | 4:fd646595de66 | 101 | break; |
sgrsn | 4:fd646595de66 | 102 | case 3: |
sgrsn | 4:fd646595de66 | 103 | pc_ -> writeData(UsusamaProtocol::encode_float2int(move_reply.theta), UsusamaProtocol::REPLY_STATE_THETA); |
sgrsn | 4:fd646595de66 | 104 | break; |
sgrsn | 4:fd646595de66 | 105 | } |
sgrsn | 4:fd646595de66 | 106 | n_reply_current_pose++; |
sgrsn | 4:fd646595de66 | 107 | if (n_reply_current_pose == 4) n_reply_current_pose = 0; |
sgrsn | 1:33d6c6f43306 | 108 | } |
sgrsn | 1:33d6c6f43306 | 109 | |
sgrsn | 1:33d6c6f43306 | 110 | bool is_enable_to_move() |
sgrsn | 1:33d6c6f43306 | 111 | { |
sgrsn | 1:33d6c6f43306 | 112 | return move_command.enable; |
sgrsn | 1:33d6c6f43306 | 113 | } |
sgrsn | 3:3625406774ec | 114 | bool disable_to_move() |
sgrsn | 3:3625406774ec | 115 | { |
sgrsn | 3:3625406774ec | 116 | move_command.enable = false; |
sgrsn | 3:3625406774ec | 117 | return move_command.enable; |
sgrsn | 3:3625406774ec | 118 | } |
sgrsn | 4:fd646595de66 | 119 | bool is_ref_pose_changed() |
sgrsn | 4:fd646595de66 | 120 | { |
sgrsn | 4:fd646595de66 | 121 | return cmd_pose_changed_; |
sgrsn | 4:fd646595de66 | 122 | } |
sgrsn | 4:fd646595de66 | 123 | void notify_use_ref_pose() |
sgrsn | 4:fd646595de66 | 124 | { |
sgrsn | 4:fd646595de66 | 125 | cmd_pose_changed_ = false; |
sgrsn | 4:fd646595de66 | 126 | } |
sgrsn | 3:3625406774ec | 127 | bool is_stop_called() |
sgrsn | 3:3625406774ec | 128 | { |
sgrsn | 3:3625406774ec | 129 | return stop_command.stop; |
sgrsn | 3:3625406774ec | 130 | } |
sgrsn | 3:3625406774ec | 131 | |
sgrsn | 3:3625406774ec | 132 | UsusamaProtocol::MoveCommand_t getMoveCommand() |
sgrsn | 1:33d6c6f43306 | 133 | { |
sgrsn | 1:33d6c6f43306 | 134 | return move_command; |
sgrsn | 1:33d6c6f43306 | 135 | } |
sgrsn | 1:33d6c6f43306 | 136 | |
sgrsn | 3:3625406774ec | 137 | void setReachedGoal(bool reached) |
sgrsn | 3:3625406774ec | 138 | { |
sgrsn | 3:3625406774ec | 139 | move_reply.reached = reached; |
sgrsn | 3:3625406774ec | 140 | } |
sgrsn | 1:33d6c6f43306 | 141 | |
sgrsn | 1:33d6c6f43306 | 142 | void receive_pc_process() |
sgrsn | 1:33d6c6f43306 | 143 | { |
sgrsn | 1:33d6c6f43306 | 144 | uint8_t reg = pc_ -> readData(); |
sgrsn | 1:33d6c6f43306 | 145 | |
sgrsn | 3:3625406774ec | 146 | if(reg == 0) return; |
sgrsn | 3:3625406774ec | 147 | |
sgrsn | 1:33d6c6f43306 | 148 | switch(reg) |
sgrsn | 1:33d6c6f43306 | 149 | { |
sgrsn | 1:33d6c6f43306 | 150 | case UsusamaProtocol::COMMAND_MOVE: |
sgrsn | 1:33d6c6f43306 | 151 | move_command.enable = pc_->_register[UsusamaProtocol::COMMAND_MOVE]; |
sgrsn | 1:33d6c6f43306 | 152 | break; |
sgrsn | 1:33d6c6f43306 | 153 | |
sgrsn | 1:33d6c6f43306 | 154 | case UsusamaProtocol::COMMAND_POSE_X: |
sgrsn | 2:92e2ae1b1886 | 155 | move_command.x = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_X] ); |
sgrsn | 1:33d6c6f43306 | 156 | break; |
sgrsn | 1:33d6c6f43306 | 157 | |
sgrsn | 1:33d6c6f43306 | 158 | case UsusamaProtocol::COMMAND_POSE_Y: |
sgrsn | 2:92e2ae1b1886 | 159 | move_command.y = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_Y] ); |
sgrsn | 1:33d6c6f43306 | 160 | break; |
sgrsn | 1:33d6c6f43306 | 161 | |
sgrsn | 1:33d6c6f43306 | 162 | case UsusamaProtocol::COMMAND_POSE_THETA: |
sgrsn | 2:92e2ae1b1886 | 163 | move_command.theta = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA] ); |
sgrsn | 1:33d6c6f43306 | 164 | break; |
sgrsn | 1:33d6c6f43306 | 165 | |
sgrsn | 1:33d6c6f43306 | 166 | case UsusamaProtocol::COMMAND_STOP: |
sgrsn | 1:33d6c6f43306 | 167 | stop_command.stop = pc_->_register[UsusamaProtocol::COMMAND_STOP]; |
sgrsn | 1:33d6c6f43306 | 168 | break; |
sgrsn | 1:33d6c6f43306 | 169 | } |
sgrsn | 1:33d6c6f43306 | 170 | last_command_ = reg; |
sgrsn | 1:33d6c6f43306 | 171 | } |
sgrsn | 1:33d6c6f43306 | 172 | |
sgrsn | 1:33d6c6f43306 | 173 | private: |
sgrsn | 1:33d6c6f43306 | 174 | UsusamaSerial *pc_; |
sgrsn | 1:33d6c6f43306 | 175 | uint8_t last_command_ = 0; |
sgrsn | 3:3625406774ec | 176 | bool cmd_pose_changed_ = false; |
sgrsn | 4:fd646595de66 | 177 | |
sgrsn | 4:fd646595de66 | 178 | int n_reply_current_pose = 0; |
sgrsn | 4:fd646595de66 | 179 | int n_reply_commad_pose = 0; |
sgrsn | 1:33d6c6f43306 | 180 | }; |
sgrsn | 1:33d6c6f43306 | 181 | |
sgrsn | 1:33d6c6f43306 | 182 | #endif |