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; }