#include "epucksercomm.h"
epuckSercomm::epuckSercomm()
{
}
int epuckSercomm::initRobots(struct robot *rbt)
{
int id;
for(id=0;id<7;id++)
{
rbt[id].serialConnection=id;
rbt[id].port=-1;
rbt[id].connected=0;
}
return 0;
}
int epuckSercomm::connectRobot(struct robot *rbt, int comPort)
{
char portname[20];
//sprintf(portname,"COM%d",comPort);
sprintf(portname,"\\\\.\\COM%d",comPort);
hSerialPort[rbt->serialConnection] = CreateFileA(portname,
GENERIC_READ | GENERIC_WRITE,
0,
0,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
0);
if (hSerialPort[rbt->serialConnection] == INVALID_HANDLE_VALUE) return -1;
dcbSerialParams.dcb.DCBlength=sizeof(dcbSerialParams);
if (!GetCommState(hSerialPort[rbt->serialConnection], &dcbSerialParams.dcb)) return -2;
dcbSerialParams.dcb.BaudRate = 115200;
dcbSerialParams.dcb.ByteSize = 8;
dcbSerialParams.dcb.StopBits = ONESTOPBIT;
dcbSerialParams.dcb.Parity = NOPARITY;
comt.ReadIntervalTimeout = 500;
SetCommTimeouts( hSerialPort[rbt->serialConnection], &comt);
if (!SetCommState(hSerialPort[rbt->serialConnection], &dcbSerialParams.dcb)) return -3;
rbt->port=comPort;
rbt->connected=1;
return 0;
}
int epuckSercomm::setRobotSpeed(struct robot *rbt, int speedL, int speedR)
{
DWORD dwBytesReadWrite = 0;
char command[5];
command[0]='D';
command[1]=speedL & 0xff;
command[2]=speedL >> 8;
command[3]=speedR & 0xff;
command[4]=speedR >>8;
if (!WriteFile(hSerialPort[rbt->serialConnection],command,5,&dwBytesReadWrite,NULL)) return -1;
return 0;
}
int epuckSercomm::disconnectRobot(struct robot *rbt)
{
CloseHandle(hSerialPort[rbt->serialConnection]);
rbt->connected=0;
return 0;
}
int epuckSercomm::printRobotSensors(struct robot *rbt)
{
printf("Robot no:%d\r\n",rbt->serialConnection);
printf("IR: %d %d %d %d %d %d %d %d\r\n",
rbt->IR[0],rbt->IR[1],rbt->IR[2],rbt->IR[3],
rbt->IR[4],rbt->IR[5],rbt->IR[6],rbt->IR[7]);
printf("LI: %d %d %d %d %d %d %d %d\r\n",
rbt->LI[0],rbt->LI[1],rbt->LI[2],rbt->LI[3],
rbt->LI[4],rbt->LI[5],rbt->LI[6],rbt->LI[7]);
printf("FL: %d %d %d\r\n",
rbt->FL[0],rbt->FL[1],rbt->FL[2]);
printf("MC: %d %d %d\r\n",
rbt->MC[0],rbt->MC[1],rbt->MC[2]);
return 0;
}
int epuckSercomm::setRobotLed(struct robot *rbt,int noLed, int value)
{
DWORD dwBytesReadWrite = 0;
char command[3];
if (value<0)value=0; //on
if (value>1)value=1; //off
command[0]='L';
command[1]= noLed; //number of led to switch
command[2]= value; //0: off, 1: on
if (!WriteFile(hSerialPort[rbt->serialConnection],command,3,&dwBytesReadWrite,NULL))
return -1;
return 0;
}
#include "epucksercomm.h"
epuckSercomm::epuckSercomm()
{
}
int epuckSercomm::initRobots(struct robot *rbt)
{
int id;
for(id=0;id<7;id++)
{
rbt[id].serialConnection=id;
rbt[id].port=-1;
rbt[id].connected=0;
}
return 0;
}
int epuckSercomm::connectRobot(struct robot *rbt, int comPort)
{
char portname[20];
//sprintf(portname,"COM%d",comPort);
sprintf(portname,"\\\\.\\COM%d",comPort);
hSerialPort[rbt->serialConnection] = CreateFileA(portname,
GENERIC_READ | GENERIC_WRITE,
0,
0,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
0);
if (hSerialPort[rbt->serialConnection] == INVALID_HANDLE_VALUE) return -1;
dcbSerialParams.dcb.DCBlength=sizeof(dcbSerialParams);
if (!GetCommState(hSerialPort[rbt->serialConnection], &dcbSerialParams.dcb)) return -2;
dcbSerialParams.dcb.BaudRate = 115200;
dcbSerialParams.dcb.ByteSize = 8;
dcbSerialParams.dcb.StopBits = ONESTOPBIT;
dcbSerialParams.dcb.Parity = NOPARITY;
comt.ReadIntervalTimeout = 500;
SetCommTimeouts( hSerialPort[rbt->serialConnection], &comt);
if (!SetCommState(hSerialPort[rbt->serialConnection], &dcbSerialParams.dcb)) return -3;
rbt->port=comPort;
rbt->connected=1;
return 0;
}
int epuckSercomm::setRobotSpeed(struct robot *rbt, int speedL, int speedR)
{
DWORD dwBytesReadWrite = 0;
char command[5];
command[0]='D';
command[1]=speedL & 0xff;
command[2]=speedL >> 8;
command[3]=speedR & 0xff;
command[4]=speedR >>8;
if (!WriteFile(hSerialPort[rbt->serialConnection],command,5,&dwBytesReadWrite,NULL)) return -1;
return 0;
}
int epuckSercomm::disconnectRobot(struct robot *rbt)
{
CloseHandle(hSerialPort[rbt->serialConnection]);
rbt->connected=0;
return 0;
}
int epuckSercomm::printRobotSensors(struct robot *rbt)
{
printf("Robot no:%d\r\n",rbt->serialConnection);
printf("IR: %d %d %d %d %d %d %d %d\r\n",
rbt->IR[0],rbt->IR[1],rbt->IR[2],rbt->IR[3],
rbt->IR[4],rbt->IR[5],rbt->IR[6],rbt->IR[7]);
printf("LI: %d %d %d %d %d %d %d %d\r\n",
rbt->LI[0],rbt->LI[1],rbt->LI[2],rbt->LI[3],
rbt->LI[4],rbt->LI[5],rbt->LI[6],rbt->LI[7]);
printf("FL: %d %d %d\r\n",
rbt->FL[0],rbt->FL[1],rbt->FL[2]);
printf("MC: %d %d %d\r\n",
rbt->MC[0],rbt->MC[1],rbt->MC[2]);
return 0;
}
int epuckSercomm::setRobotLed(struct robot *rbt,int noLed, int value)
{
DWORD dwBytesReadWrite = 0;
char command[3];
if (value<0)value=0; //on
if (value>1)value=1; //off
command[0]='L';
command[1]= noLed; //number of led to switch
command[2]= value; //0: off, 1: on
if (!WriteFile(hSerialPort[rbt->serialConnection],command,3,&dwBytesReadWrite,NULL))
return -1;
return 0;
}
To copy to clipboard, switch view to plain text mode
#ifndef EPUCKSERCOMM_H
#define EPUCKSERCOMM_H
#include <QObject>
#include <stdio.h>
#include "mode.h"
#include "windows.h"
#include "time.h"
#include "defs.h"
class epuckSercomm
: public QObject{
public:
epuckSercomm();
int setRobotLed(struct robot *rbt,int noLed, int value);
int printRobotSensors(struct robot *rbt);
int disconnectRobot(struct robot *rbt);
int setRobotSpeed(struct robot *rbt, int speedL, int speedR);
int connectRobot(struct robot *rbt, int comPort);
int initRobots(struct robot *rbt);
//struct robot robots[7];
HANDLE hSerialPort[7]; //up to 7 connection
COMMCONFIG dcbSerialParams;
COMMTIMEOUTS comt;
// fine
//stat
clock_t startTime;
clock_t endTime;
int dtime;
};
#endif // EPUCKSERCOMM_H
#ifndef EPUCKSERCOMM_H
#define EPUCKSERCOMM_H
#include <QObject>
#include <stdio.h>
#include "mode.h"
#include "windows.h"
#include "time.h"
#include "defs.h"
class epuckSercomm : public QObject
{
public:
epuckSercomm();
int setRobotLed(struct robot *rbt,int noLed, int value);
int printRobotSensors(struct robot *rbt);
int disconnectRobot(struct robot *rbt);
int setRobotSpeed(struct robot *rbt, int speedL, int speedR);
int connectRobot(struct robot *rbt, int comPort);
int initRobots(struct robot *rbt);
//struct robot robots[7];
HANDLE hSerialPort[7]; //up to 7 connection
COMMCONFIG dcbSerialParams;
COMMTIMEOUTS comt;
// fine
//stat
clock_t startTime;
clock_t endTime;
int dtime;
};
#endif // EPUCKSERCOMM_H
To copy to clipboard, switch view to plain text mode
epuckSercomm *epuck = new epuckSercomm();
epuck->initRobots(&robots[0]);
epuck->connectRobot(&robots[1], 1036);
epuck->setRobotLed(&robots[1],7,1);
epuck->setRobotLed(&robots[1],7,0);
epuckSercomm *epuck = new epuckSercomm();
epuck->initRobots(&robots[0]);
epuck->connectRobot(&robots[1], 1036);
epuck->setRobotLed(&robots[1],7,1);
epuck->setRobotLed(&robots[1],7,0);
To copy to clipboard, switch view to plain text mode
Bookmarks