#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