/** * @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); }