65 lines
1.9 KiB
C#
65 lines
1.9 KiB
C#
|
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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private void Reset(){
|
||
|
|
ResetState();
|
||
|
|
}
|
||
|
|
|
||
|
|
public void ResetState() {
|
||
|
|
angularSpeedRPM = 0;
|
||
|
|
angularSpeedRPS = 0;
|
||
|
|
current = 0;
|
||
|
|
torque = 0;
|
||
|
|
LoadTorque = 0;
|
||
|
|
previousSpeedDerivative = 0;
|
||
|
|
previousCurrentDerivative = 0;
|
||
|
|
}
|
||
|
|
}
|