/**
* @private
* Init all variables.
* @param index corresponds to INSTRUCTION_THETA_INDEX or INSTRUCTION_ALPHA_INDEX
*/
void initNextPositionVars(int index) {
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
// Initialization of MotionInstruction : TODO : do not reset it !
MotionInstruction* localInst = &(motionDefinition->inst[index]);
localInst->nextPosition = 0;
localInst->a = 0;
localInst->speed = 0;
localInst->speedMax = 0;
localInst->endSpeed = 0;
PidComputationValues* computationValues = &(pidMotion->computationValues);
// Initialization of MotionError
MotionError* localErr = &(computationValues->err[index]);
localErr->previousError = 0;
localErr->error = 0;
localErr->derivativeError = 0;
localErr->integralError = 0;
// Initialization of Motion
Motion* localMotion = &(computationValues->motion[index]);
localInst->initialSpeed = localMotion->currentSpeed;
localMotion->position = 0;
localMotion->oldPosition = 0;
localMotion->u = 0;
localInst->profileType = 0;
localInst->pidType = 0;
localInst->motionType = 0;
// Initialization of motionEnd & motionBlocked
MotionEndInfo* localEnd = &(computationValues->motionEnd[index]);
resetMotionEndData(localEnd);
}
/**
* @private
* Init all variables.
* @param index corresponds to THETA or ALPHA
*/
void initNextPositionVars(int index) {
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
// Initialization of MotionInstruction : TODO : do not reset it !
MotionInstruction* localInst = &(motionDefinition->inst[index]);
localInst->nextPosition = 0;
localInst->a = 0;
localInst->speed = 0;
localInst->speedMax = 0;
localInst->endSpeed = 0;
PidComputationValues* computationValues = &(pidMotion->computationValues);
// Initialization of MotionError
PidMotionError* localErr = &(computationValues->errors[index]);
localErr->previousError = 0;
localErr->error = 0;
localErr->derivativeError = 0;
localErr->integralError = 0;
// Initialization of Motion
PidCurrentValues* localCurrentValues = &(computationValues->currentValues[index]);
localInst->initialSpeed = localCurrentValues->currentSpeed;
localCurrentValues->position = 0;
localCurrentValues->oldPosition = 0;
localCurrentValues->u = 0;
localInst->profileType = PROFILE_TYPE_TRAPEZE;
localInst->pidType = PID_TYPE_GO_INDEX;
localInst->motionParameterType = MOTION_PARAMETER_TYPE_MAINTAIN_POSITION;
// Initialization of motionEnd & motionBlocked
MotionEndInfo* localEnd = &(computationValues->motionEnd[index]);
resetMotionEndData(localEnd);
}
