00001 #include "SIGEL_Simulation/SIG_DynaMoCommandInterface.h" 00002 #include "SIGEL_Tools/SIG_IO.h" 00003 #include "pointvector.h" 00004 #include <cmath> 00005 00006 SIGEL_Simulation::SIG_DynaMoCommandInterface::SIG_DynaMoCommandInterface(SIG_DynaMoSimulationData& theSimulationData) 00007 : simulationData(theSimulationData) 00008 { }; 00009 00010 void SIGEL_Simulation::SIG_DynaMoCommandInterface::moveDrive 00011 (int driveNo, 00012 QVector<SIG_Register> const& registers) 00013 { 00014 #ifdef SIG_DEBUG 00015 simulationData.gptestvalue=registers[0]->getValue(); 00016 #endif 00017 00018 if (simulationData.dynaSystem.dynaDrives.size()==0) return; 00019 int modDriveNo=driveNo % simulationData.dynaSystem.dynaDrives.size(); 00020 SIG_DynaDrive drive=simulationData.dynaSystem.getDrive(modDriveNo); 00021 00022 DL_Scalar maxforce=drive.drive->getMaxForce(); 00023 DL_Scalar minforce=drive.drive->getMinForce(); 00024 DL_Scalar regsize1=registers[0]->getMaxValue(); 00025 DL_Scalar regsize2=registers[1]->getMaxValue(); 00026 DL_Scalar force1=registers[0]->getValue()*maxforce/regsize1; 00027 DL_Scalar force2=registers[1]->getValue()*maxforce/regsize2; 00028 if (fabs(force1)<minforce) force1=minforce*force1/fabs(force1); 00029 if (fabs(force2)<minforce) force2=minforce*force2/fabs(force2); 00030 00031 drive.applyForce(force1,force2); 00032 00033 };