hidaka sato / UsusamaSerial

Serial library for Ususama

Dependents:   ususama_serial_demo WRS2021_mecanum_driver

ususama_controller.hpp@1:33d6c6f43306, 2021-08-23 (annotated)

Committer:
sgrsn
Date:
Mon Aug 23 17:02:24 2021 +0000
Revision:
1:33d6c6f43306
Child:
2:92e2ae1b1886
one

Who changed what in which revision?

UserRevisionLine numberNew 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 1:33d6c6f43306 53 pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_X], UsusamaProtocol::REPLY_STATE_X);
sgrsn 1:33d6c6f43306 54 pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_Y], UsusamaProtocol::REPLY_STATE_Y);
sgrsn 1:33d6c6f43306 55 pc_ -> writeData(pc_->_register[UsusamaProtocol::COMMAND_POSE_THETA], UsusamaProtocol::REPLY_STATE_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 1:33d6c6f43306 91 move_command.x = 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 1:33d6c6f43306 95 move_command.y = 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 1:33d6c6f43306 99 move_command.theta = 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