void setNextPosition(enum InstructionType instructionType,
        enum MotionParameterType motionParameterType,
        enum PidType pidType,
        float pNextPosition,
        float pa,
        float pSpeed) {
    initNextPositionVars(instructionType);

    PidMotion* pidMotion = getPidMotion();
    PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
    MotionInstruction* localInst = &(motionDefinition->inst[instructionType]);

    localInst->nextPosition = pNextPosition;
    localInst->motionParameterType = motionParameterType;
    localInst->pidType = pidType;

    if (pNextPosition > 0.001f) {
        localInst->a = pa * A_FACTOR;
        localInst->speed = pSpeed * SPEED_FACTOR;
    } 
    // Acceleration and speed becomes negative
    else if (pNextPosition < -0.001f) {
        localInst->a = -pa * A_FACTOR;
        localInst->speed = -pSpeed * SPEED_FACTOR;
    } 
    // pNextPosition == 0.0f Don't change the position
    else {
        if (motionParameterType == MOTION_PARAMETER_TYPE_MAINTAIN_POSITION) {
            localInst->a = 1.0f;
            localInst->speed = 100.0f;
        } else {
            // Slavery free
            localInst->a = 0.0f;
            localInst->speed = 0.0f;
        }
    }
    computeMotionInstruction(localInst);
    
    // When using classic motion, we compute a U value with alpha/theta (2 main parameters), and not with spline (3 values)
    pidMotion->currentMotionDefinition.computeU = &simpleMotionUCompute;
}
void setNextPosition(int instructionIndex,
                     unsigned char motionType,
                     unsigned char pidType,
                     float pNextPosition,
                     float pa,
                     float pSpeed) {
    initNextPositionVars(instructionIndex);

    PidMotion* pidMotion = getPidMotion();
    PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
    MotionInstruction* localInst = &(motionDefinition->inst[instructionIndex]);

    localInst->nextPosition = pNextPosition;
    localInst->motionType = motionType;
    localInst->pidType = pidType;

    if (pNextPosition > 0.0f) {
        localInst->a = pa * A_FACTOR;
        localInst->speed = pSpeed * SPEED_FACTOR;
    }        // Acceleration and speed becomes negative
    else if (pNextPosition < 0L) {
        localInst->a = -pa * A_FACTOR;
        localInst->speed = -pSpeed * SPEED_FACTOR;
    }        // Don't change the position
    else {
        if (motionType == MOTION_TYPE_MAINTAIN_POSITION) {
            localInst->a = 1.0f;
            localInst->speed = 100.0f;
        } else {
            // Slavery free
            localInst->a = 0.0f;
            localInst->speed = 0.0f;
        }
    }
    computeMotionInstruction(localInst);


    pidMotion->currentMotionDefinition.computeU = &simpleMotionUCompute;
}