schunk_powercube_chain: schunk_powercube_chain: simulatedMotor.cpp Source File

00001 
00060 #include <schunk_powercube_chain/simulatedMotor.h>
00061 #include <math.h>
00062 
00063                 
00064 simulatedMotor::simulatedMotor(double lowLimit, double upLimit, double maxAcc, double maxVel)
00065         : m_lastMove(0, 0, 0, 1, 1)
00066 {
00067         m_ll = lowLimit;
00068         m_ul = upLimit;
00069         m_amax = fabs(maxAcc);
00070         m_vmax = fabs(maxVel);
00071         //deb = NULL;
00072         m_lastMove.start(); // Will be finished immediately, so that motor is at x=0, v=0
00073         T0 = 0.0008;
00074 }
00075 
00077 // Zunächst die Steuerungs-Funktionen: //
00079 
00081 void simulatedMotor::moveRamp(double targetAngle, double vmax, double amax)
00082 {
00083         double x = m_lastMove.pos();
00084         double v = m_lastMove.vel();
00085         
00086         // Range Check:
00087         if ( targetAngle < m_ll ) targetAngle = m_ll;
00088         else if ( targetAngle > m_ul ) targetAngle = m_ul;
00089         
00090         double vm = fabs(vmax);
00091         double am = fabs(amax);
00092         
00093         if ( vm > m_vmax ) vm = m_vmax;
00094         if ( am > m_amax ) am = m_amax;
00095         
00096         m_lastMove = RampCommand(x, v, targetAngle, am, vm);
00097         m_lastMove.start();
00098 }
00099 
00100 
00102 void simulatedMotor::moveVel(double vel)
00103 {
00104         double x = m_lastMove.pos();
00105         double v = m_lastMove.vel();
00106         
00107         double targetAngle = x;
00108         
00109         // Move with constant v is RampMove to the corresponding limit!
00110         if ( vel > 0 ) 
00111                 targetAngle = m_ul;
00112         else if ( vel < 0)
00113                 targetAngle = m_ll;
00114         
00115         double vm = fabs(vel);
00116         if ( vm > m_vmax ) vm = m_vmax; 
00117         
00118         double a = fabs(vel-v) / T0;
00119         
00120         m_lastMove = RampCommand(x, v, targetAngle, a, vm);
00121         m_lastMove.start();
00122 }
00123 
00124 
00126 void simulatedMotor::movePos(double pos)
00127 {
00128         double x = m_lastMove.pos();
00129         double v = m_lastMove.vel();
00130         
00131         double targetAngle = pos;
00132 
00133         // Range Check:
00134         if ( targetAngle < m_ll ) targetAngle = m_ll;
00135         else if ( targetAngle > m_ul ) targetAngle = m_ul;
00136         
00137         double vm = fabs(m_vmax);
00138         
00139         // move backwards?
00140         if ( pos < x )
00141                 vm = -vm;
00142         
00143         double a = fabs(vm-v) / T0;
00144         
00145         m_lastMove = RampCommand(x, v, targetAngle, a, vm);
00146         m_lastMove.start();
00147 }
00148 
00149 
00151 void simulatedMotor::stop()
00152 {
00153         // Stops immediately (would not be possible with a real motor)
00154         double x = m_lastMove.pos();
00155         double v = 0;
00156         
00157         m_lastMove = RampCommand(x, v, x, 1, 1);
00158         m_lastMove.start();
00159 }
00160 
00162 RampCommand simulatedMotor::getRampMove(double targetAngle, double vmax, double amax)
00163 {
00164         double x = m_lastMove.pos();
00165         double v = m_lastMove.vel();
00166         
00167         // Range Check:
00168         if ( targetAngle < m_ll ) targetAngle = m_ll;
00169         else if ( targetAngle > m_ul ) targetAngle = m_ul;
00170         
00171         double vm = fabs(vmax);
00172         double am = fabs(amax);
00173         
00174         if ( vm > m_vmax ) vm = m_vmax;
00175         if ( am > m_amax ) am = m_amax;
00176         
00177         RampCommand rc(x, v, targetAngle, am, vm);
00178         return rc;
00179 }