*

2025-05-13 03:30:23 +03:00
parent 224cfc8cf6
commit 5c489bb6ee
554 changed files with 1114771 additions and 0 deletions
@@ -0,0 +1,62 @@
+using UnityEngine;
+using UnityEditor;
+using System.Collections.Generic;
+using System.Linq;
+using System;
+
+public class BrushDcMotor : MonoBehaviour {
+	public double maxRpm = 3000;
+    public double angularSpeedRPM;
+	public double angularSpeedRPS; //radians per second
+
+    [Header("[State]")]
+    public double current = 0.0f;
+    public double torque = 0.0f;
+    public double LoadTorque = 0.0;
+
+    [Header("[Editable variables]")]
+    public double k_Friction = 7.71074154118413e-06; // N.m.s //b
+    public double k_Torque = 1.0999964640773; // N.m/Amp
+    public double k_BackEMF = 0.00112523063632681;// V/rad/sec
+    public double rotorInertia = 5.5370674874546e-07;//kg.m^2
+    public double Vin = 3.0;//V
+    public double R = 16.6; //ohm
+    public double L = 0.0009; //H
+    private double previousSpeedDerivative;
+    private double previousCurrentDerivative;
+
+    public void Integrate(double dt) {
+        //calc derivatives
+        var currentDerivative = (Vin - current*R - k_BackEMF*angularSpeedRPS)/L;
+        var speedDerivative =  (-k_Friction * angularSpeedRPS + k_Torque * current - LoadTorque) / rotorInertia;
+
+        //trapezoidal integration
+        angularSpeedRPS += (speedDerivative + previousSpeedDerivative) * 0.5 * dt;
+        current += (currentDerivative + previousCurrentDerivative) * 0.5 * dt;
+
+        angularSpeedRPM = MathEx.RadSToRpm * angularSpeedRPS;
+
+        previousSpeedDerivative = speedDerivative;
+        previousCurrentDerivative = currentDerivative;
+    }
+
+    void FixedUpdate()
+    {
+        double dt = Time.fixedDeltaTime;
+        for (int i  = 0; i < 10; ++i)
+        {
+            Integrate(dt / 10.0);
+        }
+       
+    }
+
+    public void ResetState() {
+        angularSpeedRPM = 0;
+        angularSpeedRPS = 0;
+        current = 0;
+        torque = 0;
+        LoadTorque = 0;
+        previousSpeedDerivative = 0;
+        previousCurrentDerivative = 0;
+    }
+}
@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 38b9362aecb4bdc4fbf40e1683741b92
+timeCreated: 1454238836
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,9 @@
+fileFormatVersion: 2
+guid: 0baf01877d8194349a763a3ed41e6dd6
+folderAsset: yes
+timeCreated: 1454240991
+licenseType: Free
+DefaultImporter:
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,20 @@
+using UnityEngine;
+using UnityEditor;
+using System.Collections;
+
+[CustomEditor(typeof(BrushDcMotor))]
+public class BrushDcMotorDebug : Editor {
+
+    public override void OnInspectorGUI() {
+        base.OnInspectorGUI();
+
+
+        var motor = target as BrushDcMotor;
+        if (GUILayout.Button("Reset state"))
+        {
+            motor.angularSpeedRPM = 0;
+            motor.angularSpeedRPS = 0;
+            motor.current = 0;
+        }
+    }
+}
@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: ca39fde841ad1a741b2c15978b88ae36
+timeCreated: 1454239743
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,78 @@
+using UnityEngine;
+using System.Collections;
+using System.Diagnostics.CodeAnalysis;
+using System;
+
+public static class MathEx {
+    public delegate float Fx(float x);
+    public delegate float Fxy(float x, float y);
+
+    public static double RadSToRpm = (60.0 / (2.0 * Math.PI));
+    public static double RpmToRadS = ((2.0 * Math.PI) / 60.0);
+
+    public static double Torque(double Power, double angularVelocity)
+    {
+        return Power/angularVelocity;
+    }
+
+    public static Vector3 Project(this Ray aRay, Vector3 aPoint)
+    {
+        Vector3 V1 = aPoint - aRay.origin;
+        Vector3 V2 = Vector3.Project(V1, aRay.direction);
+        return aRay.origin + V2;
+    }
+
+    public static double Power(double Torque, double angularVelocity) {
+        return Torque * angularVelocity;
+    }
+
+    public static double Clamp(double value, double min, double max)
+    {
+        return value < min ? min : (value > max ? max : value);
+    }
+
+    public static double Clamp01(double value) {
+        return value < 0.0f ? 0.0f : (value > 1.0f ? 1.0f : value);
+    }
+
+    public static float FindExtremumAgument(Fx func, float xMin, float xMax, float xStep) {
+        float prevY = func(xMin);
+        float prevDerivative = 0;
+        while(xMin < xMax) {
+            float y = func(xMin);
+            float derivative = y - prevY;
+            if(derivative < 0.00001f) {
+                derivative = 0;
+            }
+
+            if((prevDerivative > 0) && (derivative <= 0)) {
+                break;
+            }
+            prevDerivative = derivative;
+            prevY = y;
+            xMin += xStep;
+        }
+        return xMin;
+    }
+
+    public static float FindExtremumValue(Fx func, float xMin, float xMax, float xStep) {
+        float prevY = func(xMin);
+        float prevDerivative = 0;
+        while(xMin < xMax) {
+            float y = func(xMin);
+            float derivative = y - prevY;
+            if(derivative < 0.00001f) {
+                derivative = 0;
+            }
+
+            if((prevDerivative > 0) && (derivative <= 0)) {
+                break;
+            }
+            prevDerivative = derivative;
+            prevY = y;
+            xMin += xStep;
+        }
+        return prevY;
+    }
+}
+
@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 8b9af6a3be49bb34ba8a37ac6e99b7b8
+timeCreated: 1430483489
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 519011c9f604d4546bbbb09900a9ba19
+timeCreated: 1454239895
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,59 @@
+using UnityEngine;
+using System.Collections;
+using System;
+
+[Serializable]
+public class PID {
+	public bool enabled = true;
+	public float kp = 1.0f;
+	public float ki = 1.0f;
+	public float kd = 1.0f;
+
+	public float outMin = -0.1f;
+	public float outMax = 0.1f;
+
+	private float _errorIntegral;
+	private float _lastError;
+	private float _setpoint;
+
+	public float LastError {get {return _lastError;}}
+
+	private float _p;
+	public float P {get {return _p;}}
+	private float _i;
+	public float I {get {return _i;}}
+	private float _d;
+	public float D {get {return _d;}}
+
+	public float Setpoint{
+		get{ return _setpoint; }
+		set{ _setpoint = value; }
+	}
+
+	public float Process(float dt, float pv){
+		if (!enabled) {
+			return 0;
+		}
+		float error = Setpoint - pv;
+		float derivative = (error - _lastError) / dt;
+
+		//derivative = Mathf.Clamp(derivative, outMin, outMax);
+
+		/*if (Mathf.Abs(integralChange) > 0.001f) {
+			integralChange = 0.001f * Mathf.Sign(integralChange);
+		}*/
+		_errorIntegral += error * dt;
+
+
+		//_errorIntegral = Mathf.Clamp(_errorIntegral, outMin, outMax);
+
+		_lastError = error;
+
+		_p = kp * error;
+		_i = ki * _errorIntegral;
+		_d = kd * derivative;
+
+		var output = _p + _i + _d;
+		return Mathf.Clamp(output, outMin, outMax);
+	}
+}
@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 9b9adb6e0ca362048bfb667033ce58d1
+timeCreated: 1454238838
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,9 @@
+fileFormatVersion: 2
+guid: 0716a095522aacd4aa2c4e26d36857a0
+folderAsset: yes
+timeCreated: 1454962741
+licenseType: Free
+DefaultImporter:
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,9 @@
+fileFormatVersion: 2
+guid: 46a12a90744060240bcc4055d3ecc3f4
+folderAsset: yes
+timeCreated: 1454962948
+licenseType: Free
+DefaultImporter:
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,23 @@
+using UnityEngine;
+using UnityEditor;
+using System.Collections;
+
+[CustomEditor(typeof(MotorPlot))]
+public class MotorPlotEditor : Editor {
+    public override void OnInspectorGUI() {
+        base.OnInspectorGUI();
+
+        if (GUILayout.Button("Update unbound plots")) {
+            (target as MotorPlot).DrawUnboundPlots();
+        }
+
+        if (GUILayout.Button("Update plots")) {
+            (target as MotorPlot).IntegratePlotsVisual();
+        }
+
+        if (GUILayout.Button("Optimize")) {
+            (target as MotorPlot).Optimize();
+        }
+    }
+}
+
@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 54857a5de4ef8e6429d006e8fdb1f801
+timeCreated: 1454962950
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,353 @@
+using UnityEngine;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEditor;
+using System.Linq;
+using System;
+
+public enum MotorPlotVariable {
+    Current,
+    AnglarSpeed,
+    OutPower,
+    InPower,
+    Efficiency,
+    Torque,
+    Voltage,
+    BackEMF
+}
+
+public enum AngularSpeedUnits {
+    RevPS,
+    RadPS,
+    RevPM
+}
+
+
+public enum MotorPlotConstrainType {
+    GlobalMin,
+    GlobalMax,
+    ValueAtX,
+    DerivAtX,
+    GlobalDeriv
+}
+
+[Serializable]
+public class MotorPlotConstrain {
+    public MotorPlotConstrainType type;
+    public double x = 0;
+}
+[Serializable]
+public class Plot
+{
+    public Color color = Color.blue;
+    public MotorPlotVariable variable;
+    public double minValue;
+    public double maxValue;
+
+    [HideInInspector]
+    public List<double> SamplesX = new List<double>();
+    [HideInInspector]
+    public List<double> SamplesY = new List<double>();
+
+    public double GetMinValue() {
+        return SamplesY.Min();
+    }
+
+    public double GetMaxValue() {
+        return SamplesY.Max();
+    }
+
+    public double NormalizeValue(double value) {
+        return (value - minValue) / (maxValue - minValue);
+    }
+
+    public double GlobalValue(double normalized_value) {
+        return normalized_value * (maxValue - minValue) + maxValue;
+    }
+
+    public double GetMinNormalized() {
+        return NormalizeValue(SamplesY.Min());
+    }
+
+    public double GetMaxNormalized() {
+        return NormalizeValue(SamplesY.Max());
+    }
+}
+
+public class MotorPlot : MonoBehaviour {
+    public MotorPlotConstrain[] constrains;
+    public Plot[] plots;
+
+    public BrushDcMotor motor;
+    public double integrationStep = 0.00001;
+    public AngularSpeedUnits angularSpeedUnits = AngularSpeedUnits.RevPM;
+
+    public float plotHeight = 10.0f;
+    public float plotWidth = 12.5f;
+    public int pointsSkip = 0;
+
+    public float plotMinX = 0;
+    public float plotMaxX = 1;
+
+    public MotorPlotVariable plotXVariable = MotorPlotVariable.Current;
+
+    private List<Vector3[]> plots_visual;
+
+    Vector3[] CreatePlot(Plot plot) {
+        Vector3[] result = new Vector3[plot.SamplesX.Count];
+        for (int i = 0; i < plot.SamplesX.Count; ++i) {
+            double x = ((plot.SamplesX[i] - plotMinX) / (plotMaxX - plotMinX)) * plotWidth;
+            result[i] = new Vector3((float)x, 0, (float)(plot.NormalizeValue(plot.SamplesY[i]) * plotHeight));
+        }
+        return result;
+    }
+
+    
+
+
+    private double k_torque_backup;
+    private double k_friction_backup;
+    private double k_backemf_backup;
+    public int maxOptimizationIterations = 10000;
+    public int integrationIterations = 1000;
+
+    private static void fvec(double[] arg, double[] fi, object obj) {
+        //errors:  eff_max, pow_max, rpm_max, cur_min, cur_max
+        //k_Friction, k_Torque, k_BackEMF, rotorInertia
+        var m = (MotorPlot)obj;
+
+        //calculate errors
+        for (int i = 0; i < m.constrains.Length; ++i) {
+            var c = m.constrains[i];
+            fi[i] = 0;
+            // if( c.type == MotorPlotConstrainType.
+            //fi[i] = 
+        }
+
+
+        /* 
+         m.motor.k_Friction = m.k_friction_backup + arg[0];
+         m.motor.k_Torque = m.k_torque_backup + arg[1];
+         m.motor.k_BackEMF = m.k_backemf_backup + arg[2];
+
+         var plots = m.IntegratePlots();
+
+         var eff_max_norm = plots.eff.GetMaxNormalized();
+         var pow_max_norm = plots.power.GetMaxNormalized();
+         var cur_max_norm = plots.current.GetMaxNormalized();
+
+         var target_eff_max_norm = (52.0 / 100.0) * plots.eff.NormalizeValue;
+         var target_pow_max_norm = (120.3 / 1000.0) * plots.power.NormalizeValue;
+         var target_rpm_max_norm = 23050.0 * plots.rpm.NormalizeValue;
+         var target_cur_min_norm = (17.0 / 1000.0) * plots.current.NormalizeValue;
+         var target_cur_max_norm = (180.0 / 1000.0) * plots.current.NormalizeValue;
+
+         var target_rpm_derivative = (0 - target_rpm_max_norm) / m.plotWidth;
+         var iter_rpm_derivative = (plots.rpm.GetMinNormalized() - (plots.rpm.Samples[plots.rpm.Samples.Count / 2] * plots.rpm.NormalizeValue)) / (m.plotWidth / 2);
+
+         var target_cur_derivative = (target_cur_max_norm - target_cur_min_norm) / m.plotWidth;
+         var iter_cur_derivative = (plots.current.GetMaxNormalized() - (plots.current.Samples[plots.current.Samples.Count / 2] * plots.current.NormalizeValue)) / (m.plotWidth / 2);
+
+         var err_eff = target_eff_max_norm - eff_max_norm;
+         var err_pow = target_pow_max_norm - pow_max_norm;
+         var err_rpm_deriv = target_rpm_derivative - iter_rpm_derivative;
+         var err_cur = target_cur_derivative - iter_cur_derivative;
+         var err_cur_max = target_cur_max_norm - cur_max_norm;
+
+         fi[0] = err_pow * err_pow;
+         fi[1] = err_rpm_deriv * err_rpm_deriv;
+         fi[2] = err_cur * err_cur;
+         fi[3] = err_cur_max * err_cur_max;
+         fi[4] = err_eff * err_eff;*/
+    }
+
+    public void Optimize() {
+        const double optStep = 0.0000001;
+        k_torque_backup = motor.k_Torque;
+        k_friction_backup = motor.k_Friction;
+        k_backemf_backup = motor.k_BackEMF;
+
+        double[] Params = new double[3] { 0, 0, 0 };
+
+        alglib.minlmstate state;
+        alglib.minlmreport rep;
+
+        try {
+            //params count, errors count, params
+            alglib.minlmcreatev(Params.Length, constrains.Length, Params, optStep, out state);
+        } catch (Exception ex) {
+            Debug.Log("Failed to optimize, bad params! " + ex.Message);
+            return;
+        }
+
+
+        alglib.minlmsetcond(state, 0.00000000001, 0.00000000001, 0.00000000001, maxOptimizationIterations);
+        alglib.minlmoptimize(state, fvec, null, this);
+        alglib.minlmresults(state, out Params, out rep);
+
+        motor.k_Friction = k_friction_backup + Params[0];
+        motor.k_Torque = k_torque_backup + Params[1];
+        motor.k_BackEMF = k_backemf_backup + Params[2];
+
+        Debug.Log(rep.iterationscount);
+        IntegratePlotsVisual();
+    }
+
+    public void DrawUnboundPlots() {
+        /*Plots result = new Plots();
+        motor.ResetState();
+
+        plot_eff = null;
+        plot_in_power = null;
+        plot_power = null;
+        plot_rps = null;
+        plot_current = null;
+
+        var oldVoltage = motor.Vin;
+
+        for (int i = 0; i < integrationIterations; ++i) {
+
+            if (i > (integrationIterations / 2)) {
+                motor.Vin = 0;
+            }
+
+            result.current.Samples.Add(motor.current);
+            result.rpm.Samples.Add(motor.angularSpeedRPM);
+            motor.Integrate(integrationStep);
+        }
+        motor.Vin = oldVoltage;
+        plot_rps = CreatePlot(result.rpm);
+        plot_current = CreatePlot(result.current);
+        SceneView.RepaintAll();*/
+    }
+
+    public double GetVariable(MotorPlotVariable var) {
+        switch (var) {
+            case MotorPlotVariable.AnglarSpeed:
+                if (angularSpeedUnits == AngularSpeedUnits.RevPM) {
+                    return motor.angularSpeedRPM;
+                } else if (angularSpeedUnits == AngularSpeedUnits.RadPS) {
+                    return motor.angularSpeedRPS;
+                } else {
+                    throw new Exception("Unit conversion is not implemented");
+                }
+            case MotorPlotVariable.Current:
+                return motor.current;
+            case MotorPlotVariable.Torque:
+                return motor.LoadTorque;
+            case MotorPlotVariable.OutPower:
+                return motor.angularSpeedRPS * motor.LoadTorque;//in Watts
+            case MotorPlotVariable.InPower:
+                return motor.current * motor.Vin;
+            case MotorPlotVariable.Efficiency:
+                return GetVariable(MotorPlotVariable.OutPower) / GetVariable(MotorPlotVariable.InPower);
+
+        }
+        return 0;
+    }
+
+    public double loadStepMul = 0.04185;
+    public bool IntegratePlots() {
+        if (plots == null || plots.Length == 0) {
+            Debug.LogWarning("No plots to integrate!");
+            return false;
+        }
+
+
+
+        motor.ResetState();
+        motor.LoadTorque = 0;
+        double loadStep = loadStepMul * integrationStep;
+
+        foreach (var p in plots) {
+            p.SamplesX.Clear();
+            p.SamplesY.Clear();
+        }
+
+        //warm up
+        for (int i = 0; i < (int)(1.0f / integrationStep) * 100; ++i) {
+            motor.Integrate(integrationStep);
+        }
+
+        int iteration = 0;
+
+        //add load torque until motor stall
+        while (motor.angularSpeedRPM > 0.0) {
+            ++iteration;
+            motor.LoadTorque = loadStep * iteration;
+            
+            //double power = (k_BackEMF / k_Torque) * angularSpeedRPS * (torque - torqueOffset);
+            /*double power = motor.angularSpeedRPS * motor.LoadTorque / 1000.0;
+              double inPower = motor.current * motor.Vin;
+              double eff = power / inPower;*/
+
+            if (iteration % (pointsSkip + 1) == 0) {
+                var xValue = GetVariable(plotXVariable);
+                foreach (var p in plots) {
+                    p.SamplesY.Add(GetVariable(p.variable));
+                    p.SamplesX.Add(xValue);
+                }
+            }
+
+            for (int m = 0; m < 10; ++m) {
+                motor.Integrate(integrationStep);
+            }
+        }
+        Debug.Log("Load torque = " + motor.LoadTorque);
+
+        Debug.Log(String.Format("{0,-25}{1,-25}{2,-25}{3,-25}{4,-25}", "Plot name", "Range min", "Range max", "Val min","Val max"));
+        //print plots
+        foreach (var p in plots)
+        {
+            var color = p.color.ToHex();
+            Debug.Log(String.Format("<color={5}>{0,-25}</color>{1,-25}{2,-25}{3,-25}{4,-25}", p.variable.ToString(), p.minValue, p.maxValue, p.GetMinValue(), p.GetMaxValue(), color));
+        }
+        return true;
+    }
+
+    public void IntegratePlotsVisual() {
+        if (!IntegratePlots())
+        {
+            return;
+        }
+        if (plots_visual == null) {
+            plots_visual = new List<Vector3[]>();
+        }
+        plots_visual.Clear();
+        foreach (var p in plots) {
+            plots_visual.Add(CreatePlot(p));
+        }
+        SceneView.RepaintAll();
+    }
+
+    void OnDrawGizmos() {
+        Handles.matrix = transform.localToWorldMatrix;
+
+        if (plots_visual != null && plots != null && plots_visual.Count == plots.Length)
+        {
+            for (int i = 0; i < plots.Length; ++i)
+            {
+                var v = plots_visual[i];
+                var p = plots[i];
+
+                if (p != null && v != null) {
+                    Handles.color = p.color;
+                    Handles.DrawPolyLine(v);
+                }
+            }
+        }
+
+
+        Handles.color = Color.black;
+        //draw borders
+        Vector3 upOffset = Vector3.forward * plotHeight;
+        Vector3 rightOffset = Vector3.right * plotWidth;
+
+        Handles.DrawLine(Vector3.zero, upOffset);
+        Handles.DrawLine(Vector3.zero, rightOffset);
+        Handles.DrawLine(upOffset, upOffset + rightOffset);
+        Handles.DrawLine(rightOffset, upOffset + rightOffset);
+
+        Handles.matrix = Matrix4x4.identity;
+    }
+}
@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 5980103fd5ec19c47a87e4cc2efe834a
+timeCreated: 1454962741
+licenseType: Free
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
@@ -0,0 +1,24 @@
+fileFormatVersion: 2
+guid: fc5e8a5b858560441ba60a30af68514d
+timeCreated: 1454238836
+licenseType: Free
+PluginImporter:
+  serializedVersion: 1
+  iconMap: {}
+  executionOrder: {}
+  isPreloaded: 0
+  platformData:
+    Any:
+      enabled: 1
+      settings: {}
+    Editor:
+      enabled: 0
+      settings:
+        DefaultValueInitialized: true
+    WindowsStoreApps:
+      enabled: 0
+      settings:
+        CPU: AnyCPU
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: