354 lines
11 KiB
C#
354 lines
11 KiB
C#
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;
|
|
}
|
|
}
|