-
Notifications
You must be signed in to change notification settings - Fork 2
/
Program.cs
166 lines (137 loc) · 7.08 KB
/
Program.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
using System;
using System.IO;
using System.Linq;
using ClosedXML.Excel;
using UnitsNet;
using static System.Math;
using SearchAThing;
using UnitsNet.Units;
// in this example
// describe scurve shaped profile speed motion
// as function of EXECUTION_TIME and ROUND_CNT
// note: cruise speed will not maintained, only increase and decrease immediately
namespace scurve_xlsx
{
class Program
{
static void Main(string[] args)
{
// nema17 - 24v datasheet
var MOTOR_TYPE = "nema17";
var MOTOR_VOLTAGE = ElectricPotentialDc.FromVoltsDc(24);
var SPEED_MAX = RotationalSpeed.FromRevolutionsPerMinute(200);
var TORQUE_MAX = Torque.FromNewtonMeters(0.4);
// problem data
var MASS = Mass.FromKilograms(2);
var LEVER_ARM_LEN = Length.FromCentimeters(1);
var ROUND_CNT = Angle.FromRevolutions(1d);
var EXECUTION_TIME = Duration.FromSeconds(2);
// config
var TIME_STEP = Duration.FromMilliseconds(1);
// speed required to achieve given ROUND_CNT in EXECUTION_TIME without cruise
var minTargetSpeed = 2 * ROUND_CNT / EXECUTION_TIME;
if (minTargetSpeed > SPEED_MAX)
{
System.Console.WriteLine($"W: given position {ROUND_CNT} round cannot established due to speed_max:{SPEED_MAX.RevolutionsPerSecond} rps vs actual required target speed:{minTargetSpeed.RevolutionsPerSecond} rps");
return;
}
// s:target speed
var s = minTargetSpeed;
// d:duration
var d = EXECUTION_TIME;
var minHoldingTorque = Torque.FromKilogramForceCentimeters(MASS.Kilograms * LEVER_ARM_LEN.Centimeters);
if (minHoldingTorque > TORQUE_MAX)
{
System.Console.WriteLine($"W: given mass {MASS} at lever arm distance {LEVER_ARM_LEN} generate {minHoldingTorque.KilogramForceCentimeters} kgfcm torque versus max {TORQUE_MAX.KilogramForceCentimeters} kgfcm");
return;
}
var minDynAccel = RotationalAcceleration.FromRevolutionsPerSecondSquared(4 * s.RevolutionsPerSecond / d.Seconds);
var I = MassMomentOfInertia.FromKilogramSquareCentimeters(MASS.Kilograms * Pow(LEVER_ARM_LEN.Centimeters, 2));
// torque = inertia * angularaccel
// F:[M*L*T-2]*r:[L] = I:[M*L2]*a:[T-2]
// [M*L2*T-2] = [M*L2*T-2]
var minDynTorque = Torque.FromKilogramForceMeters(I.KilogramSquareMeters * minDynAccel.RadiansPerSecondSquared);
if (minDynTorque > TORQUE_MAX)
{
System.Console.WriteLine($"W: accelerating given mass {MASS} at angaccel {minDynAccel} generates torque {minDynTorque.NewtonCentimeters} Ncm great than max {TORQUE_MAX.NewtonCentimeters} Ncm");
return;
}
var srcPathfilename = Path.Combine(AppDomain.CurrentDomain.BaseDirectory, "template.xlsx");
var dstPathfilename = "output.xlsx";
File.Copy(srcPathfilename, dstPathfilename, true);
using (var wb = new ClosedXML.Excel.XLWorkbook(dstPathfilename))
{
var ws = wb.Worksheets.First();
IXLCell cell = null;
Action<int, int, object> setCell = (r, c, val) =>
{
cell = ws.Cell(r, c);
cell.Value = val;
};
Action<int, int, object> setCellBold = (r, c, val) =>
{
cell = ws.Cell(r, c);
cell.Value = val;
cell.Style.Font.SetBold();
};
var row = 1;
int col = 1;
var colTime = col++;
var colAccel = col++;
var colSpeed = col++;
var colPosRot = col++;
setCellBold(row, colTime, "TIME (s)");
setCellBold(row, colAccel, "ACCEL (rps2)");
setCellBold(row, colSpeed, "SPEED (rps)");
setCellBold(row, colPosRot, "POS (rot)");
++row;
var t = Duration.FromSeconds(0);
var tMax = t + EXECUTION_TIME;
var halfTMax = tMax / 2;
ws.Cell("MotorType").Value = MOTOR_TYPE;
ws.Cell("MotorSpeedMax").Value = SPEED_MAX;
ws.Cell("MotorTorqueMaxAtSpeedMax").Value = TORQUE_MAX;
ws.Cell("MotorVoltage").Value = MOTOR_VOLTAGE;
ws.Cell("ProblemDuration").Value = EXECUTION_TIME;
ws.Cell("ProblemLoadLeverArmLength").Value = LEVER_ARM_LEN;
ws.Cell("ProblemLoadMass").Value = MASS;
ws.Cell("ProblemRevolutions").Value = ROUND_CNT;
ws.Cell("ResultingTorque").Value = minDynTorque.ToUnit(TorqueUnit.NewtonMeter);
ws.Cell("ResultingAccel").Value = minDynAccel;
ws.Cell("ResultingSpeedMax").Value = minTargetSpeed.ToUnit(RotationalSpeedUnit.RevolutionPerMinute);
var tEps = Duration.FromNanoseconds(1);
while (t.LessThanOrEqualsTol(tEps, tMax))
{
setCell(row, colTime, t.Seconds);
var accel = RotationalAcceleration.FromRevolutionsPerSecondSquared(0);
var speed = RotationalSpeed.FromRevolutionsPerSecond(0);
var pos = Angle.FromRevolutions(0);
if (t.LessThanOrEqualsTol(tEps, halfTMax))
{
accel = RotationalAcceleration.FromRevolutionsPerSecondSquared(
2 * s.RevolutionsPerSecond / d.Seconds * (1 - Cos(4 * PI * t / d)));
speed = RotationalSpeed.FromRevolutionsPerSecond(
2 * s.RevolutionsPerSecond / d.Seconds * (t - d * Sin(4 * PI * t / d) / (4 * PI)).Seconds);
pos = s * d * (Cos(4 * PI * t / d) - 1) / (8 * Pow(PI, 2)) + (s * t) * (t / d);
}
if (t.GreatThanOrEqualsTol(tEps, halfTMax))
{
var th = t - d / 2;
accel = RotationalAcceleration.FromRevolutionsPerSecondSquared(
2 * s.RevolutionsPerSecond / d.Seconds * (Cos(4 * PI * th / d) - 1));
speed = RotationalSpeed.FromRevolutionsPerSecond(
2 * s.RevolutionsPerSecond * Sin(4 * PI * th / d) / (4 * PI)
- (2 * s * th / d - s).RevolutionsPerSecond);
pos = s * d * (1 - Cos(4 * PI * th / d)) / (8 * Pow(PI, 2)) - (s * th) * (th / d) + s * th + s * d / 4;
}
setCell(row, colAccel, accel.RevolutionsPerSecondSquared);
setCell(row, colSpeed, speed.RevolutionsPerSecond);
setCell(row, colPosRot, pos.Revolutions);
++row;
t += TIME_STEP;
}
wb.Save();
}
}
}
}