| | | | | -------------------- | --------------------------------------------------- | ----- | | 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; ```