bool Actuator::setTargetTVP(double qInit,double q_target,int signMovement,double timeInit,double timeFinal,double ta, double _time)
{
double val;
if(getTypeTrajectoryTVP()=="MaximumSpeedAcceleration")
{
//Acceleration phase
if (_time<(timeInit+ta) && _time>=timeInit)
val=qInit+signMovement*((getAcceleration()*0.5)*square(_time-timeInit));
//Velocity constant phase
if (_time>=(timeInit+ta) && _time<=(timeFinal-ta))
val=qInit+signMovement*(getSpeed()*(_time-timeInit-ta*0.5));
//Deceleration phase
if (_time>(timeFinal-ta) && _time<=timeFinal)
val=q_target-signMovement*((getAcceleration()*0.5)*square(timeFinal-_time));
}
else if(getTypeTrajectoryTVP()=="BangBang")
{
//Acceleration phase
if (_time<(timeFinal*0.5) && _time>=timeInit)
val=qInit+signMovement*((getMaxAcceleration()*0.5)*square(_time-timeInit));
//Deceleration phase
if (_time>=(timeFinal*0.5) && _time<=timeFinal)
val=q_target-signMovement*((getMaxAcceleration()*0.5)*square(timeFinal-_time));
}
return setTarget(val);
}
void
ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
{
/* Check for max acceleration */
setMaxAcceleration();
/* check if zero input stick */
const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON);
/* default is acceleration */
ManualIntentionZ intention = ManualIntentionZ::acceleration;
/* check zero input stick */
if (is_current_zero) {
intention = ManualIntentionZ::brake;
}
/*
* update intention
*/
if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
/* we start with lowest acceleration */
_acc_state_dependent = _acc_max_down.get();
/* reset slew-rate: this ensures that there
* is no delay present when user demands to brake
*/
_vel_sp_prev = _vel;
}
switch (intention) {
case ManualIntentionZ::brake: {
/* limit jerk when braking to zero */
float jerk = (_acc_max_up.get() - _acc_state_dependent) / dt;
if (jerk > _jerk_max.get()) {
_acc_state_dependent = _jerk_max.get() * dt + _acc_state_dependent;
} else {
_acc_state_dependent = _acc_max_up.get();
}
break;
}
case ManualIntentionZ::acceleration: {
_acc_state_dependent = (getMaxAcceleration() - _acc_max_down.get())
* fabsf(_stick) + _acc_max_down.get();
break;
}
}
_intention = intention;
}
asynStatus PIGCSMotorController::setAccelerationCts( PIasynAxis* pAxis, double accelerationCts)
{
double acceleration = fabs(accelerationCts) * pAxis->m_CPUdenominator / pAxis->m_CPUnumerator;
if (acceleration == pAxis->m_acceleration)
return asynSuccess;
if (pAxis->m_maxAcceleration < 0)
{
getMaxAcceleration(pAxis);
}
if (acceleration > pAxis->m_maxAcceleration)
acceleration = pAxis->m_maxAcceleration;
return setAcceleration(pAxis, acceleration);
}
MIMO::Ptr MotionProfileTrapezoidal::clone() {
MotionProfileTrapezoidal::Ptr tmp =
boost::make_shared< MotionProfileTrapezoidal > ();
tmp->setProgressExpression(
getProgressExpression()->clone()
);
for (int i=0;i<nrOfOutputs();++i) {
tmp->addOutput(
getStartValue(i)->clone(),
getEndValue(i)->clone(),
getMaxVelocity(i)->clone(),
getMaxAcceleration(i)->clone() );
}
return tmp;
}
