123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349 |
- /*******************************************************************************
- * Copyright (c) 2018 INSA - GEI, Toulouse, France.
- * All rights reserved. This program and the accompanying materials
- * are made available "AS IS", without any warranty of any kind.
- *
- * INSA assumes no responsibility for errors or omissions in the
- * software or documentation available.
- *
- * Contributors:
- * Lucien Senaneuch - Initial API and implementation
- * Sebastien DI MERCURIO - Maintainer since Octobre 2018
- *******************************************************************************/
-
- /**
- * \file robot.cpp
- * \author L.Senaneuch
- * \version 1.0
- * \date 06/06/2017
- * \brief Fonction permettant la communication avec le robot.
- *
- * \details Ce fichier regroupe des fonctions facilitant la communication avec le robot en utilisant le port serie USART
- */
-
- #include "Robot.h"
-
- #include <unistd.h>
- #include <fcntl.h>
- #include <termios.h>
-
- #include <stdlib.h>
- #include "definitions.h"
-
- using namespace std;
-
- #define ROBOT_CMD_ENDING_CHAR '\r'
-
- #define ROBOT_CMD_PING 'p'
- #define ROBOT_CMD_RESET 'r'
- #define ROBOT_CMD_START_WITHOUT_WD 'u'
- #define ROBOT_CMD_START_WITH_WD 'W'
- #define ROBOT_CMD_RELOAD_WD 'w'
- #define ROBOT_CMD_GET_VBAT 'v'
- #define ROBOT_CMD_IS_BUSY 'b'
- #define ROBOT_CMD_MOVE 'M'
- #define ROBOT_CMD_TURN 'T'
- #define ROBOT_CMD_GET_VERSION 'V'
- #define ROBOT_CMD_POWER_OFF 'z'
-
- #define ROBOT_CMD_OK_ANS "O"
- #define ROBOT_CMD_ERR_ANS "E"
- #define ROBOT_CMD_UNKNOW_ANS "C"
-
- const string Robot::InvalidAnswerException = "Invalid answer exception";
-
- Robot::Robot() {
-
- }
-
- Robot::~Robot() {
- this->Close();
- }
-
- char Robot::Open() {
- return Serial::Open(DEFAULT_SERIAL_PORT,9600);
- }
-
- char Robot::Open(const char *path) {
- return Serial::Open(path,9600);
- }
-
- char Robot::Close() {
- return Serial::Close();
- }
-
- int Robot::GetLastCommandStatus() {
- return this->lastCommandStatus;
- }
-
- void Robot::Ping() {
- #ifndef __STUB__
- string ans;
- SendCommand(string(1,ROBOT_CMD_PING), &ans);
-
- CheckAnswer(ans);
- #else
- return SUCCESS;
- #endif /* __STUB__ */
- }
-
- void Robot::Reset() {
- #ifndef __STUB__
- string ans;
- SendCommand(string(1,ROBOT_CMD_RESET), &ans);
-
- CheckAnswer(ans);
- #else
- return SUCCESS;
- #endif /* __STUB__ */
- }
-
- void Robot::StartWithoutWatchdog() {
- #ifndef __STUB__
- string ans;
- SendCommand(string(1,ROBOT_CMD_START_WITHOUT_WD), &ans);
-
- CheckAnswer(ans);
- #else
- return SUCCESS;
- #endif /* __STUB__ */
- }
-
- void Robot::StartWithWatchdog() {
- #ifndef __STUB__
- string ans;
- SendCommand(string(1,ROBOT_CMD_START_WITH_WD), &ans);
-
- CheckAnswer(ans);
- #else
- return SUCCESS;
- #endif /* __STUB__ */
- }
-
- void Robot::ResetWatchdog() {
- #ifndef __STUB__
- string ans;
- SendCommand(string(1,ROBOT_CMD_RELOAD_WD), &ans);
-
- CheckAnswer(ans);
- #else
- return SUCCESS;
- #endif /* __STUB__ */
- }
-
- void Robot::Move(int distance) {
- #ifndef __STUB__
- string ans;
- SendCommand(string(1,ROBOT_CMD_MOVE) + "="+to_string(distance), &ans);
-
- CheckAnswer(ans);
- #else
- return SUCCESS;
- #endif /* __STUB__ */
- }
-
- void Robot::Turn(int angle) {
- #ifndef __STUB__
- string ans;
- SendCommand(string(1,ROBOT_CMD_TURN) + "="+to_string(angle), &ans);
-
- CheckAnswer(ans);
- #else
- return SUCCESS;
- #endif /* __STUB__ */
- }
-
- void Robot::PowerOff() {
- #ifndef __STUB__
- string ans;
- SendCommand(string(1,ROBOT_CMD_POWER_OFF), &ans);
-
- CheckAnswer(ans);
- #else
- return SUCCESS;
- #endif /* __STUB__ */
- }
-
- char Robot::GetBatteryLevel() {
- #ifndef __STUB__
- string ans;
-
- SendCommand(string(1,ROBOT_CMD_GET_VBAT), &ans);
- lastCommandStatus=SUCCESS;
-
- if (ans.length()==1) {
- if ((ans[0] != '2') && (ans[0] != '1') && (ans[0] != '0')) {
- lastCommandStatus=INVALID_ANSWER;
- throw (InvalidAnswerException + " raised in Robot::GetBatteryLevel. Invalid battery value (" + ans[0] +")\n");
- }
- } else {
- lastCommandStatus=INVALID_ANSWER;
- throw (InvalidAnswerException + " raised in Robot::GetBatteryLevel. Invalid answer length (" + to_string(ans.length())+")\n");
- }
-
- return ans[0];
- #else
- return BATTERY_FULL;
- #endif /* __STUB__ */
- }
-
- string Robot::GetVersion() {
- #ifndef __STUB__
- string ans;
-
- SendCommand(string(1,ROBOT_CMD_GET_VERSION), &ans);
- lastCommandStatus=SUCCESS;
-
- if (ans.find("version")== string::npos) {
- lastCommandStatus=INVALID_ANSWER;
- throw (InvalidAnswerException + " raised in Robot::GetVersion. Invalid version (" + ans +")\n");
- }
-
- return ans;
- #else
- return "1.3";
- #endif /* __STUB__ */
- }
-
- bool Robot::IsBusy() {
- #ifndef __STUB__
- string ans;
-
- SendCommand(string(1,ROBOT_CMD_IS_BUSY), &ans);
- lastCommandStatus=SUCCESS;
-
- if (ans.length()!=1) {
- lastCommandStatus=INVALID_ANSWER;
- throw (InvalidAnswerException + " raised in Robot::GetBatteryLevel. Invalid answer length (" + to_string(ans.length())+")\n");
- }
-
- if (ans[0]=='1')
- return true;
- else if (ans[0]=='0')
- return false;
- else {
- lastCommandStatus=INVALID_ANSWER;
- throw (InvalidAnswerException + " raised in Robot::IsBusy. Invalid value (" + ans[0] +")\n");
- }
- #else
- return false;
- #endif /* __STUB__ */
- }
-
- void Robot::CommunicationProlog() {}
- void Robot::CommunicationEpilog() {}
-
- /****************************/
- /* PRIVATE */
- /****************************/
-
- char Robot::SendCommand(string cmd, string *ans) {
- string commandString;
- commandString = this->AddChecksum(cmd);
-
- this->CommunicationProlog(); // action avant le debut de la commande
-
- ssize_t status = Send(commandString);
-
- if (status == (ssize_t)commandString.length()) {
- // Recuperation de la reponse
- string commandResponse;
- vector<char> endingChars;
-
- endingChars.push_back('\r');
- try {
- commandResponse = Serial::Receive(endingChars,30);
- } catch (string e) {
- if (e.find("Timeout")!= string::npos)
- lastCommandStatus= TIMEOUT_COMMAND;
- else
- lastCommandStatus=INVALID_ANSWER;
-
- if (lastCommandStatus == TIMEOUT_COMMAND)
- throw (Serial::TimeoutException + " raised in Robot::SendCommand. Timeout while receiving answer from robot.\n");
- else
- throw (e + " raised in Robot::SendCommand.\n");
- }
-
- this->CommunicationEpilog(); // Action a faire apres la commande
-
- if (ValidateChecksum(&commandResponse, commandResponse)) {
- ans->assign(commandResponse);
- } else {
- lastCommandStatus = INVALID_ANSWER;
- throw InvalidAnswerException;
- }
-
- lastCommandStatus=SUCCESS;
- } else {
- lastCommandStatus=INVALID_COMMUNICATION_PORT;
-
- throw IOErrorException;
- }
-
- return lastCommandStatus;
- }
-
- void Robot::CheckAnswer(string ans) {
-
- if (ans.length()==1)
- {
- switch (ans.at(0)) {
- case 'o':
- case 'O':
- lastCommandStatus=SUCCESS;
- break;
- case 'e':
- case 'E':
- lastCommandStatus=REJECTED_COMMAND;
- break;
- case 'c':
- case 'C':
- lastCommandStatus=INVALID_COMMAND;
- break;
- default:
- lastCommandStatus = INVALID_ANSWER;
- throw InvalidAnswerException;
- }
- } else {
- lastCommandStatus = INVALID_ANSWER;
- throw InvalidAnswerException;
- }
- }
-
- string Robot::AddChecksum(string cmd) {
- string commandWithChecksum(cmd);
- char checksum=0;
-
- commandWithChecksum.resize(cmd.length()+2, ' ');
-
- for (size_t i=0; i<commandWithChecksum.length()-2; i++) {
- checksum = checksum^commandWithChecksum.at(i);
- }
-
- commandWithChecksum.at(commandWithChecksum.length()-2) = checksum;
- commandWithChecksum.at(commandWithChecksum.length()-1) = '\r';
-
- return commandWithChecksum;
- }
-
- bool Robot::ValidateChecksum(string *answerWithoutChecksum, string answer) {
- string localAnswer(answer, 0, answer.length()-1); // recopie de la chaine initiale, sans le dernier caractere (checksum)
- char checksum=0;
- bool status=false;
-
- if (answer.length()<2) // trop court pour contenir un checksum
- throw InvalidAnswerException;
- else {
- for (size_t i=0; i< answer.length()-1; i++) {
- checksum = checksum^answer.at(i);
- }
-
- if (answer.at(answer.length()-1) == checksum) {
- status=true;
- }
- }
-
- answerWithoutChecksum->assign(localAnswer);
- return status;
- }
|