hidaka sato / UsusamaSerial

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?

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 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