/* configure event driver */
void CanNode::setEventDriver(bool flag)
{
if(flag)
sendSDO("22 23 64 00 10 00 00 00");
else
sendSDO("22 23 64 00 00 00 00 00");
}
void setSyncInterval(uint16_t deviceID,
std::chrono::milliseconds syncInterval) {
sendSDO(deviceID, "ip_time_units",
static_cast<uint32_t>( syncInterval.count() ));
sendSDO(deviceID, "ip_time_index", "milliseconds");
sendSDO(deviceID, "sync_timeout_factor", 0); // 0 = disable sync timeout
}
/* init node */
void CanNode::initCanNode(int can_idx, int node_id)
{
/* get the channels */
CAN_MSG msg;
//Digit input channels
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,0);
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,1);
sendSDO("40 00 60 00");
_sleep(100);
if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
m_Channels[0] = (int)msg.a_data[4] * 4;
/*else
QMessageBox::warning(0,"digit","read error");*/
//Digit output channels
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,0);
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,1);
sendSDO("40 00 62 00");
_sleep(100);
if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
m_Channels[1] = (int)msg.a_data[4] * 4;
/*else
QMessageBox::warning(0,"digit","read error");*/
//IP2302 has 4 digit input channels and 4 digit output channels
//sendState(00,80);
//_sleep(1000);
//if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
//{
// m_Channels[0] = 4;
// m_Channels[1] = 4;
//}
//Analog input channels
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,0);
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,1);
sendSDO("40 01 64 00");
_sleep(100);
if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
m_Channels[2] = (int)msg.a_data[4];
//Analog output channels
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,0);
CAN_ConfigQueue(can_idx,node_id,CAN_RX_QUE,1);
sendSDO("40 11 64 00");
_sleep(100);
if(CAN_ReadMsg(can_idx, node_id,1,&msg) == CAN_OK)
m_Channels[3] = (int)msg.a_data[4];
//QMessageBox::about(0,"channels",QString("channels:%1,%2,%3,%4").arg(m_Channels[0]).arg(m_Channels[1]).arg(m_Channels[2]).arg(m_Channels[3]));
sendState(01,00);
setHeartBeat(0);
setEventDriver(false);
}
void Device::makeRPDOMapping(int object, uint8_t sync_type)
{
int counter;
for(counter=0; counter < rpdo_registers_.size();counter++)
{
uint32_t data = rpdo_registers_[counter].size + (rpdo_registers_[counter].subindex << 8) + (rpdo_registers_[counter].index << 16);
sendSDO(ObjectKey(RPDO_map + object, counter + 1, 32), data);
}
sendSDO(ObjectKey(RPDO+object, 2, 8), sync_type);
ROS_DEBUG_STREAM("Mapping " << std::hex << counter << " objects at CANid " << (int)CANid_ << " to RPDO" << object + 1);
sendSDO(ObjectKey(RPDO_map+object,0,8), counter);
}
void init(std::string deviceFile, std::chrono::milliseconds syncInterval) {
// canopen::devices must be set up before this function is called
CAN_Close(h);
syncMsg.ID = 0x80;
syncMsg.MSGTYPE = 0x00;
syncMsg.LEN = 0x00;
NMTmsg.ID = 0;
NMTmsg.MSGTYPE = 0x00;
NMTmsg.LEN = 2;
nodeguardMsg.MSGTYPE = 0x01; // remote frame
nodeguardMsg.LEN = 0;
// if (atFirstInit) {
if (!canopen::openConnection(deviceFile)) {
std::cout << "Cannot open CAN device; aborting." << std::endl;
exit(EXIT_FAILURE);
}
if (atFirstInit)
canopen::initListenerThread(canopen::defaultListener);
// atFirstInit = false;
// }
for (auto device : devices) {
sendSDO(device.second.CANid_, ip_time_units, (uint8_t) syncInterval.count() );
sendSDO(device.second.CANid_, ip_time_index, ip_time_index_milliseconds);
sendSDO(device.second.CANid_, sync_timeout_factor, sync_timeout_factor_disable_timeout);
// NMT & motor state machine:
if (atFirstInit) {
canopen::sendNMT(device.second.CANid_, canopen::NMT_stop);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
canopen::sendNMT(device.second.CANid_, canopen::NMT_start);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
setMotorState(device.second.CANid_, "operation_enable");
/* canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_shutdown);
canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_switch_on);
canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_enable_operation); */
}
if (atFirstInit)
atFirstInit = false;
}
/* set heartbeat time */
void CanNode::setHeartBeat(int ms)
{
UINT8 pdata[8] ={0x22,0x17,0x10,0x00};
QString str = "22 17 10 00 ";
int low = 10*ms%256;
int high = 10*ms/256;
str += QString::number(low,16)+" "+QString::number(high,16)+" 00 00";
sendSDO(str);
}
void Device::makeTPDOMapping(int object, uint8_t sync_type)
{
int counter;
for(counter = 0; counter < tpdo_registers_.size(); counter++)
{
uint32_t data = tpdo_registers_[counter].size + (tpdo_registers_[counter].subindex << 8) + (tpdo_registers_[counter].index << 16);
sendSDO(ObjectKey(TPDO_map + object, counter + 1, 32), data);
}
sendSDO(ObjectKey(TPDO+object,2,8), sync_type);
ROS_DEBUG_STREAM("Mapping " << std::hex << counter << " objects at CANid " << (int)CANid_ << " to TPDO" << object + 1);
sendSDO(ObjectKey(TPDO+object,3,16), 10);
if(device_type == "imu" || device_type == "encoder") // send cyclic every 10ms
{
sendSDO(ObjectKey(TPDO+object,5,16), 10);
}
sendSDO(ObjectKey(TPDO_map+object,0,8), counter);
}
UNS8 sendSDOabort(UNS8 bus_id, UNS16 index, UNS8 subIndex, UNS32 abortCode)
{
s_SDO sdo;
UNS8 ret;
MSG_WAR(0x2A5F,"Sending SDO abort", abortCode);
sdo.len = 8;
sdo.body.SCS = 0x80;
sdo.nodeId = bDeviceNodeId;
// Index
sdo.body.data[0] = index & 0xFF; // LSB
sdo.body.data[1] = (index >> 8) & 0xFF; // MSB
// Subindex
sdo.body.data[2] = subIndex;
// Data
sdo.body.data[3] = (UNS8)(abortCode & 0xFF);
sdo.body.data[4] = (UNS8)((abortCode >> 8) & 0xFF);
sdo.body.data[5] = (UNS8)((abortCode >> 16) & 0xFF);
sdo.body.data[6] = (UNS8)((abortCode >> 24) & 0xFF);
ret = sendSDO(bus_id, sdo);
return ret;
}
void Device::disableRPDO(int object)
{
int32_t data;
switch(object)
{
case 0:
data = (RPDO1_msg + CANid_) + (0x00 << 16) + (0x80 << 24);
break;
case 1:
data = (RPDO2_msg + CANid_) + (0x00 << 16) + (0x80 << 24);
break;
case 2:
data = (RPDO3_msg + CANid_) + (0x00 << 16) + (0x80 << 24);
break;
case 3:
data = (RPDO4_msg + CANid_) + (0x00 << 16) + (0x80 << 24);
break;
default:
std::cout << "BAD OBJECT NUMBER IN disableRPDO! Number is " << object << std::endl;
return;
}
sendSDO(ObjectKey(RPDO+object,1,32), data);
}
void Device::enableTPDO(int object)
{
int32_t data;
switch(object)
{
case 0:
data = COB_PDO1_TX + CANid_;
break;
case 1:
data = COB_PDO2_TX + CANid_;
break;
case 2:
data = COB_PDO3_TX + CANid_;
break;
case 3:
data = COB_PDO4_TX + CANid_;
break;
default:
std::cout << "Incorrect object number handed over to enableTPDO" << std::endl;
return;
}
sendSDO(ObjectKey(TPDO+object,1,32), data);
}
void Device::disableTPDO(int object)
{
int32_t data;
switch(object)
{
case 0:
data = (TPDO1_msg + CANid_) + (1 << 31);
break;
case 1:
data = (TPDO2_msg + CANid_) + (1 << 31);
break;
case 2:
data = (TPDO3_msg + CANid_) + (1 << 31);
break;
case 3:
data = (TPDO4_msg + CANid_) + (1 << 31);
break;
default:
std::cout << "Incorrect object for mapping" << std::endl;
return;
}
sendSDO(ObjectKey(TPDO+object,1,32), data);
}
void Device::enableRPDO(int object)
{
int32_t data;
switch(object)
{
case 0:
data = COB_PDO1_RX + CANid_;
break;
case 1:
data = COB_PDO2_RX + CANid_;
break;
case 2:
data = COB_PDO3_RX + CANid_;
break;
case 3:
data = COB_PDO4_RX + CANid_;
break;
default:
ROS_ERROR("Wrong object number in enableRPDO");
return;
}
sendSDO(ObjectKey(RPDO+object, 1, 32), data);
}
bool homing(uint16_t deviceID) {
// set current position as device 0 position
// returns "true" if "drive_referenced" has appeared in device statusword
canopen::setMotorState(deviceID, "operation_enable");
sendSDO(deviceID, "modes_of_operation", "homing_mode");
sendSDO(deviceID, "controlword", "start_homing|enable_ip_mode");
// wait for drive to start moving:
while (!sendSDO(deviceID, "statusword").checkForConstant("drive_is_moving"))
std::this_thread::sleep_for(std::chrono::milliseconds(10));
// wait for drive to stop moving:
while (sendSDO(deviceID, "statusword").checkForConstant("drive_is_moving"))
std::this_thread::sleep_for(std::chrono::milliseconds(10));
while (!sendSDO(deviceID, "statusword").checkForConstant("drive_referenced"))
std::this_thread::sleep_for(std::chrono::milliseconds(10));
// return true if drive signals it is referenced; false otherwise:
return sendSDO(deviceID, "statusword").checkForConstant("drive_referenced");
}
UNS8 SDOmGR(UNS8 bus_id, UNS8 line) //Flux Manager
{
UNS8 res;
UNS8 i;
UNS8 line_transfers;
UNS8 t, n, c, e, s;
s_SDO sdo;
MSG_WAR(0x3A11, "SDOmGR ", 0);
res = 0xFF;
sdo.nodeId = transfers[bus_id][line].nodeId ;
if(TS_HAVE_TO_DO(transfers[bus_id][line].state))
{
switch(TS_ACTIVITY(transfers[bus_id][line].state))
{
// Initiate a Domain (up/down)load ( first frame )
case TS_ACTIVATED:
MSG_WAR(0x3A12, "Initiate Domain ", 0);
//memcpy(&sdo.body.data[0], &transfers[bus_id][line].index, 2);
// This Memcpy depends on packing structure. Avoid
sdo.body.data[0] = transfers[bus_id][line].index & 0xFF; // LSB
sdo.body.data[1] = (transfers[bus_id][line].index >> 8) & 0xFF; // MSB of index (16 b)
sdo.body.data[2] = transfers[bus_id][line].subindex;
if(TS_IS_DOWNLOAD(transfers[bus_id][line].state))
{
// Number of bytes to transfer < 5 -> expedited tranfer
MSG_WAR(0x3A13, "Download ", 0);
if(transfers[bus_id][line].count < 5)
{
n = 4 - transfers[bus_id][line].count;
e = 1;
s = 1;
sdo.len = 8 - n;
for(i=0; i<4-n; i++)
sdo.body.data[i+3] = transfers[bus_id][line].data[i];
// Next call will finish.
transfers[bus_id][line].offset = transfers[bus_id][line].count;
} else
{ // Normal transfer
n = 0;
e = 0;
s = 1;
//the first byte of D containts the LSB of number of data to be
//download and the last byte of D contains the MSB
sdo.body.data[3] = transfers[bus_id][line].count;
sdo.body.data[6] = 0;
sdo.body.data[4] = 0;
sdo.body.data[5] = 0;
sdo.len = 8;
transfers[bus_id][line].offset = 0;
}
sdo.body.SCS = IDD_client(n,e,s);
} else
{ // initiate upload, expedited transfer
MSG_WAR(0x3A14, "Upload ", 0);
sdo.len = 4;
sdo.body.SCS = IDU_client;
transfers[bus_id][line].offset = 0;
}
res = sendSDO(bus_id, sdo);
TS_SET_ACTIVITY(transfers[bus_id][line].state,TS_WORKING | TS_WAIT_SERVER);
break;
// Follow Domain (up/down)load
// ( following frames, if more that one is needed )
case TS_WORKING:
MSG_WAR(0x3A15, "Domain Segment ", 0);
getSDOlineOnUse( bus_id,transfers[bus_id][line].nodeId, &line_transfers );
line = line_transfers;
if(TS_IS_DOWNLOAD(transfers[bus_id][line].state))
{
MSG_WAR(0x3A16, "Download ", 0);
i = transfers[bus_id][line].count - transfers[bus_id][line].offset;
if(i <= 0)
{ // Download Finished
res = 0;
transfers[bus_id][line].state = TS_FREE;
resetSDOline( bus_id, line );
break;
}
else
{ // Follow segmented transfer
if(i>7)
{
n = 0; // There is no unused byte
c = 0; // It is not the last message
}
else
{
n = 7 - i; // There could have unused bytes
c = 1; // This is the last message
}
sdo.len = 8 - n;
for(i=0; i<7-n; i++)
sdo.body.data[i] =transfers[bus_id][line].data[transfers[bus_id][line].offset++];
// take the toggle bit
t = (transfers[bus_id][line].state & TS_TOGGLE) >> 4;
sdo.body.SCS = DDS_client(t,n,c);
// toggle afterward
transfers[bus_id][line].state ^= TS_TOGGLE;
if (c)
resetSDOline( bus_id, line );
}
}
else
{ // Upload
MSG_WAR(0x3A17, "Upload ", 0);
//memcpy(&sdo.body.data[0], &transfers[bus_id][line].index, 2);
// This Memcpy depends on packing structure. Avoid
sdo.body.data[0] = transfers[bus_id][line].index & 0xFF; // LSB
sdo.body.data[1] = (transfers[bus_id][line].index >> 8) & 0xFF; // MSB of index (16 b)
sdo.body.data[2] = transfers[bus_id][line].subindex;
sdo.len = 4;
// take toggle bit
t = (transfers[bus_id][line].state & TS_TOGGLE) >> 4;
sdo.body.SCS = UDS_client(t);
// toggle afterward
transfers[bus_id][line].state ^= TS_TOGGLE;
}
res = sendSDO(bus_id, sdo);
TS_SET_ACTIVITY(transfers[bus_id][line].state,TS_WORKING | TS_WAIT_SERVER);
break;
default: // Transfer not in use or transfer error. Blub blub...
resetSDOline( bus_id, line );
break;
}
void Device::clearRPDOMapping(int object)
{
sendSDO(ObjectKey(RPDO_map+object,0,8), 0);
}
bool driveMode(uint16_t deviceID, std::string mode) {
sendSDO(deviceID, "modes_of_operation", mode);
// todo: waitForConstant
return
sendSDO(deviceID, "modes_of_operation_display").checkForConstant(mode);
}
double getPos(uint16_t deviceID) {
Message m( sendSDO(deviceID, "position_actual_value") );
return mdeg2rad( m.values_[0] );
}