00001 /* 00002 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA) 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 00018 #include "PowerCubeSim_OROCOS.h" 00019 #include "simulatedArm.h" 00020 #include <vector> 00021 00022 using namespace RTT; 00023 00024 PowerCubeSim_OROCOS::PowerCubeSim_OROCOS(std::string name) : OrocosRTTArmDriverInterface(name) 00025 { 00026 00027 } 00028 00029 PowerCubeSim_OROCOS::~PowerCubeSim_OROCOS() 00030 { 00031 } 00032 00033 bool PowerCubeSim_OROCOS::configureHook() 00034 { 00035 if ( m_powercubectrl.Init(false) ) 00036 { 00037 log(Info) << "PowerCubeSim initialized successfully." << endlog(); 00038 return true; 00039 } 00040 else 00041 { 00042 log(Info) << "Error while initializing PowerCubeSim:" << endlog(); 00043 //log(Info) << m_powercubectrl.getErrorMessage() << endlog(); 00044 return false; 00045 } 00046 } 00047 00048 bool PowerCubeSim_OROCOS::startHook() 00049 { 00050 /* if ( m_in_Angles.connected() ) 00051 { 00052 // only one of the input ports should be used simultaniously 00053 if ( m_in_Velocities.connected() || m_in_Currents.connected() ) { 00054 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog(); 00055 return false; 00056 } 00057 m_in_Angles_connected = true; 00058 log(Info) << "PowerCubeSim succesfully detected connection at in_Angles" << endlog(); 00059 return true; 00060 } 00061 if ( m_in_Velocities.connected() ) 00062 { 00063 // only one of the input ports should be used simultaniously 00064 if ( m_in_Angles.connected() || m_in_Currents.connected() ) { 00065 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog(); 00066 return false; 00067 } 00068 m_in_Velocities_connected = true; 00069 log(Info) << "PowerCubeSim succesfully detected connection at in_Velocities" << endlog(); 00070 return true; 00071 } 00072 if ( m_in_Currents.connected() ) 00073 { 00074 log(Info) << "Error, port \"setCur_R\" is connected to PowerCubeSim_OROCOS.\""; 00075 log(Info) << "Current movement is not yet supported by PowerCubeSim." << endlog(); 00076 return false; 00077 /* 00078 // only one of the input ports should be used simultaniously 00079 if ( m_in_Angles.connected() || m_in_Velocities.connected() ) { 00080 log(Info) << "Error in PowerCubeSim.startHook(): more than one input port is connected!" << endlog(); 00081 return false; 00082 } 00083 m_in_Current_connected = true; 00084 return true; 00085 00086 } 00087 else 00088 { 00089 log(Info) << "No Input port is connected, PowerCubeSim will only return the current state." << endlog(); 00090 } */ 00091 return true; 00092 } 00093 00094 void PowerCubeSim_OROCOS::updateHook() 00095 { 00096 //log(Info) << "updateHook is being executed." << endlog(); 00097 Jointd setpositions, setvelocities; 00098 setpositions = set_position_inport.Get(); 00099 setvelocities = set_velocity_inport.Get(); 00100 if(setpositions.size() == 7) 00101 { 00102 00103 } 00104 else if(setvelocities.size() == 7) 00105 { 00106 00107 } 00108 00109 current_position_outport.Set(m_powercubectrl.getConfig()); 00110 current_velocity_outport.Set(m_powercubectrl.getJointVelocities()); 00111 } 00112 00113 void PowerCubeSim_OROCOS::stopHook() 00114 { 00115 stopArm(); 00116 } 00117 00118 bool PowerCubeSim_OROCOS::stopArm() 00119 { 00120 //stop 00121 m_powercubectrl.stop(); 00122 return true; 00123 } 00124 00125 bool PowerCubeSim_OROCOS::isArmStopped() 00126 { 00127 //isStopped 00128 if(m_powercubectrl.statusMoving()) 00129 return false; 00130 return true; 00131 }