robot.cpp
Go to the documentation of this file.
/*
* Copyright (C) 2018 dimercur
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "robot.h"
int fd;
int getChar(char * c);
int readSerial(char * msg);
char checkSumGO(char * msg);
int receiveMsg(void);
int sendCmd(char cmd, const char * arg);
int open_communication_robot(const char * path)
{
#ifndef __STUB__
struct termios options;
fd = open(path, O_RDWR | O_NOCTTY | O_NDELAY);
if(fd !=-1)
{
fcntl(fd, F_SETFL, 0);
tcgetattr(fd, &options);
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
cfsetospeed (&options, B9600);
cfsetispeed (&options, B9600);
options.c_cc[VMIN]=0;
options.c_cc[VTIME]=0;
tcsetattr(fd, TCSANOW, &options);
return 0;
}
else
{
perror("can't openSerial");
return -1;
}
#else
return 0;
#endif
}
int close_communication_robot(void)
{
#ifndef __STUB__
return close(fd);
#else
return 0;
#endif
}
int send_command_to_robot(char cmd, const char * arg)
{
#ifndef __STUB__
sendCmd(cmd,arg);
// TODO : check return from sendCmd
return receiveMsg();
#else
int reponse;
switch(cmd)
{
case DMB_PING:
reponse = 0;
break;
case DMB_IDLE:
reponse = 0;
break;
case DMB_START_WITH_WD:
reponse = 0;
break;
case DMB_RELOAD_WD:
reponse = 0;
break;
case DMB_GET_VBAT:
reponse = 2;
break;
case DMB_IS_BUSY:
reponse = 1;
break;
case DMB_START_WITHOUT_WD:
reponse = 0;
break;
case DMB_MOVE:
reponse = 0;
break;
case DMB_TURN:
reponse = 0;
break;
case DMB_GO_FORWARD:
reponse = 0;
break;
case DMB_GO_BACK:
reponse = 0;
break;
case DMB_GO_LEFT:
reponse = 0;
break;
case DMB_GO_RIGHT:
reponse = 0;
break;
case DMB_STOP_MOVE:
reponse = 0;
break;
default:
reponse = 0;
break;
}
return reponse;
#endif
}
/****************************/
/* PRIVATE */
/****************************/
int sendCmd(char cmd, const char * arg)
{
char cmdWithArg[20]={};
cmdWithArg[0]=cmd;
switch(cmd)
{
case DMB_GO_FORWARD: strcpy(cmdWithArg,"M=+64000");
break;
case DMB_GO_BACK: strcpy(cmdWithArg,"M=-64000");
break;
case DMB_GO_LEFT: strcpy(cmdWithArg,"T=+64000");
break;
case DMB_GO_RIGHT: strcpy(cmdWithArg,"T=-64000");
break;
case DMB_STOP_MOVE: strcpy(cmdWithArg,"M=0");
break;
case DMB_MOVE: strcat(cmdWithArg,"=");
strcat(cmdWithArg,arg);
break;
case DMB_TURN: strcat(cmdWithArg,"=");
strcat(cmdWithArg,arg);
break;
}
int sizeCmd = strlen(cmdWithArg);
cmdWithArg[sizeCmd] = checkSumGO(cmdWithArg);
cmdWithArg[sizeCmd+1] = '\r';
cmdWithArg[sizeCmd+2] = '\0';
return write(fd,cmdWithArg,strlen(cmdWithArg));
}
int receiveMsg(void)
{
char msg[20];
int b;
if((b = readSerial(msg))!=ROBOT_TIMED_OUT)
{
int taille = strlen(msg);
char checksum = msg[taille-2];
msg[taille-1] = 0;
msg[taille-2] = 0;
if(checksum!=checkSumGO(msg))
{
return ROBOT_CHECKSUM;
}
else
{
switch(msg[0])
{
case 'O' : return 0;
case 'E' : return ROBOT_ERROR;
case 'C' : return ROBOT_UKNOWN_CMD;
default : return atoi(&msg[0]);
}
}
}
else
{
return ROBOT_TIMED_OUT;
}
}
int getChar(char * c)
{
int n =0;
int delay =0;
while((n=read(fd,c,1)) <=0)
{
usleep(5000);
delay++;
if(delay > 10)
{
return ROBOT_TIMED_OUT;
}
}
return n;
}
int readSerial(char * msg)
{
char car=0;
int i=0;
for(int j = 0 ; j < 20 ; j++)
msg[j]=0;
while(car !='\r' && car!='\n') {
if(i>=20)
return -5;
if(getChar(&car)==ROBOT_TIMED_OUT) {
return ROBOT_TIMED_OUT;
}
msg[i] = car;
i++;
}
return i;
}
char checkSumGO(char * msg)
{
char resultat = 0;
int i = 0;
int taille = strlen(msg);
for(i=0;i<taille;i++)
{
resultat^=msg[i];
}
return resultat;
}
DMB_PING
#define DMB_PING
Definition: definitions.h:33
DMB_RELOAD_WD
#define DMB_RELOAD_WD
Definition: definitions.h:37
DMB_GO_LEFT
#define DMB_GO_LEFT
Definition: definitions.h:44
DMB_START_WITH_WD
#define DMB_START_WITH_WD
Definition: definitions.h:36
DMB_GO_BACK
#define DMB_GO_BACK
Definition: definitions.h:43
robot.h
Fonctions for communicating with robot.
checkSumGO
char checkSumGO(char *msg)
Definition: robot.cpp:237
readSerial
int readSerial(char *msg)
Definition: robot.cpp:216
ROBOT_ERROR
#define ROBOT_ERROR
Definition: definitions.h:50
ROBOT_CHECKSUM
#define ROBOT_CHECKSUM
Definition: definitions.h:51
DMB_START_WITHOUT_WD
#define DMB_START_WITHOUT_WD
Definition: definitions.h:35
receiveMsg
int receiveMsg(void)
Definition: robot.cpp:168
getChar
int getChar(char *c)
Definition: robot.cpp:199
ROBOT_UKNOWN_CMD
#define ROBOT_UKNOWN_CMD
Definition: definitions.h:49
DMB_IDLE
#define DMB_IDLE
Definition: definitions.h:34
DMB_GO_RIGHT
#define DMB_GO_RIGHT
Definition: definitions.h:45
sendCmd
int sendCmd(char cmd, const char *arg)
Definition: robot.cpp:138
DMB_IS_BUSY
#define DMB_IS_BUSY
Definition: definitions.h:39
DMB_GET_VBAT
#define DMB_GET_VBAT
Definition: definitions.h:38
ROBOT_TIMED_OUT
#define ROBOT_TIMED_OUT
Definition: definitions.h:48
DMB_GO_FORWARD
#define DMB_GO_FORWARD
Definition: definitions.h:42
send_command_to_robot
int send_command_to_robot(char cmd, const char *arg)
Envoi une commande au robot et attends sa réponse.
Definition: robot.cpp:74
fd
int fd
Definition: robot.cpp:28
open_communication_robot
int open_communication_robot(const char *path)
Ouvre la communication avec le robot.
Definition: robot.cpp:36
DMB_STOP_MOVE
#define DMB_STOP_MOVE
Definition: definitions.h:46
close_communication_robot
int close_communication_robot(void)
Ferme la communication avec le robot.
Definition: robot.cpp:64
DMB_TURN
#define DMB_TURN
Definition: definitions.h:41
DMB_MOVE
#define DMB_MOVE
Definition: definitions.h:40