Serial library for Ususama
Dependents: ususama_serial_demo WRS2021_mecanum_driver
ususama_controller.hpp@2:92e2ae1b1886, 2021-08-24 (annotated)
- Committer:
- sgrsn
- Date:
- Tue Aug 24 07:05:55 2021 +0000
- Revision:
- 2:92e2ae1b1886
- Parent:
- 1:33d6c6f43306
- Child:
- 3:3625406774ec
Fix calculation of checksum
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 | 1:33d6c6f43306 | 13 | |
sgrsn | 1:33d6c6f43306 | 14 | UsusamaController(UsusamaSerial *pc) |
sgrsn | 1:33d6c6f43306 | 15 | { |
sgrsn | 1:33d6c6f43306 | 16 | pc_ = pc; |
sgrsn | 1:33d6c6f43306 | 17 | } |
sgrsn | 1:33d6c6f43306 | 18 | |
sgrsn | 1:33d6c6f43306 | 19 | void update() |
sgrsn | 1:33d6c6f43306 | 20 | { |
sgrsn | 1:33d6c6f43306 | 21 | |
sgrsn | 1:33d6c6f43306 | 22 | // stopコマンドをチェック |
sgrsn | 1:33d6c6f43306 | 23 | if(stop_command.stop > 0) |
sgrsn | 1:33d6c6f43306 | 24 | { |
sgrsn | 1:33d6c6f43306 | 25 | move_command.enable = false; |
sgrsn | 1:33d6c6f43306 | 26 | } |
sgrsn | 1:33d6c6f43306 | 27 | } |
sgrsn | 1:33d6c6f43306 | 28 | |
sgrsn | 1:33d6c6f43306 | 29 | void write_robot_state(float state_x, float state_y, float state_theta) |
sgrsn | 1:33d6c6f43306 | 30 | { |
sgrsn | 1:33d6c6f43306 | 31 | // 送られてきたコマンドによって何を送るか決める |
sgrsn | 1:33d6c6f43306 | 32 | switch(last_command_) |
sgrsn | 1:33d6c6f43306 | 33 | { |
sgrsn | 1:33d6c6f43306 | 34 | case UsusamaProtocol::COMMAND_MOVE: |
sgrsn | 1:33d6c6f43306 | 35 | reply_current_pose(state_x, state_y, state_theta); |
sgrsn | 1:33d6c6f43306 | 36 | break; |
sgrsn | 1:33d6c6f43306 | 37 | |
sgrsn | 1:33d6c6f43306 | 38 | case UsusamaProtocol::COMMAND_POSE_X: |
sgrsn | 1:33d6c6f43306 | 39 | case UsusamaProtocol::COMMAND_POSE_Y: |
sgrsn | 1:33d6c6f43306 | 40 | case UsusamaProtocol::COMMAND_POSE_THETA: |
sgrsn | 1:33d6c6f43306 | 41 | reply_commad_pose(); |
sgrsn | 1:33d6c6f43306 | 42 | break; |
sgrsn | 1:33d6c6f43306 | 43 | |
sgrsn | 1:33d6c6f43306 | 44 | case UsusamaProtocol::COMMAND_STOP: |
sgrsn | 1:33d6c6f43306 | 45 | reply_current_pose(state_x, state_y, state_theta); |
sgrsn | 1:33d6c6f43306 | 46 | break; |
sgrsn | 1:33d6c6f43306 | 47 | } |
sgrsn | 1:33d6c6f43306 | 48 | |
sgrsn | 1:33d6c6f43306 | 49 | } |
sgrsn | 1:33d6c6f43306 | 50 | |
sgrsn | 1:33d6c6f43306 | 51 | void reply_commad_pose() |
sgrsn | 1:33d6c6f43306 | 52 | { |
sgrsn | 2:92e2ae1b1886 | 53 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_X], UsusamaProtocol::REPLY_COMMAND_X); |
sgrsn | 2:92e2ae1b1886 | 54 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_Y], UsusamaProtocol::REPLY_COMMAND_Y); |
sgrsn | 2:92e2ae1b1886 | 55 | pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA], UsusamaProtocol::REPLY_COMMAND_THETA); |
sgrsn | 1:33d6c6f43306 | 56 | } |
sgrsn | 1:33d6c6f43306 | 57 | |
sgrsn | 1:33d6c6f43306 | 58 | void reply_current_pose(float x, float y, float theta) |
sgrsn | 1:33d6c6f43306 | 59 | { |
sgrsn | 1:33d6c6f43306 | 60 | int x_int = UsusamaProtocol::encode_float2int( x ); |
sgrsn | 1:33d6c6f43306 | 61 | int y_int = UsusamaProtocol::encode_float2int( y ); |
sgrsn | 1:33d6c6f43306 | 62 | int theta_int = UsusamaProtocol::encode_float2int( theta ); |
sgrsn | 1:33d6c6f43306 | 63 | |
sgrsn | 1:33d6c6f43306 | 64 | pc_ -> writeData(x_int, UsusamaProtocol::REPLY_STATE_X); |
sgrsn | 1:33d6c6f43306 | 65 | pc_ -> writeData(y_int, UsusamaProtocol::REPLY_STATE_Y); |
sgrsn | 1:33d6c6f43306 | 66 | pc_ -> writeData(theta_int, UsusamaProtocol::REPLY_STATE_THETA); |
sgrsn | 1:33d6c6f43306 | 67 | } |
sgrsn | 1:33d6c6f43306 | 68 | |
sgrsn | 1:33d6c6f43306 | 69 | bool is_enable_to_move() |
sgrsn | 1:33d6c6f43306 | 70 | { |
sgrsn | 1:33d6c6f43306 | 71 | return move_command.enable; |
sgrsn | 1:33d6c6f43306 | 72 | } |
sgrsn | 1:33d6c6f43306 | 73 | |
sgrsn | 1:33d6c6f43306 | 74 | UsusamaProtocol::MoveCommand_t getReferencePose() |
sgrsn | 1:33d6c6f43306 | 75 | { |
sgrsn | 1:33d6c6f43306 | 76 | return move_command; |
sgrsn | 1:33d6c6f43306 | 77 | } |
sgrsn | 1:33d6c6f43306 | 78 | |
sgrsn | 1:33d6c6f43306 | 79 | |
sgrsn | 1:33d6c6f43306 | 80 | void receive_pc_process() |
sgrsn | 1:33d6c6f43306 | 81 | { |
sgrsn | 1:33d6c6f43306 | 82 | uint8_t reg = pc_ -> readData(); |
sgrsn | 1:33d6c6f43306 | 83 | |
sgrsn | 1:33d6c6f43306 | 84 | switch(reg) |
sgrsn | 1:33d6c6f43306 | 85 | { |
sgrsn | 1:33d6c6f43306 | 86 | case UsusamaProtocol::COMMAND_MOVE: |
sgrsn | 1:33d6c6f43306 | 87 | move_command.enable = pc_->_register[UsusamaProtocol::COMMAND_MOVE]; |
sgrsn | 1:33d6c6f43306 | 88 | break; |
sgrsn | 1:33d6c6f43306 | 89 | |
sgrsn | 1:33d6c6f43306 | 90 | case UsusamaProtocol::COMMAND_POSE_X: |
sgrsn | 2:92e2ae1b1886 | 91 | move_command.x = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_X] ); |
sgrsn | 1:33d6c6f43306 | 92 | break; |
sgrsn | 1:33d6c6f43306 | 93 | |
sgrsn | 1:33d6c6f43306 | 94 | case UsusamaProtocol::COMMAND_POSE_Y: |
sgrsn | 2:92e2ae1b1886 | 95 | move_command.y = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_Y] ); |
sgrsn | 1:33d6c6f43306 | 96 | break; |
sgrsn | 1:33d6c6f43306 | 97 | |
sgrsn | 1:33d6c6f43306 | 98 | case UsusamaProtocol::COMMAND_POSE_THETA: |
sgrsn | 2:92e2ae1b1886 | 99 | move_command.theta = UsusamaProtocol::decode_int2float( pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA] ); |
sgrsn | 1:33d6c6f43306 | 100 | break; |
sgrsn | 1:33d6c6f43306 | 101 | |
sgrsn | 1:33d6c6f43306 | 102 | case UsusamaProtocol::COMMAND_STOP: |
sgrsn | 1:33d6c6f43306 | 103 | stop_command.stop = pc_->_register[UsusamaProtocol::COMMAND_STOP]; |
sgrsn | 1:33d6c6f43306 | 104 | break; |
sgrsn | 1:33d6c6f43306 | 105 | } |
sgrsn | 1:33d6c6f43306 | 106 | |
sgrsn | 1:33d6c6f43306 | 107 | last_command_ = reg; |
sgrsn | 1:33d6c6f43306 | 108 | } |
sgrsn | 1:33d6c6f43306 | 109 | |
sgrsn | 1:33d6c6f43306 | 110 | private: |
sgrsn | 1:33d6c6f43306 | 111 | UsusamaSerial *pc_; |
sgrsn | 1:33d6c6f43306 | 112 | uint8_t last_command_ = 0; |
sgrsn | 1:33d6c6f43306 | 113 | }; |
sgrsn | 1:33d6c6f43306 | 114 | |
sgrsn | 1:33d6c6f43306 | 115 | #endif |