174 lines
8.5 KiB
Markdown
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;
|
|
``` |