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);
}
RigidBodyHierarchyGeometry::RigidBodyHierarchyGeometry(
RigidBody rb, const ParticlesTemp &constituents)
: display::SingletonGeometry(rb) {
h_ = internal::get_rigid_body_hierarchy(
rb, IMP::internal::get_index(constituents), ObjectKey());
node_ = 0;
layer_ = 0;
}
GBindingPool::ObjectKey GBindingPool::doMakeObjectKey(const GObjectGlueDataPointer & glueData)
{
return ObjectKey(
glueData->getClassData() ? glueData->getClassData()->getMetaClass() : nullptr,
objectAddressFromVariant(glueData->getInstance()),
glueData->getCV()
);
}
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);
}
CoreCloseBipartitePairContainer
::CoreCloseBipartitePairContainer(SingletonContainer *a,
SingletonContainer *b,
double distance,
double slack):
P(a->get_model(), "CoreCloseBipartitePairContainer") {
std::ostringstream oss;
oss << "BCPC " << get_name() << " hierarchy " << this;
ObjectKey key=ObjectKey(oss.str());
initialize(a,b, base::get_invalid_index<ParticleIndexTag>(),
base::get_invalid_index<ParticleIndexTag>(),
distance, slack, key);
}
IMPCORE_BEGIN_NAMESPACE
RigidClosePairsFinder::RigidClosePairsFinder(ClosePairsFinder *cpf)
: ClosePairsFinder("RigidCPF") {
std::ostringstream oss;
oss << "RigidClosePairsFinderHiearchy " << this;
k_ = ObjectKey(oss.str());
if (cpf) {
cpf_ = cpf;
} else {
cpf_ = base::Pointer<ClosePairsFinder>(new GridClosePairsFinder());
}
}
GObjectGlueDataPointer GBindingPool::newObjectGlueData(
const GClassGlueDataPointer & classData,
const GObjectInstancePointer & objectInstance,
const GScriptInstanceCv cv
)
{
const ObjectKey key = ObjectKey(
classData->getMetaClass(),
objectInstance->getInstanceAddress(),
cv
);
GObjectGlueDataPointer result;
auto it = this->objectMap.find(key);
if(it != this->objectMap.end()) {
result = it->second.lock();
}
if(! result) {
result = GObjectGlueDataPointer(new GObjectGlueData(this->context.lock(), classData, objectInstance, cv));
}
return result;
}
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);
}
void Device::clearRPDOMapping(int object)
{
sendSDO(ObjectKey(RPDO_map+object,0,8), 0);
}
