int main(int argc, char ** argv)
{
int board, i, j;
int retSize = -1;
char *devName = GPIO_IR_DEV;
int pin = GPIO_PIN(7);
char modStr[BUF_SIZE];
struct input_event evKey;
if ((board = boardInit()) < 0)
printf("Fail to init board\n");
if (board == BOARD_NANOPI_T2)
pin = GPIO_PIN(15);
sprintf(modStr, "modprobe %s gpio=%d", IR_DRIVER_MODULE, pintoGPIO(pin));
system(modStr);
signal(SIGINT, IRIntHandler);
sleep(1);
irFD = openHW(devName, O_RDWR);
if (irFD < 0) {
printf("Fail to open GPIO IR device\n");
return -1;
}
printf("Press the IR remoter\n");
for (i=0; i<IR_EVENT_TIMES; i++) {
if (selectHW(irFD, 0, 0) == 1) {
retSize = readHW(irFD, &evKey, sizeof(struct input_event));
for (j=0; j<(int) retSize / sizeof(struct input_event); j++)
printf("%2d: Type=%d, Code=%d, Value=%x\n", i, evKey.type, evKey.code, evKey.value);
}
}
closeHW(irFD);
system("rmmod "IR_DRIVER_MODULE);
return 0;
}
MitsubishiArmInterface::MitsubishiArmInterface(std::string & port)
{
//joint_state_pub=n_priv.advertise<sensor_msgs::JointState>( "joint_states", 1);
pos.resize(joint_number);
vel.resize(joint_number);
eff.resize(joint_number);
cmd.resize(joint_number);
cmd_previous.resize(joint_number);
// Open File Descriptor
//USB = open( "/dev/ttyUSB1", O_RDWR | O_NOCTTY);
USB = open( port.c_str(), O_RDWR | O_NOCTTY);
// Error Handling
if ( USB < 0 )
{
std::cout << "Error " << errno << " opening " << port.c_str() << ": " << strerror (errno) << std::endl;
}
// Configure Port
memset (&tty, 0, sizeof tty);
// Error Handling
if ( tcgetattr ( USB, &tty ) != 0 )
{
std::cout << "Error " << errno << " from tcgetattr: " << strerror(errno) << std::endl;
}
// Set Baud Rate
cfsetospeed (&tty, B19200);
cfsetispeed (&tty, B19200);
long BAUD =B19200;
tty.c_cflag = BAUD | CRTSCTS | CS8 | CLOCAL | CREAD | PARENB;
tty.c_cc[VMIN]=0;
tty.c_cc[VTIME]=20;
// Flush Port, then applies attributes
tcflush( USB, TCIFLUSH );
if ( tcsetattr ( USB, TCSANOW, &tty ) != 0)
{
std::cout << "Error " << errno << " from tcsetattr" << std::endl;
}
else
std::cout << "connected successfuly" <<std::endl;
usleep(10000);
char buf [256];
read( USB, &buf, 1); // CLEAN BUFFER
readHW();
cmd=pos;
cmd_previous=cmd;
// convert to radians and add to state
for(int i=0; i< pos.size(); ++i)
{
std::cout << cmd[i] << std::endl;
}
std::cout << "Init done!" << '\n';
// init();
return;
}
