hidaka sato / UsusamaSerial

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?

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