void CANSocket::open(int port) throw(std::logic_error, std::runtime_error)
{
if (isOpen()) {
(logMessage("CANSocket::%s(): This object is already associated with a CAN port.")
% __func__).raise<std::logic_error>();
}
logMessage("CANSocket::open(%d)") % port;
int ret;
ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW);
if (ret < 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_socket(): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
handle = ret;
struct ifreq ifr;
char devname[10];
sprintf(devname, "rtcan%d", port);
strncpy(ifr.ifr_name, devname, IFNAMSIZ);
ret = rt_dev_ioctl(handle, SIOCGCANSTATE, &ifr);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(SIOCGCANSTATE): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
} else {
// Note: The Xenomai documentation says that "ifr_ifru will be filled
// with an instance of can_mode_t." This is a documentation bug. In
// ksrc/drivers/can/rtcan_raw_dev.c ifr_ifru actually gets filled with
// in instance of can_state_t, which makes a lot more sense.
can_state_t* state = (can_state_t*) &ifr.ifr_ifru;
if (*state != CAN_STATE_ACTIVE) {
const int NUM_STATES = 8;
const char canStateStrs[NUM_STATES][18] = {
"active",
"warning",
"passive",
"bus-off",
"scanning-baudrate",
"stopped",
"sleeping",
"unknown state"
};
int index = *state;
if (index < 0 || index >= NUM_STATES) {
index = NUM_STATES - 1;
}
logMessage(" WARNING: CAN_STATE is %s (%d)") % canStateStrs[index] % *state;
if (*state == CAN_STATE_BUS_OFF) {
logMessage(" Setting CAN_MODE = CAN_MODE_START");
can_mode_t* mode = (can_mode_t*) &ifr.ifr_ifru;
*mode = CAN_MODE_START;
ret = rt_dev_ioctl(handle, SIOCSCANMODE, &ifr);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(SIOCSCANMODE): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
// According to Xenomai documentation, the transition from
// bus-off to active takes at least 128 * 11 * 1e-6 = 0.0014
// seconds and there's no way to detect when it's done :(
btsleep(0.1);
}
}
}
struct can_filter recvFilter[1];
recvFilter[0].can_id = (Puck::HOST_ID << Puck::NODE_ID_WIDTH) | CAN_INV_FILTER;
recvFilter[0].can_mask = Puck::FROM_MASK;
ret = rt_dev_setsockopt(handle, SOL_CAN_RAW, CAN_RAW_FILTER, &recvFilter, sizeof(recvFilter));
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_setsockopt(CAN_RAW_FILTER): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
// Note: This must be done after the rt_dev_ioctl(SIOCGCANSTATE) call above,
// otherwise rt_dev_send() will fail with ret = -6 (No such device or
// address). The ifr.ifr_index gets overwritten because it is actually a
// member of the ifr.ifr_ifru union.
ret = rt_dev_ioctl(handle, SIOCGIFINDEX, &ifr);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(SIOCGIFINDEX): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
struct sockaddr_can toAddr;
memset(&toAddr, 0, sizeof(toAddr));
toAddr.can_ifindex = ifr.ifr_ifindex;
toAddr.can_family = AF_CAN;
ret = rt_dev_bind(handle, (struct sockaddr *) &toAddr, sizeof(toAddr));
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_bind(): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
nanosecs_rel_t timeout = (nanosecs_rel_t) CommunicationsBus::TIMEOUT;
ret = rt_dev_ioctl(handle, RTCAN_RTIOC_RCV_TIMEOUT, &timeout);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(RCV_TIMEOUT): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
ret = rt_dev_ioctl(handle, RTCAN_RTIOC_SND_TIMEOUT, &timeout);
if (ret != 0) {
close();
(logMessage("CANSocket::%s(): Could not open CAN port. rt_dev_ioctl(SND_TIMEOUT): (%d) %s")
% __func__ % -ret % strerror(-ret)).raise<std::runtime_error>();
}
}
int wam_main(int argc, char** argv, ProductManager& pm,
systems::Wam& wam_) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DIMENSION);
systems::Wam<DIMENSION>* wam = (systems::Wam<DIMENSION>*)(&wam_);
Robot robot(wam, pm, filename, config.getRoot());
if (!robot.init())
return 1;
robot.loop = true;
curState = PLAYING;
float sleep_s = 0.002;
//while (roboting) {
while(pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE){
if(robot.problem){
//cout << "contact!" << endl;
//cout << "hd: " << robot.hand_debug.str() << endl;
cout << "uh-oh" << endl;
robot.hand_debug.str("");
robot.problem = false;
}
switch (curState) {
case QUIT:
//roboting = false;
break;
case PLAYING:
switch (lastState) {
case STOPPED:
robot.moveToStart();
robot.init_data_logger();
robot.reconnectSystems();
robot.startplayback();
lastState = PLAYING;
break;
case PAUSED:
robot.startplayback();
lastState = PLAYING;
break;
case PLAYING:
if (robot.playbackActive()) {
btsleep(sleep_s);
break;
} else if (robot.loop) {
loop_count++;
robot.disconnectSystems();
lastState = STOPPED;
curState = PLAYING;
break;
} else {
curState = STOPPED;
break;
}
default: break;
}
break;
case PAUSED:
switch (lastState) {
case PLAYING:
robot.pauseplayback();
lastState = PAUSED;
break;
case PAUSED:
btsleep(sleep_s);
break;
case STOPPED:
break;
default: break;
}
break;
case STOPPED:
switch (lastState) {
case PLAYING:
robot.disconnectSystems();
lastState = STOPPED;
break;
case PAUSED:
robot.disconnectSystems();
lastState = STOPPED;
break;
case STOPPED:
btsleep(sleep_s);
break;
default: break;
}
break;
}
/*if(!collecting_data){
robot.collect_data_stream();
collecting_data = true;
}*/
}
robot.disconnectSystems();
robot.output_data_stream();
//wam->moveHome();
//printf("\n\n");
//pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
