void shassis_support_impl::find_shassis( shasis_group i, nm::node_info_ptr group_node )
{
#if !defined(OSG_NODE_IMPL)
auto it = nodes_manager_->get_node_tree_iterator(group_node->node_id());
nm::visit_sub_tree(it, [this, i](nm::node_info_ptr node)->bool
{
std::string name = node->name();
if (boost::starts_with(node->name(), "shassi_"))
{
nm::node_info_ptr wheel_node;
nm::visit_sub_tree(this->nodes_manager_->get_node_tree_iterator(node->node_id()), [&wheel_node](nm::node_info_ptr n)->bool
{
if (boost::starts_with(n->name(), "wheel"))
{
wheel_node = n;
return false;
}
return true;
});
if (wheel_node)
{
FIXME ("We don't have collision volumes")
if (auto collision = wheel_node->get_collision())
{
cg::rectangle_3 bound = model_structure::bounding(*collision);
double radius = 0.75 * (bound.size().y / 2.);
this->shassis_groups_[i]->add_chassis(shassis_t(node, wheel_node, radius));
}
else
{
double radius = get_wheel_radius(node);//0.75 * (node->get_bound().size().z / 2.);
this->shassis_groups_[i]->add_chassis(shassis_t(node, wheel_node, radius/*0.75 * node->get_bound().radius*/));
}
}
}
return true;
});
//////////////////////////////////////////////////////////////////////////////
/// \brief Saves the complete current configuration parameters to the eeprom
/// When this function is called, it marks the given memory area with a flag and writes the current config parameters to the specified addresses (.h)
/// \param config The Memory Area where the configuration should be saved [1...4]
//////////////////////////////////////////////////////////////////////////////
VMC_UCHAR_8 save_configuration(VMC_UCHAR_8 config ) {
//loop counter
VMC_UCHAR_8 i;
//variables to save the calculated memory addresses
VMC_UINT_16 base;
VMC_UINT_16 motor;
// Which memory area is the target-->which address is the actual base address
base = (_OFFSET_CONFIG_ * config) + _OFFSET_BASE_;
i2c_init();
//Set the flag
i2c_storeByte(0xaa, base + _ADDR_FLAG_);
//Write the motor independent Parameter to the eeprom, addresses in headerfile
i2c_storeInt(get_timeout(), base + (2* _ADDR_TIMEOUT_) );
//For every motor: calculate a motorspecific offset
for(i = 0; i < _NUM_MOTORS_; i++) {
motor = base + ((i + 1) * _OFFSET_MOTOR_ );
//and write all motorrdepending parameters to the eeprom
i2c_storeInt(get_max_current(i), motor + (2 * _ADDR_MAX_CURRENT_) );
i2c_storeInt(get_max_RPM(i), motor + (2 * _ADDR_MAX_RPM_) );
i2c_storeInt(get_nom_current(i), motor + (2 * _ADDR_NOM_CURRENT_) );
i2c_storeInt(get_nom_RPM(i), motor + (2 * _ADDR_NOM_RPM_) );
i2c_storeInt(get_gear_reduction(i), motor + (2 * _ADDR_GEAR_REDUCTION_) );
i2c_storeInt(get_wheel_radius(i), motor + (2 * _ADDR_WHEEL_RADIUS_) );
i2c_storeInt(get_spec_torque(i), motor + (2 * _ADDR_SPEC_TORQUE_) );
i2c_storeInt(get_ticks_rot(i), motor + (2 * _ADDR_TICKS_ROT_) );
i2c_storeInt(get_max_temp(i), motor + (2 * _ADDR_MAX_TEMP_) );
i2c_storeInt(get_nom_temp(i), motor + (2 * _ADDR_NOM_TEMP_) );
i2c_storeInt(get_winding_t(i), motor + (2 * _ADDR_WINDING_T_) );
i2c_storeInt(get_winding_g_z(i), motor + (2 * _ADDR_WINDING_G_Z_) );
i2c_storeInt(get_winding_g_n(i), motor + (2 * _ADDR_WINDING_G_N_) );
i2c_storeInt(get_chassis_t(i), motor + (2 * _ADDR_CHASSIS_T_) );
i2c_storeInt(get_chassis_g_z(i), motor + (2 * _ADDR_CHASSIS_G_Z_) );
i2c_storeInt(get_chassis_g_n(i), motor + (2 * _ADDR_CHASSIS_G_N_) );
// Controller Parameters
i2c_storeInt(ctrl_get_Kpr(i), motor + (2 * _ADDR_KP_) );
i2c_storeInt(ctrl_get_Tn(i), motor + (2 * _ADDR_TN_) );
i2c_storeInt(ctrl_get_Tv(i), motor + (2 * _ADDR_TV_) );
i2c_storeInt(ctrl_get_deadband(i), motor + (2 * _ADDR_DEADBAND_) );
i2c_storeInt(ctrl_get_nRamp(i), motor + (2 * _ADDR_NRAMP_) );
i2c_storeInt(ctrl_get_pRamp(i), motor + (2 * _ADDR_PRAMP_) );
i2c_storeInt(get_direction(i), motor + (2 * _ADDR_DIRECTION_) );
i2c_storeInt(get_controller_active(i), motor + (2 * _ADDR_CONTROLLER_) );
i2c_storeInt(get_use_PWM(i), motor + (2 * _ADDR_USEPWM_) );
i2c_storeInt(get_currentlimiter_active(i), motor + (2 * _ADDR_LIMITER_) );
}
//Initializize the cycletime counter because of the "break"
init_cycletime_counter();
return 1;
}
