Obsidean_VM/01-Documentation/Vetromeccanica/FB Interpolator - Lineal - ...

174 lines
8.5 KiB
Markdown

| | | |
| -------------------- | --------------------------------------------------- | ----- |
| 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 | |
```pascal
#"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;
```