Preparation for motor simulation
Jload = Marm
Larm² ⁄ 3
+ Mweight
Larm²
Tload(α) =
Mweight Larm g sin(α)
Tmot = KM I − B ω
ω' =
(Tmot −
Tload ⁄ (ηρ))
⁄ (Jmot
+ Jgear
+ Jload ⁄ ρ²)
Back to the page top
Practice in motor simulation
| Motor: maxon 118637 | ||
|---|---|---|
| (RE 13 φ13mm, Graphite Brushes, 3W) | ||
| Vo | 12 [V] | |
| ωo | 1371.83 [rad/s] | (13100 [rpm]) |
| Io | 0.0444 [A] | (44.4 [mA]) |
| R | 9.07 [Ω] | |
| KM | 0.842×10-2 [Nm/A] | (8.42 [mNm/A]) |
| Jmot | 0.541×10-7 [kgm²] | (0.541 [gcm²]) |
| Gear: maxon 110315 | ||
|---|---|---|
| (Planetary Gearhead GP 13 A) | ||
| ρ | 67.49 | (185193 / 2744) |
| η | 0.75 | (75 [%]) |
| Jgear | 0.15×10-8 [kgm²] | (0.015 [gcm²] |
double t = 0.0, Δtsim = 0.0001;
double ω = 0.0, θ = 0.0; α = 0.0;
double Tload ()
{
return Mweight * Larm * G * sin(α);
}
double Jload ()
{
return Marm * Larm * Larm / 3 + Mweight * Larm * Larm;
}
double motor (double V)
{
double I, Tmot, Tload, Jload, ω';
I = (V - K * ω) / R;
Tmot = K * I - (K * Io / ωo) * ω;
ω' = (Tmot - Tload()/(η*ρ)) / (Jmot + Jgear + Jload()/(ρ*ρ));
return ω';
}
void sim_step (double V)
{
double ωold;
ωold = ω;
ω += motor(V) * Δtsim;
θ += (ω + ωold) / 2 * Δtsim;
α = θ / ρ;
}
void sim_loop (double V, double Δt)
{
double tsim_end
tsim_end = t + Δt;
while (t < tsim_end) {
printf("%f %f¥n", t, α);
sim_step(V);
t += Δtsim;
}
}
double ω = 0.0, θ = 0.0; α = 0.0;
double Tload ()
{
return Mweight * Larm * G * sin(α);
}
double Jload ()
{
return Marm * Larm * Larm / 3 + Mweight * Larm * Larm;
}
double motor (double V)
{
double I, Tmot, Tload, Jload, ω';
I = (V - K * ω) / R;
Tmot = K * I - (K * Io / ωo) * ω;
ω' = (Tmot - Tload()/(η*ρ)) / (Jmot + Jgear + Jload()/(ρ*ρ));
return ω';
}
void sim_step (double V)
{
double ωold;
ωold = ω;
ω += motor(V) * Δtsim;
θ += (ω + ωold) / 2 * Δtsim;
α = θ / ρ;
}
void sim_loop (double V, double Δt)
{
double tsim_end
tsim_end = t + Δt;
while (t < tsim_end) {
printf("%f %f¥n", t, α);
sim_step(V);
t += Δtsim;
}
}
Closed-loop motor simulation
| Encoder: maxon 241062 | ||
|---|---|---|
| (Encoder MR, Type S, 256 pulses per turn, 2 channels) | ||
| Nenc | 256 [pulses/turn] | (1024 [counts/turn]) |
V = Kp e
double Δtc = 0.001;
double ENC_get ()
{
return floor(θ / dθ);
}
void cont_step_p (double Kp, double θgoal)
{
double e, V;
e = θgoal - ENC_get() * dθ
V = Kp * e;
if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
sim_loop(V, Δtc);
}
void cont_loop (double Kp, double αgoal, double tend)
{
while (t < tend) {
printf("%f %f\n", t, α);
cont_step_p(Kp, αgoal * ρ);
}
}
double ENC_get ()
{
return floor(θ / dθ);
}
void cont_step_p (double Kp, double θgoal)
{
double e, V;
e = θgoal - ENC_get() * dθ
V = Kp * e;
if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
sim_loop(V, Δtc);
}
void cont_loop (double Kp, double αgoal, double tend)
{
while (t < tend) {
printf("%f %f\n", t, α);
cont_step_p(Kp, αgoal * ρ);
}
}
V = Kp e
+ Ki ∫ e dt
V = Kp e
+ Ki
Σe(τ)Δtc
void cont_step_pi (double Kp, double Ki, double θgoal)
{
double e, Vp, V;
static double Vi = 0.0;
e = θgoal - ENC_get() * dθ
Vp = Kp * e;
Vi += Ki * e * Δtc;
if (Vi > Vmp) Vi = Vmp; else if (Vi < -Vmp) Vi = -Vmp;
V = Vi + Vp;
if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
sim_loop(V, Δtc);
}
void cont_loop (double Kp, double Ki, double αgoal, double tend)
{
while (t < tend) {
printf("%f %f\n", t, α);
cont_step_pi(Kp, Ki, αgoal * ρ);
}
}
{
double e, Vp, V;
static double Vi = 0.0;
e = θgoal - ENC_get() * dθ
Vp = Kp * e;
Vi += Ki * e * Δtc;
if (Vi > Vmp) Vi = Vmp; else if (Vi < -Vmp) Vi = -Vmp;
V = Vi + Vp;
if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
sim_loop(V, Δtc);
}
void cont_loop (double Kp, double Ki, double αgoal, double tend)
{
while (t < tend) {
printf("%f %f\n", t, α);
cont_step_pi(Kp, Ki, αgoal * ρ);
}
}
V = Kp e
+ Kd (de ⁄ dt)
V = Kp e
+ Kd
(e(t) − e(t−1))
⁄ Δtc
V = Kp e
− Kd (dθ ⁄ dt)
= Kp e
− Kd
(θ(t) − θ(t−1)) ⁄ Δtc
void cont_step_pd (double Kp, double Kd, double θgoal)
{
double θ, e, V;
static double θold = 0.0;
θ = ENC_get() * dθ;
e = θgoal - θ;
V = Kp * e - Kd * (θ - θold) / Δtc;
if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
sim_step(V);
θold = θ;
}
void cont_loop (double Kp, double Kd, double αgoal, double tend)
{
while (t < tend) {
printf("%f %f\n", t, α);
cont_step_pd(Kp, Kd, αgoal * ρ);
}
}
{
double θ, e, V;
static double θold = 0.0;
θ = ENC_get() * dθ;
e = θgoal - θ;
V = Kp * e - Kd * (θ - θold) / Δtc;
if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
sim_step(V);
θold = θ;
}
void cont_loop (double Kp, double Kd, double αgoal, double tend)
{
while (t < tend) {
printf("%f %f\n", t, α);
cont_step_pd(Kp, Kd, αgoal * ρ);
}
}
V = Kp e
+ Ki ∫ e dt
+ Kd (dθ ⁄ dt)
void cont_step_pid (double Kp, double Ki, double Kd,
double θgoal)
{
double θ, e, Vp, Vd, V;
static double Vi = 0.0;
static double θold = 0.0;
θ = ENC_get() * dθ;
e = θgoal - θ;
Vp = Kp * e;
Vi += Ki * e * Δtc;
if (Vi > Vmp) Vi = Vmp; else if (Vi < -Vmp) Vi = -Vmp;
Vd = - Kd * (θ - θold) / Δtc;
V = Vp + Vi + Vd;
if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
sim_loop(V, Δtc);
θold = θ;
}
void cont_loop (double Kp, double Ki, double Kd, double αgoal, double tend)
{
while (t < tend) {
printf("%f %f\n", t, α);
cont_step_pid(Kp, Ki, Kd, αgoal * ρ);
}
}
{
double θ, e, Vp, Vd, V;
static double Vi = 0.0;
static double θold = 0.0;
θ = ENC_get() * dθ;
e = θgoal - θ;
Vp = Kp * e;
Vi += Ki * e * Δtc;
if (Vi > Vmp) Vi = Vmp; else if (Vi < -Vmp) Vi = -Vmp;
Vd = - Kd * (θ - θold) / Δtc;
V = Vp + Vi + Vd;
if (V > Vmp) V = Vmp; else if (V < -Vmp) V = -Vmp;
sim_loop(V, Δtc);
θold = θ;
}
void cont_loop (double Kp, double Ki, double Kd, double αgoal, double tend)
{
while (t < tend) {
printf("%f %f\n", t, α);
cont_step_pid(Kp, Ki, Kd, αgoal * ρ);
}
}
Motor simulation