No Description
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

comrobot.cpp 11KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359
  1. /*
  2. * Copyright (C) 2018 dimercur
  3. *
  4. * This program is free software: you can redistribute it and/or modify
  5. * it under the terms of the GNU General Public License as published by
  6. * the Free Software Foundation, either version 3 of the License, or
  7. * (at your option) any later version.
  8. *
  9. * This program is distributed in the hope that it will be useful,
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. * GNU General Public License for more details.
  13. *
  14. * You should have received a copy of the GNU General Public License
  15. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  16. */
  17. #include "comrobot.h"
  18. #include <stdio.h>
  19. #include <stdlib.h>
  20. #include <unistd.h>
  21. #include <fcntl.h>
  22. #include <termios.h>
  23. #include <string>
  24. #include <stdexcept>
  25. #ifdef __FOR_PC__
  26. #define USART_FILENAME "/dev/ttyUSB0"
  27. #else
  28. #define USART_FILENAME "/dev/ttyS0"
  29. #endif /* __FOR_PC__ */
  30. /*
  31. * Constants to be used for communicating with robot. Contains command tag
  32. */
  33. const char LABEL_ROBOT_PING = 'p';
  34. const char LABEL_ROBOT_RESET = 'r';
  35. const char LABEL_ROBOT_START_WITH_WD = 'W';
  36. const char LABEL_ROBOT_START_WITHOUT_WD = 'u';
  37. const char LABEL_ROBOT_RELOAD_WD = 'w';
  38. const char LABEL_ROBOT_MOVE = 'M';
  39. const char LABEL_ROBOT_TURN = 'T';
  40. const char LABEL_ROBOT_GET_BATTERY = 'v';
  41. const char LABEL_ROBOT_GET_STATE = 'b';
  42. const char LABEL_ROBOT_POWEROFF = 'z';
  43. const char LABEL_ROBOT_OK = 'O';
  44. const char LABEL_ROBOT_ERROR = 'E';
  45. const char LABEL_ROBOT_UNKNOWN_COMMAND = 'C';
  46. const char LABEL_ROBOT_SEPARATOR_CHAR = '=';
  47. const char LABEL_ROBOT_ENDING_CHAR = 0x0D; // carriage return (\\r)
  48. /**
  49. * Open serial link with robot
  50. * @return File descriptor
  51. * @throw std::runtime_error if it fails
  52. */
  53. int ComRobot::Open() {
  54. return this->Open(USART_FILENAME);
  55. }
  56. /**
  57. * Open serial link with robot
  58. * @param usart Filename of usart to open
  59. * @return File descriptor
  60. * @throw std::runtime_error if it fails
  61. */
  62. int ComRobot::Open(string usart) {
  63. struct termios options;
  64. fd = open(usart.c_str(), O_RDWR | O_NOCTTY /*| O_NDELAY*/); //Open in blocking read/write mode
  65. if (fd == -1) {
  66. cerr << "[" << __PRETTY_FUNCTION__ << "] Unable to open UART (" << usart << "). Ensure it is not in use by another application" << endl << flush;
  67. throw std::runtime_error{"Unable to open UART"};
  68. exit(EXIT_FAILURE);
  69. } else {
  70. fcntl(fd, F_SETFL, 0);
  71. tcgetattr(fd, &options);
  72. options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  73. cfsetospeed(&options, B9600);
  74. cfsetispeed(&options, B9600);
  75. options.c_cc[VMIN] = 0;
  76. options.c_cc[VTIME] = 1; /* Timeout of 100 ms per character */
  77. tcsetattr(fd, TCSANOW, &options);
  78. }
  79. return fd;
  80. }
  81. /**
  82. * Close serial link
  83. * @return Success if above 0, failure if below 0
  84. */
  85. int ComRobot::Close() {
  86. return close(fd);
  87. }
  88. /**
  89. * Send a message to robot
  90. * @param msg Message to send to robot
  91. * @return 1 if success, 0 otherwise
  92. * @attention Message is destroyed (delete) after being sent. You do not need to delete it yourself
  93. * @attention Write is blocking until message is written into buffer (linux side)
  94. * @warning Write is not thread save : check that multiple tasks can't access this method simultaneously
  95. */
  96. Message *ComRobot::Write(Message* msg) {
  97. Message *msgAnswer;
  98. string s;
  99. if (this->fd != -1) {
  100. Write_Pre();
  101. s = MessageToString(msg);
  102. AddChecksum(s);
  103. //cout << "[" <<__PRETTY_FUNCTION__<<"] Send command: "<<s<<endl<<flush;
  104. int count = write(this->fd, s.c_str(), s.length()); //Filestream, bytes to write, number of bytes to write
  105. if (count < 0) {
  106. cerr << "[" << __PRETTY_FUNCTION__ << "] UART TX error (" << to_string(count) << ")" << endl << flush;
  107. msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR);
  108. } else { /* write successfull, read answer from robot */
  109. try {
  110. s = Read();
  111. //cout << "Answer = "<<s<<endl<<flush;
  112. if (VerifyChecksum(s)) {
  113. msgAnswer = StringToMessage(s);
  114. } else msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND);
  115. } catch (std::runtime_error &e) {
  116. s = string(e.what());
  117. if (s.find("imeout")) { // timeout detecté
  118. msgAnswer = new Message(MESSAGE_ANSWER_ROBOT_TIMEOUT);
  119. } else {
  120. msgAnswer = new Message(MESSAGE_ANSWER_COM_ERROR);
  121. }
  122. }
  123. }
  124. } else {
  125. cerr << __PRETTY_FUNCTION__ << ": Com port not open" << endl << flush;
  126. throw std::runtime_error{"Com port not open"};
  127. }
  128. // deallocation of msg
  129. delete(msg);
  130. return msgAnswer;
  131. }
  132. /**
  133. * Get a message from robot
  134. * @return Message currently received
  135. * @attention A message object is created (new) when receiving data from robot. You MUST remember to destroy is (delete) after use
  136. * @attention Read method is blocking until a message is received
  137. * @warning Read is not thread safe : Do not call it in multiple tasks simultaneously
  138. */
  139. string ComRobot::Read() {
  140. string s;
  141. int rxLength;
  142. unsigned char receivedChar;
  143. do {
  144. rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max)
  145. if (rxLength == 0) { // timeout
  146. // try again
  147. rxLength = read(this->fd, (void*) &receivedChar, 1); //Filestream, buffer to store in, number of bytes to read (max)
  148. if (rxLength == 0) { // re-timeout: it sucks !
  149. throw std::runtime_error{"ComRobot::Read: Timeout when reading from com port"};
  150. }
  151. } else if (rxLength < 0) { // big pb !
  152. throw std::runtime_error{"ComRobot::Read: Unknown problem when reading from com port"};
  153. } else { // everything ok
  154. if ((receivedChar != '\r') && (receivedChar != '\n')) s += receivedChar;
  155. }
  156. } while ((receivedChar != '\r') && (receivedChar != '\n'));
  157. return s;
  158. }
  159. Message *ComRobot::SendCommand(Message* msg, MessageID answerID, int maxRetries) {
  160. int counter = maxRetries;
  161. Message *msgSend;
  162. Message *msgRcv;
  163. Message *msgTmp;
  164. do {
  165. msgSend = msg->Copy();
  166. cout << "S => " << msgSend->ToString() << endl << flush;
  167. msgTmp = Write(msgSend);
  168. cout << "R <= " << msgTmp->ToString() << endl << flush;
  169. if (msgTmp->CompareID(answerID)) counter = 0;
  170. else counter--;
  171. if (counter == 0) msgRcv = msgTmp->Copy();
  172. delete(msgTmp);
  173. } while (counter);
  174. delete (msg);
  175. return msgRcv;
  176. }
  177. /**
  178. * Convert an array of char to its message representation (when receiving data from stm32)
  179. * @param bytes Array of char
  180. * @return Message corresponding to received array of char
  181. */
  182. Message* ComRobot::StringToMessage(string s) {
  183. Message *msg;
  184. switch (s[0]) {
  185. case LABEL_ROBOT_OK:
  186. msg = new Message(MESSAGE_ANSWER_ACK);
  187. break;
  188. case LABEL_ROBOT_ERROR:
  189. msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR);
  190. break;
  191. case LABEL_ROBOT_UNKNOWN_COMMAND:
  192. msg = new Message(MESSAGE_ANSWER_ROBOT_UNKNOWN_COMMAND);
  193. break;
  194. case '0':
  195. msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_EMPTY);
  196. break;
  197. case '1':
  198. msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_LOW);
  199. break;
  200. case '2':
  201. msg = new MessageBattery(MESSAGE_ROBOT_BATTERY_LEVEL, BATTERY_FULL);
  202. break;
  203. default:
  204. msg = new Message(MESSAGE_ANSWER_ROBOT_ERROR);
  205. cerr << "[" << __PRETTY_FUNCTION__ << "] Unknown message received from robot (" << s << ")" << endl << flush;
  206. }
  207. return msg;
  208. }
  209. /**
  210. * Convert a message to its array of char representation (for sending command to stm32)
  211. * @param msg Message to be sent to robot
  212. * @param buffer Array of char, image of message to send
  213. */
  214. string ComRobot::MessageToString(Message *msg) {
  215. string s;
  216. float val_f;
  217. int val_i;
  218. unsigned char *b;
  219. switch (msg->GetID()) {
  220. case MESSAGE_ROBOT_PING:
  221. s += LABEL_ROBOT_PING;
  222. break;
  223. case MESSAGE_ROBOT_RESET:
  224. s += LABEL_ROBOT_RESET;
  225. break;
  226. case MESSAGE_ROBOT_POWEROFF:
  227. s += LABEL_ROBOT_POWEROFF;
  228. break;
  229. case MESSAGE_ROBOT_START_WITHOUT_WD:
  230. s += LABEL_ROBOT_START_WITHOUT_WD;
  231. break;
  232. case MESSAGE_ROBOT_START_WITH_WD:
  233. s += LABEL_ROBOT_START_WITH_WD;
  234. break;
  235. case MESSAGE_ROBOT_RELOAD_WD:
  236. s += LABEL_ROBOT_RELOAD_WD;
  237. break;
  238. case MESSAGE_ROBOT_BATTERY_GET:
  239. s += LABEL_ROBOT_GET_BATTERY;
  240. break;
  241. case MESSAGE_ROBOT_STATE_GET:
  242. s += LABEL_ROBOT_GET_STATE;
  243. break;
  244. case MESSAGE_ROBOT_GO_FORWARD:
  245. s += LABEL_ROBOT_MOVE;
  246. s += LABEL_ROBOT_SEPARATOR_CHAR;
  247. s.append(to_string(500000));
  248. break;
  249. case MESSAGE_ROBOT_GO_BACKWARD:
  250. s += LABEL_ROBOT_MOVE;
  251. s += LABEL_ROBOT_SEPARATOR_CHAR;
  252. s.append(to_string(-500000));
  253. break;
  254. case MESSAGE_ROBOT_GO_LEFT:
  255. s += LABEL_ROBOT_TURN;
  256. s += LABEL_ROBOT_SEPARATOR_CHAR;
  257. s.append(to_string(-500000));
  258. break;
  259. case MESSAGE_ROBOT_GO_RIGHT:
  260. s += LABEL_ROBOT_TURN;
  261. s += LABEL_ROBOT_SEPARATOR_CHAR;
  262. s.append(to_string(500000));
  263. break;
  264. case MESSAGE_ROBOT_STOP:
  265. s += LABEL_ROBOT_MOVE;
  266. s += LABEL_ROBOT_SEPARATOR_CHAR;
  267. s.append(to_string(0));
  268. break;
  269. case MESSAGE_ROBOT_MOVE:
  270. s += LABEL_ROBOT_MOVE;
  271. s += LABEL_ROBOT_SEPARATOR_CHAR;
  272. s.append(to_string(((MessageInt*) msg)->GetValue()));
  273. break;
  274. case MESSAGE_ROBOT_TURN:
  275. s += LABEL_ROBOT_TURN;
  276. s += LABEL_ROBOT_SEPARATOR_CHAR;
  277. s.append(to_string(((MessageInt*) msg)->GetValue()));
  278. break;
  279. default:
  280. cerr << "[" << __PRETTY_FUNCTION__ << "] Invalid message for robot (" << msg->ToString() << ")" << endl << flush;
  281. throw std::runtime_error{"Invalid message"};
  282. }
  283. return s;
  284. }
  285. /**
  286. * Add a checksum and carriage return to a command string
  287. * @param[in,out] s String containing command for robot, without ending char (carriage return)
  288. */
  289. void ComRobot::AddChecksum(string &s) {
  290. unsigned char checksum = 0;
  291. for (string::iterator it = s.begin(); it != s.end(); ++it) {
  292. checksum ^= (unsigned char) *it;
  293. }
  294. s += (char) checksum; // Add calculated checksum
  295. s += (char) LABEL_ROBOT_ENDING_CHAR;
  296. }
  297. /**
  298. * Verify if checksum of an incoming answer from robot is valid,
  299. * then remove checksum from incoming answer (if checksum is ok)
  300. * @param[in,out] s String containing incoming answer from robot
  301. * @return true is checksum is valid, false otherwise.
  302. */
  303. bool ComRobot::VerifyChecksum(string &s) {
  304. unsigned char checksum = 0;
  305. for (string::iterator it = s.begin(); it != s.end(); ++it) {
  306. checksum ^= (unsigned char) *it;
  307. }
  308. if (checksum == 0) { // checksum is ok, remove last char of string (checksum)
  309. s.pop_back(); // remove last char
  310. return true;
  311. } else return false;
  312. }