Change this function modifying it appropriately, in order to implement a linear interpolation, this should avoid the changes of robot configurations  confdata that for the interpolated point are unknown:
FUNC robtarget MidPos(robtarget p0,robtarget p1)
    VAR robtarget p;
    VAR pos v;

    p:=p0;
    v:=p1.trans-p0.trans;
    Add p.trans.x,(v.x*0.5);
    Add p.trans.y,(v.y*0.5);
    Add p.trans.z,(v.z*0.5);
    RETURN p;
  ENDFUNC

The following is my previous linear interpolating function:
LOCAL PROC PathInterpolation(robtarget p1, robtarget p2, INOUT robtarget pInterpolatedIN{*}, PERS tooldata tToolToUse \PERS wobjdata wobjToUse )
        !Support variables
        VAR robtarget dnResult;
        VAR num count := 1;
        VAR num t:= 0;
        VAR jointtarget jResult;
        VAR jointtarget jResultPrev;
        VAR robtarget dnResultPrev;
        
        
        WHILE count<=nSize DO
        t:= (count-1)*ndt;
        
        ! Linear interpolation for positions
        dnResult.trans.x := (1 - t) * p1.trans.x + t * p2.trans.x;
        dnResult.trans.y := (1 - t) * p1.trans.y + t * p2.trans.y;
        dnResult.trans.z := (1 - t) * p1.trans.z + t * p2.trans.z;
        
        ! Slerp interpolation for orientations
        dnResult.rot := Slerp(p1.rot, p2.rot, t);
        
        ! Copy the other attributes
        dnResult.robconf := p1.robconf;
        !dnResult.robconf.cfx := p1.robconf.cfx;
!        dnResult.extax := p1.extax;
!        IF count<> 1 THEN
!            jResult := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
!            IF jResultPrev.robax.rax_5 + jResult.robax.rax_5<=0.1  AND jResultPrev.robax.rax_5<0 THEN
!                dnResult.robconf.cfx:=1;
!            ELSEIF jResultPrev.robax.rax_5 + jResult.robax.rax_5<=0.1  AND jResultPrev.robax.rax_5>0 THEN
!                dnResult.robconf.cfx:=0;
!            ENDIF
!            jResultPrev:=jResult;
!        ELSEIF count = 1 THEN
!            jResultPrev := CalcJointT(dnResult, tToolToUse\WObj:=wobjToUse);
!        ENDIF
!        IF 0<jResult.robax.rax_1 AND jResult.robax.rax_1<= 90 THEN
!            dnResult.robconf.cf1:=1;
!        ELSEIF  90<jResult.robax.rax_1 AND jResult.robax.rax_1<= 180 THEN
!            dnResult.robconf.cf1:=2;
!        ELSEIF  -90<jResult.robax.rax_1 AND jResult.robax.rax_1<= 0 THEN
!            dnResult.robconf.cf1:=-1;
!        ENDIF
            
            
        
        ! Set confdata to undefined to allow the controller to find the best configuration
!        dnResult.robconf := [9E+09, 9E+09, 9E+09, 9E+09];
!        dnResult.extax := [9E+09, 9E+09, 9E+09, 9E+09, 9E+09, 9E+09];
        
        ! DEBUG
        !TPWrite "t: "+ NumToStr(t,5) + "count: " + NumToStr(count,1);
        
        ! Save in the array
        pInterpolatedIN{count} := dnResult;
        Incr count;
        
        ENDWHILE
        ! Ensure the last point exactly matches p2
        dnResult := p2;
        pInterpolatedIN{nSize} := dnResult; ! Ensure the last point is exactly p2
        
    ENDPROC

    !Slerp : Perform SLERP interpolation for ORIENTATION
    LOCAL FUNC orient Slerp(orient p1, orient p2, num t)
        ! Support variables
        VAR num dot ;
        VAR num n_theta;
        VAR num dnSinTheta;
        VAR num scale1;
        VAR num scale2;
        VAR orient dnResult;
        VAR num magnitude;
        
        ! Compute the dot product (cosine of the angle between the quaternions)
        dot := p1.q1*p2.q1 + p1.q2*p2.q2 + p1.q3*p2.q3 + p1.q4*p2.q4;
        
        ! If the dot product is negative, negate one quaternion to take the shorter path
        IF dot < 0 THEN
            p1.q1 := -p1.q1;
            p1.q2 := -p1.q2;
            p1.q3 := -p1.q3;
            p1.q4 := -p1.q4;
            dot := -dot;
        ENDIF
    
        ! If the quaternions are too close, use linear interpolation
        IF dot > 0.9995 THEN
            dnResult.q1 := (1 - t) * p1.q1 + t * p2.q1;
            dnResult.q2 := (1 - t) * p1.q2 + t * p2.q2;
            dnResult.q3 := (1 - t) * p1.q3 + t * p2.q3;
            dnResult.q4 := (1 - t) * p1.q4 + t * p2.q4;
        ELSE
            ! Compute the angle and sin(n_theta)
            n_theta := ACOS(dot);
            dnSinTheta := SIN(n_theta);
            
            ! Compute the scaling factors
            scale1 := SIN((1 - t) * n_theta) / dnSinTheta;
            scale2 := SIN(t * n_theta) / dnSinTheta;
            
            ! Compute the interpolated quaternion
            dnResult.q1 := scale1 * p1.q1 + scale2 * p2.q1;
            dnResult.q2 := scale1 * p1.q2 + scale2 * p2.q2;
            dnResult.q3 := scale1 * p1.q3 + scale2 * p2.q3;
            dnResult.q4 := scale1 * p1.q4 + scale2 * p2.q4;
        ENDIF
        
        ! Normalize the resulting quaternion
        magnitude := SQRT(dnResult.q1*dnResult.q1 + dnResult.q2*dnResult.q2 + dnResult.q3*dnResult.q3 + dnResult.q4*dnResult.q4);
        dnResult.q1 := dnResult.q1 / magnitude;
        dnResult.q2 := dnResult.q2 / magnitude;
        dnResult.q3 := dnResult.q3 / magnitude;
        dnResult.q4 := dnResult.q4 / magnitude;
        
        ! Return the normalized result
        RETURN dnResult;
    ENDFUNC
Modify it accordingly considering that
        ! Calculate nDistance between the actual robotarget point pInitial and the end robotarget point pFinal:
        nDistance := Euclidean_distance(pInitial, pFinal);
        
        ! Calculate the interpolating nSize according to the nDistance. The nDistance between the interpolated point is equal to nds
        nSize := Round(nDistance/nds)+1;
        ndt := 1/nSize;