*

Unity/Assets/simulation_framework/BrushDcMotor.cs

@@ -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;
+    }
+}