Miguel
/
Obsidean_VM
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Obsidean_VM/01-Documentation/Vetromeccanica/FB Interpolator - Lineal - ...

8.5 KiB
Raw Blame History

Input
Sensor Bool 0.0
Motor Speed Feedback Real 2.0
Output
InOut
Static
Ons Bool 6.0
Times Array[1.."Interpolation_Max_Points"] of LReal 8.0
Speeds Array[1.."Interpolation_Max_Points"] of LReal 88.0
Coefficients Array[0..2] of LReal 168.0
Count Int 192.0
EstimateSpeed LReal 194.0
Mode Int 202.0
Last Speed Feedback LReal 204.0
A Array[1.."Interpolation_Max_Points", 0..2] of LReal 212.0
B Array[0.."Interpolation_Max_Points"] of LReal 452.0
ATA Array[0..2, 0..2] of LReal 540.0
ATB Array[0..2] of LReal 612.0
TimeRecorded LReal 636.0
Time Pass LReal 644.0
Time Last Pulse LReal 652.0
Temp
Sensor Pulse Bool 0.0
i Int 2.0
j Int 4.0
k Int 6.0
Sum LReal 8.0
Speed LReal 16.0
Time LReal 24.0
Det LReal 32.0
N LReal 40.0
Denominator LReal 48.0
SumX2 LReal 56.0
SumX LReal 64.0
SumXY LReal 72.0
SumY LReal 80.0
ElapsedRuntime_s LReal 88.0
Constant
Mode Linear Int
Mode Quadratic Int 
#"Sensor Pulse" := #Sensor AND NOT #Ons;
#Ons := #Sensor;

#ElapsedRuntime_s := RUNTIME(#TimeRecorded);
#"Time Pass" := #"Time Pass" + #ElapsedRuntime_s;

IF #"Sensor Pulse" THEN
    #Speed := 3600.0 / (#"Time Pass" - #"Time Last Pulse" );
    #"Time Last Pulse" := #"Time Pass";
END_IF;

IF #"Sensor Pulse" THEN
    #"Sensor Pulse" := FALSE;
    
    IF #Count = "Interpolation_Max_Points" THEN
        FOR #i := 1 TO "Interpolation_Max_Points" - 1 DO
            #Times[#i] := #Times[#i + 1];
            #Speeds[#i] := #Speeds[#i + 1];
        END_FOR;
        #Count := #Count - 1;
    END_IF;
    
    #Count := #Count + 1;
    #Times[#Count] := #"Time Pass";
    #Speeds[#Count] := #Speed;
    
    IF #Mode = #"Mode Quadratic" THEN
        IF #Count >= "Interpolation_Max_Points" THEN
            // Construir matriz A y vector B
            
            FOR #i := 1 TO #Count DO
                #Time := #Times[#i];
                #A[#i, 0] := (#Time * #Time);
                #A[#i, 1] := (#Time);
                #A[#i, 2] := 1.0;
                #B[#i - 1] := #Speeds[#i];
            END_FOR;
            
            // Calcular A^T * A y A^T * B
            FOR #i := 0 TO 2 DO
                FOR #j := 0 TO 2 DO
                    #Sum := 0.0;
                    FOR #k := 1 TO #Count DO
                        #Sum := #Sum + #A[#k, #i] * #A[#k, #j];
                    END_FOR;
                    #ATA[#i, #j] := #Sum;
                END_FOR;
                
                #Sum := 0.0;
                FOR #k := 1 TO #Count DO
                    #Sum := #Sum + #A[#k, #i] * #B[#k - 1];
                END_FOR;
                #ATB[#i] := #Sum;
            END_FOR;
            
            // Resolver el sistema de ecuaciones (ATA * x = ATB) usando la regla de Cramer
            #Det := #ATA[0, 0] * (#ATA[1, 1] * #ATA[2, 2] - #ATA[1, 2] * #ATA[2, 1])
            - #ATA[0, 1] * (#ATA[1, 0] * #ATA[2, 2] - #ATA[1, 2] * #ATA[2, 0])
            + #ATA[0, 2] * (#ATA[1, 0] * #ATA[2, 1] - #ATA[1, 1] * #ATA[2, 0]);
            
            IF ABS(#Det) > 1.0e-10 THEN
                #Coefficients[0] := (#ATB[0] * (#ATA[1, 1] * #ATA[2, 2] - #ATA[1, 2] * #ATA[2, 1])
                - #ATA[0, 1] * (#ATB[1] * #ATA[2, 2] - #ATA[1, 2] * #ATB[2])
                + #ATA[0, 2] * (#ATB[1] * #ATA[2, 1] - #ATA[1, 1] * #ATB[2])) / #Det;
                
                #Coefficients[1] := (#ATA[0, 0] * (#ATB[1] * #ATA[2, 2] - #ATA[1, 2] * #ATB[2])
                - #ATB[0] * (#ATA[1, 0] * #ATA[2, 2] - #ATA[1, 2] * #ATA[2, 0])
                + #ATA[0, 2] * (#ATA[1, 0] * #ATB[2] - #ATB[1] * #ATA[2, 0])) / #Det;
                
                #Coefficients[2] := (#ATA[0, 0] * (#ATA[1, 1] * #ATB[2] - #ATB[1] * #ATA[2, 1])
                - #ATA[0, 1] * (#ATA[1, 0] * #ATB[2] - #ATB[1] * #ATA[2, 0])
                + #ATB[0] * (#ATA[1, 0] * #ATA[2, 1] - #ATA[1, 1] * #ATA[2, 0])) / #Det;
            END_IF;
        END_IF;
    ELSIF #Mode = #"Mode Linear" THEN
        IF #Count >= "Interpolation_Max_Points" THEN
            // Cálculo de los coeficientes para la interpolación lineal
            #SumX := 0.0;
            #SumY := 0.0;
            #SumXY := 0.0;
            #SumX2 := 0.0;
            
            FOR #i := 1 TO #Count DO
                #Time := #Times[#i];
                #SumX := #SumX + #Time;
                #SumY := #SumY + #Speeds[#i];
                #SumXY := #SumXY + #Time * #Speeds[#i];
                #SumX2 := #SumX2 + #Time * #Time;
            END_FOR;
            
            #N := INT_TO_REAL(#Count);
            #Denominator := #N * #SumX2 - #SumX * #SumX;
            
            IF ABS(#Denominator) > 1.0e-10 THEN
                // Cálculo de la pendiente (m) y la intersección (b) de la línea y = mx + b
                #Coefficients[0] := (#N * #SumXY - #SumX * #SumY) / #Denominator; // Pendiente (m)
                #Coefficients[1] := (#SumY * #SumX2 - #SumX * #SumXY) / #Denominator; // Intersección (b)
            ELSE
                // Si el denominador es muy cercano a cero, usamos una línea horizontal
                #Coefficients[0] := 0.0; // Pendiente (m) = 0
                #Coefficients[1] := #SumY / #N; // Intersección (b) = promedio de velocidades
            END_IF;
        END_IF;
    END_IF;
END_IF;

IF #Count >= "Interpolation_Max_Points" THEN
    IF #Mode = #"Mode Quadratic" THEN
        #EstimateSpeed := #Coefficients[0] * #"Time Pass" * #"Time Pass" +
        #Coefficients[1] * #"Time Pass" +
        #Coefficients[2];
        
    ELSIF #Mode = #"Mode Linear" THEN
        
        #EstimateSpeed := #Coefficients[0] * #"Time Pass" + #Coefficients[1];
        
    END_IF;
    
    IF #EstimateSpeed < 0 THEN
        #EstimateSpeed := 0;
        #Count := 0;
    END_IF;
ELSE
    #EstimateSpeed := #"Motor Speed Feedback";
END_IF;