265 lines
8.5 KiB
C#
265 lines
8.5 KiB
C#
using UnityEngine;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEditor;
|
|
using System.Linq;
|
|
using System;
|
|
|
|
public class MotorPlot : MonoBehaviour {
|
|
|
|
private Vector3[] plot_current;
|
|
private Vector3[] plot_rps;
|
|
private Vector3[] plot_power;
|
|
private Vector3[] plot_in_power;
|
|
private Vector3[] plot_eff;
|
|
|
|
|
|
public Motor motor;
|
|
|
|
const float plotHeight = 10.0f;
|
|
const float plotWidth = plotHeight * 1.25f;
|
|
|
|
Vector3[] CreatePlot(Plot plot) {
|
|
Vector3[] result = new Vector3[plot.Samples.Count];
|
|
for (int i = 0; i < plot.Samples.Count; ++i) {
|
|
float x = (plotWidth / plot.Samples.Count) * i;
|
|
result[i] = new Vector3(x, 0, (float)(plot.Samples[i] * plotHeight * plot.NormalizeValue));
|
|
}
|
|
return result;
|
|
}
|
|
|
|
public class Plot {
|
|
public Plot(double _NormalizeValue) {
|
|
NormalizeValue = _NormalizeValue;
|
|
}
|
|
public double NormalizeValue;
|
|
public List<double> Samples = new List<double>();
|
|
|
|
public double GetMin() {
|
|
return Samples.Min();
|
|
}
|
|
|
|
public double GetMinNormalized() {
|
|
return Samples.Min() * NormalizeValue;
|
|
}
|
|
|
|
public double GetMaxNormalized() {
|
|
return Samples.Max() * NormalizeValue;
|
|
}
|
|
|
|
public double GetMax() {
|
|
return Samples.Max();
|
|
}
|
|
}
|
|
|
|
public class Plots {
|
|
public Plot current = new Plot(1000.0 / 200.0);
|
|
public Plot rpm = new Plot(1.0 / 24000.0);
|
|
public Plot power = new Plot((1.0 * 1000.0f / 150.0));
|
|
public Plot in_power = new Plot((1.0 * 1000.0f / 150.0));
|
|
public Plot eff = new Plot(100.0 / 54.0);
|
|
}
|
|
|
|
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;
|
|
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) / plotWidth;
|
|
var iter_rpm_derivative = (plots.rpm.GetMinNormalized() - (plots.rpm.Samples[plots.rpm.Samples.Count / 2] * plots.rpm.NormalizeValue)) / (plotWidth / 2);
|
|
|
|
var target_cur_derivative = (target_cur_max_norm - target_cur_min_norm) / plotWidth;
|
|
var iter_cur_derivative = (plots.current.GetMaxNormalized() - (plots.current.Samples[plots.current.Samples.Count / 2] * plots.current.NormalizeValue)) / (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, 6, 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.minlmsetbc(state, FunctionParametersMin, FunctionParametersMax);
|
|
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(motor.integrationStep);
|
|
}
|
|
motor.Vin = oldVoltage;
|
|
plot_rps = CreatePlot(result.rpm);
|
|
plot_current = CreatePlot(result.current);
|
|
SceneView.RepaintAll();
|
|
}
|
|
|
|
public Plots IntegratePlots() {
|
|
Plots result = new Plots();
|
|
motor.ResetState();
|
|
|
|
motor.LoadTorque = 0;
|
|
double loadStep = 0.04185 * motor.integrationStep;
|
|
|
|
//warm up
|
|
for (int i = 0; i < (int)(1.0f / motor.integrationStep) * 100; ++i) {
|
|
motor.Integrate(motor.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;
|
|
|
|
result.current.Samples.Add(motor.current);
|
|
result.rpm.Samples.Add(motor.angularSpeedRPM);
|
|
result.eff.Samples.Add(eff);
|
|
result.power.Samples.Add(power);
|
|
result.in_power.Samples.Add(inPower);
|
|
|
|
for (int m = 0; m < 10; ++m) {
|
|
motor.Integrate(motor.integrationStep);
|
|
}
|
|
|
|
}
|
|
Debug.Log("Load torque = " + motor.LoadTorque);
|
|
return result;
|
|
}
|
|
|
|
public void IntegratePlotsVisual() {
|
|
var plots = IntegratePlots();
|
|
|
|
//convert values to plots
|
|
plot_current = CreatePlot(plots.current);
|
|
plot_rps = CreatePlot(plots.rpm);
|
|
plot_power = CreatePlot(plots.power);
|
|
plot_in_power = CreatePlot(plots.in_power);
|
|
plot_eff = CreatePlot(plots.eff);
|
|
|
|
SceneView.RepaintAll();
|
|
}
|
|
|
|
void OnDrawGizmos() {
|
|
Handles.matrix = Matrix4x4.identity;
|
|
Handles.color = Color.red;
|
|
if (plot_current != null) {
|
|
Handles.DrawPolyLine(plot_current);
|
|
}
|
|
|
|
Handles.color = Color.blue;
|
|
if (plot_rps != null) {
|
|
Handles.DrawPolyLine(plot_rps);
|
|
}
|
|
|
|
Handles.color = Color.yellow;
|
|
if (plot_power != null) {
|
|
Handles.DrawPolyLine(plot_power);
|
|
}
|
|
|
|
Handles.color = Color.green;
|
|
if (plot_eff != null) {
|
|
Handles.DrawPolyLine(plot_eff);
|
|
}
|
|
|
|
Handles.color = Color.gray;
|
|
if (plot_in_power != null) {
|
|
Handles.DrawPolyLine(plot_in_power);
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|