mirror of
https://github.com/yoboujon/dumber.git
synced 2025-06-08 22:00:49 +02:00
349 lines
8.6 KiB
C++
349 lines
8.6 KiB
C++
/*******************************************************************************
|
|
* 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;
|
|
}
|