From 597eb3821de45a02fe5820266dbc87069162211a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 6 Aug 2007 10:52:37 +0200 Subject: Added motor models. --- digital/asserv/models/compare_motors.m | 43 +++++++ digital/asserv/models/inductance.m | 37 ++++++ digital/asserv/models/limit_current.m | 44 ++++++++ digital/asserv/models/motor.m | 200 +++++++++++++++++++++++++++++++++ digital/asserv/models/pwm.m | 23 ++++ 5 files changed, 347 insertions(+) create mode 100644 digital/asserv/models/compare_motors.m create mode 100644 digital/asserv/models/inductance.m create mode 100644 digital/asserv/models/limit_current.m create mode 100644 digital/asserv/models/motor.m create mode 100644 digital/asserv/models/pwm.m diff --git a/digital/asserv/models/compare_motors.m b/digital/asserv/models/compare_motors.m new file mode 100644 index 00000000..a8dee3f3 --- /dev/null +++ b/digital/asserv/models/compare_motors.m @@ -0,0 +1,43 @@ +% Compare different motors. +motor; + +%global to_file = 1; + +function u = control (t) + global sys; + if (t < 1) + u = sys.motor.u; + else + u = 0; + end +end + +global sys; +sys.motor = re40g; +sys.gear = gear4x; +sys.load = robot10w80; +sys.load.J /= 2; % Two motors. +sys.control.func = "control"; + +plotterm ("re40g"); +sys_plot (2); + +global sys; +sys.motor = re25g; +sys.gear = gear20x; +sys.load = robot10w80; +sys.load.J /= 2; % Two motors. +sys.control.func = "control"; + +plotterm ("re25g"); +sys_plot (2); + +global sys; +sys.motor = re25cll; +sys.gear = gear10x; +sys.load = robot10w80; +sys.load.J /= 2; % Two motors. +sys.control.func = "control"; + +plotterm ("re25cll"); +sys_plot (2); diff --git a/digital/asserv/models/inductance.m b/digital/asserv/models/inductance.m new file mode 100644 index 00000000..edc8b777 --- /dev/null +++ b/digital/asserv/models/inductance.m @@ -0,0 +1,37 @@ +% Show influence of inductance when not doing PWM. +motor; + +%global to_file = 1; + +function u = control (t) + global sys; + if (t < 1) + u = sys.motor.u; + else + u = 0; + end +end + +global sys; +sys.motor = re40g; +sys.gear = gear4x; +sys.load = robot10w80; +sys.load.J /= 2; % Two motors. +sys.control.func = "control"; + +% With inductance. +plotterm ("w_induct"); +sys_plot (2); + +% With inductance zoom. +plotterm ("w_inductz"); +sys_plot (2 / 100); + +% Without inductance. +sys.motor.L = 0; +plotterm ("wo_induct"); +sys_plot (2); + +% Without inductance zoom. +plotterm ("wo_inductz"); +sys_plot (2 / 100); diff --git a/digital/asserv/models/limit_current.m b/digital/asserv/models/limit_current.m new file mode 100644 index 00000000..e3d3d790 --- /dev/null +++ b/digital/asserv/models/limit_current.m @@ -0,0 +1,44 @@ +% Limit current. +motor; + +%global to_file = 1; + +function u = onoff (t) + global sys; + if (t < 1) + u = sys.control.u; + else + u = 0; + end +end + +function u = limit_current (t, x) + global sys; + u = onoff (t); + % Limit current by knowing the current speed and the motor terminal + % resistance. + umax = sys.control.current * sys.motor.R + x(2) / sys.motor.Kw; + umin = - sys.control.current * sys.motor.R + x(2) / sys.motor.Kw; + % Uncomment this if the maximum current can be overshooted when braking: + %umax = max ([umax, 0]); + %umin = min ([umin, 0]); + u = min ([u, umax]); + u = max ([u, umin]); +end + +global sys; +sys.motor = re40g; +sys.gear = gear4x; +sys.load = robot10w80; +sys.load.J /= 2; % Two motors. +sys.control.func = "limit_current"; +sys.control.current = 20; +sys.control.u = sys.motor.u; + +plotterm ("limit"); +sys_plot (2); + +sys.control.func = "onoff"; + +plotterm ("nolimit"); +sys_plot (2); diff --git a/digital/asserv/models/motor.m b/digital/asserv/models/motor.m new file mode 100644 index 00000000..8f501a75 --- /dev/null +++ b/digital/asserv/models/motor.m @@ -0,0 +1,200 @@ +% Motor models. + +% Maxon RE40G 150W 24V 148877 +re40g.u = 24; % Nominal voltage (V). +re40g.Kw = 317 * 2*pi / 60; % Speed constant ((rad/s)/V). +re40g.Km = 30.2 / 1000; % Torque constant (N.m/A). +re40g.Rf = 0; % Bearing friction (N.m/(rad/s)). +re40g.R = 0.316; % Terminal resistance (Ohm). +re40g.L = 0.08 / 1000; % Terminal inductance (H). +re40g.J = 134 / 1000 / 10000; % Rotor moment of inertia (kg.m^2). + +% Maxon RE25G 20W 24V 118752 +re25g.u = 24; % Nominal voltage (V). +re25g.Kw = 407 * 2*pi / 60; % Speed constant ((rad/s)/V). +re25g.Km = 23.4 / 1000; % Torque constant (N.m/A). +re25g.Rf = 0; % Bearing friction (N.m/(rad/s)). +re25g.R = 2.32; % Terminal resistance (Ohm). +re25g.L = 0.24 / 1000; % Terminal inductance (H). +re25g.J = 10.3 / 1000 / 10000; % Rotor moment of inertia (kg.m^2). + +% Maxon RE25CLL 10W 12V 118743 +re25cll.u = 12; % Nominal voltage (V). +re25cll.Kw = 407 * 2*pi / 60; % Speed constant ((rad/s)/V). +re25cll.Km = 23.4 / 1000; % Torque constant (N.m/A). +re25cll.Rf = 0; % Bearing friction (N.m/(rad/s)). +re25cll.R = 2.18; % Terminal resistance (Ohm). +re25cll.L = 0.24 / 1000; % Terminal inductance (H). +re25cll.J = 10.3 / 1000 / 10000;% Rotor moment of inertia (kg.m^2). + +% x4 orthogonal gear box. +gear4x.i = 4; % Ratio. +gear4x.ro = 0.75; % Efficiency. + +% x20.25 epicycloid gear box. +gear20x.i = 20.25; % Ratio. +gear20x.ro = 0.75; % Efficiency. + +% x10 orthogonal gear box. +gear10x.i = 10; % Ratio. +gear10x.ro = 0.75; % Efficiency. + +% 10 kg robot with 80 mm wheels. +robot10w80.wr = 0.04; % Wheel radius (m). +robot10w80.m = 10; % Mass (kg). +robot10w80.J = robot10w80.m * robot10w80.wr ^ 2; + % Moment of inertia at wheel radius (kg.m^2). +%robot10w80.Rf = ; % Friction ?, TODO. + +% u(t) = R i(t) + L di(t)/dt + w(t)/Kw +% Jr dw(t)/dt = Km i(t) - Rf w(t) +% dth(t)/dt = w(t) +% +% t: Simulation time (s). +% u(t): Voltage at motor terminals (V). +% i(t): Current going trough the motor (A). +% w(t): Rotation speed (w for omega) (rad/s). +% th(t): Rotor position (th for theta) (rad). +% Kw: Speed constant ((rad/s)/V). +% Km: Torque constant (N.m/A). +% R: Motor terminal resistance (Ohm). +% L: Motor terminal inductance (H). +% Jr: Moment of inertia at rotor (with load) (kg.m^2). +% Jr = motor.J + load.J / gear.i ^ 2 / gear.ro +% Rf: Bearing friction (N.m/(rad/s)). + +% if L == 0: i(t) = 1/R (u(t) - w(t)/Kw) +% else: di(t)/dt = 1/L (u(t) - R i(t) - w(t)/Kw) +% dw(t)/dt = 1/Jr (Km i(t) - Rf w(t)) +% dth(t)/dt = w(t) + +% x = [ i(t); w(t); th(t) ] +% xdot = [ di(t)/dt; dw(t)/dt; dth(t)/dt ] +function xdot = sys_der (x, t) + global sys; + mo = sys.motor; + ge = sys.gear; + lo = sys.load; + + persistent t_progress = -1 + if (t_progress > t + sys.progress * 50) + t_progress = 0; + end + if (sys.progress && t > t_progress) + printf ("time: %f %d%%\r", t, t / sys.progress); + t_progress = t + sys.progress; + end + + u = feval (sys.control.func, t, x); + + i = x(1); + w = x(2); + th = x(3); + + Jr = mo.J + lo.J / ge.i ** 2 / ge.ro; + + if (mo.L == 0) + i = 1 / mo.R * (u - w / mo.Kw); + xdot(1) = i; % will ignore this one. + else + xdot(1) = 1 / mo.L * (u - mo.R * i - w / mo.Kw); + end + xdot(2) = 1 / Jr * (mo.Km * i - mo.Rf * w); + xdot(3) = w; +end + +% When no feedback needed, resolve equation in one go. +function [t, x] = __sys_ode_simple__ (dur, steps) + % first, last, number of values + t = linspace (0, dur, steps + 1)'; + x0 = [ 0; 0; 0 ]; + x = lsode ("sys_der", x0, t); +end + +% When feedback function needed, resolve equation step by step. +function [t, x] = __sys_ode_feedback__ (dur, steps) + global sys; + t = linspace (0, dur, steps + 1)'; + x = zeros (steps + 1, 3); + for i = 1:steps + feval (sys.feedback.func, x(i, :), t(i)); + %tx = lsode ("sys_der", x(i, :), t(i : i + 1)); + %x(i + 1, :) = tx(2, :); + xdot = sys_der (x(i, :), t(i)); + x(i + 1, :) = x(i, :) + (t(i + 1) - t(i)) * xdot; + end +end + +% Plot the current system. +function sys_plot (dur, steps) + global sys; + % Default number of steps. + if (nargin < 2) + steps = 1024; + end + % Prepare progress meter. + sys.progress = dur / 100; + % Resolve differential equation. + if (isfield (sys, "feedback")) + [t, x] = __sys_ode_feedback__ (dur, steps); + else + [t, x] = __sys_ode_simple__ (dur, steps); + end + % Plot. + multiplot (2, 2); + if (sys.motor.L == 0) + i = zeros (length(t), 1); + for j = 1:length(t) + x(j, 1) = sys_der (x(j, :), t(j))(1); + end + end + mplot (t, x(:, 1), "1;current (A);"); + if (0) % plot dw + dw = zeros (length(t), 1); + for i = 1:length(t) + xdot = sys_der (x(i, :), t(i)); + dw(i) = xdot(2); + end + mplot (t, dw, "2;dw;"); + else + mplot (t, x(:, 2) * 60/2/pi, "2;rpm;"); + end + mplot (t, x(:, 2) / sys.gear.i * sys.load.wr, "3;speed (m/s);"); + mplot (t, x(:, 3) / sys.gear.i * sys.load.wr, "4;distance (m);"); + multiplot (0, 0); + % Finish progress meter. + printf ("\n"); +end + +% Setup plot for x11 or png depending of to_file global. +function plotterm (title) + persistent win = -1; + global to_file; + if (to_file) + __gnuplot_set__ term png; + eval (sprintf ('__gnuplot_set__ output "%s.png"', title)); + else + win += 1; + eval (sprintf ('__gnuplot_set__ term x11 %d title "%s" persist', + win, title)); + end +end + +%% Control functions. + +% Constant voltage. +function u = constant (t) + global sys; + u = sys.control.u; +end + +% PWM voltage. +function u = pwm (t) + global sys; + if (mod (t, sys.control.period) < sys.control.pwm * sys.control.period) + u = sys.motor.u; + else + u = 0; + end +end + diff --git a/digital/asserv/models/pwm.m b/digital/asserv/models/pwm.m new file mode 100644 index 00000000..30d3d94b --- /dev/null +++ b/digital/asserv/models/pwm.m @@ -0,0 +1,23 @@ +% Show influence of PWM control over voltage control. +motor; + +%global to_file = 1; + +global sys; +sys.motor = re40g; +sys.gear = gear4x; +sys.load = robot10w80; +sys.load.J /= 2; % Two motors. +sys.control.func = "pwm"; +sys.control.period = 1/20000; + +% PWM: 50% +sys.control.pwm = 0.5; +plotterm ("pwm"); +sys_plot (10/20000); + +% Voltage: 50% +sys.control.pwm = 1; +sys.motor.u *= 0.5; +plotterm ("voltage"); +sys_plot (10/20000); -- cgit v1.2.3