driving_physics 0.0.1.2 → 0.0.3.1
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- checksums.yaml +4 -4
- data/README.md +23 -1
- data/Rakefile +84 -0
- data/VERSION +1 -1
- data/demo/car.rb +85 -72
- data/demo/gearbox.rb +4 -3
- data/demo/motor.rb +12 -10
- data/demo/mruby/car.rb +184 -0
- data/demo/mruby/motor.rb +140 -0
- data/demo/pid_controller.rb +143 -0
- data/demo/powertrain.rb +1 -1
- data/driving_physics.gemspec +2 -1
- data/lib/driving_physics/car.rb +14 -5
- data/lib/driving_physics/cli.rb +7 -35
- data/lib/driving_physics/cockpit.rb +167 -0
- data/lib/driving_physics/disk.rb +10 -30
- data/lib/driving_physics/environment.rb +2 -0
- data/lib/driving_physics/gearbox.rb +62 -32
- data/lib/driving_physics/motor.rb +218 -63
- data/lib/driving_physics/mruby.rb +48 -0
- data/lib/driving_physics/pid_controller.rb +111 -0
- data/lib/driving_physics/powertrain.rb +8 -13
- data/lib/driving_physics/scalar_force.rb +2 -1
- data/lib/driving_physics/timer.rb +34 -0
- data/lib/driving_physics/tire.rb +6 -6
- data/lib/driving_physics/vector_force.rb +23 -0
- data/lib/driving_physics.rb +28 -22
- data/test/disk.rb +23 -59
- data/test/gearbox.rb +36 -0
- data/test/motor.rb +236 -0
- data/test/powertrain.rb +21 -0
- data/test/scalar_force.rb +3 -6
- data/test/tire.rb +25 -53
- data/test/vector_force.rb +76 -37
- metadata +28 -4
@@ -13,22 +13,36 @@ module DrivingPhysics
|
|
13
13
|
# Likewise, 1st gear is a _smaller_ gear ratio than 3rd
|
14
14
|
class Gearbox
|
15
15
|
class Disengaged < RuntimeError; end
|
16
|
+
class ClutchDisengage < Disengaged; end
|
16
17
|
|
17
18
|
RATIOS = [1/5r, 2/5r, 5/9r, 5/7r, 1r, 5/4r]
|
18
19
|
FINAL_DRIVE = 11/41r # 1/3.73
|
20
|
+
REVERSE = -1
|
21
|
+
REVERSE_RATIO = -1/10r
|
19
22
|
|
20
|
-
attr_accessor :
|
23
|
+
attr_accessor :ratios, :final_drive, :spinner, :fixed_mass
|
24
|
+
attr_reader :gear, :clutch
|
25
|
+
|
26
|
+
def self.gear_interval!(gear, min: REVERSE, max:)
|
27
|
+
if gear < min or gear > max
|
28
|
+
raise(ArgumentError, format("gear %s should be between %d and %d",
|
29
|
+
gear.inspect, min, max))
|
30
|
+
end
|
31
|
+
raise(ArgumentError, "gear should be an integer") if !gear.is_a? Integer
|
32
|
+
gear
|
33
|
+
end
|
21
34
|
|
22
35
|
def initialize(env)
|
23
36
|
@ratios = RATIOS
|
24
37
|
@final_drive = FINAL_DRIVE
|
25
|
-
@gear = 0
|
38
|
+
@gear = 0 # neutral
|
39
|
+
@clutch = 1.0 # fully engaged (clutch pedal out)
|
26
40
|
|
27
41
|
# represent all rotating mass
|
28
42
|
@spinner = Disk.new(env) { |m|
|
29
43
|
m.radius = 0.15
|
30
|
-
m.base_friction = 5/
|
31
|
-
m.omega_friction = 15/
|
44
|
+
m.base_friction = 5.0/1000
|
45
|
+
m.omega_friction = 15.0/100_000
|
32
46
|
m.mass = 15
|
33
47
|
}
|
34
48
|
@fixed_mass = 30 # kg
|
@@ -36,6 +50,10 @@ module DrivingPhysics
|
|
36
50
|
yield self if block_given?
|
37
51
|
end
|
38
52
|
|
53
|
+
def clutch=(val)
|
54
|
+
@clutch = DrivingPhysics.unit_interval! val
|
55
|
+
end
|
56
|
+
|
39
57
|
# given torque, apply friction, determine alpha
|
40
58
|
def alpha(torque, omega: 0)
|
41
59
|
@spinner.alpha(torque + @spinner.rotating_friction(omega))
|
@@ -53,57 +71,69 @@ module DrivingPhysics
|
|
53
71
|
@fixed_mass + @spinner.mass
|
54
72
|
end
|
55
73
|
|
74
|
+
def gear=(val)
|
75
|
+
@gear = self.class.gear_interval!(val, max: self.top_gear)
|
76
|
+
end
|
77
|
+
|
56
78
|
def top_gear
|
57
79
|
@ratios.length
|
58
80
|
end
|
59
81
|
|
60
82
|
def to_s
|
61
|
-
[
|
83
|
+
[self.inputs,
|
84
|
+
format("Ratios: %s", @ratios.inspect),
|
62
85
|
format(" Final: %s Mass: %.1f kg Rotating: %.1f kg",
|
63
86
|
@final_drive.inspect, self.mass, @spinner.mass),
|
64
87
|
].join("\n")
|
65
88
|
end
|
66
89
|
|
90
|
+
def inputs
|
91
|
+
format("Gear: %d Clutch: %.1f%%", @gear, @clutch * 100)
|
92
|
+
end
|
93
|
+
|
67
94
|
def ratio(gear = nil)
|
68
95
|
gear ||= @gear
|
69
|
-
|
70
|
-
|
96
|
+
case gear
|
97
|
+
when REVERSE
|
98
|
+
REVERSE_RATIO * @final_drive
|
99
|
+
when 0
|
100
|
+
raise(Disengaged, "Cannot determine gear ratio")
|
101
|
+
else
|
102
|
+
@ratios.fetch(gear - 1) * @final_drive
|
103
|
+
end
|
71
104
|
end
|
72
105
|
|
73
106
|
def axle_torque(crank_torque)
|
74
|
-
crank_torque / self.ratio
|
107
|
+
crank_torque * @clutch / self.ratio
|
75
108
|
end
|
76
109
|
|
77
|
-
def
|
78
|
-
|
110
|
+
def output_torque(crank_torque, crank_rpm, axle_omega: nil)
|
111
|
+
axle_alpha = self.alpha(self.axle_torque(crank_torque),
|
112
|
+
omega: self.axle_omega(crank_rpm,
|
113
|
+
axle_omega: axle_omega))
|
114
|
+
self.implied_torque(axle_alpha)
|
79
115
|
end
|
80
116
|
|
81
|
-
|
82
|
-
|
83
|
-
|
84
|
-
|
85
|
-
|
86
|
-
|
87
|
-
proportion = diff.to_f / old_rpm
|
88
|
-
if proportion.abs < 0.01
|
89
|
-
[:ok, proportion]
|
90
|
-
elsif proportion.abs < 0.1
|
91
|
-
[:slip, proportion]
|
92
|
-
elsif @gear == 1 and new_rpm < old_rpm and old_rpm <= 1500
|
93
|
-
[:get_rolling, proportion]
|
94
|
-
else
|
95
|
-
[:mismatch, proportion]
|
117
|
+
# take into account the old axle_omega and @clutch
|
118
|
+
def axle_omega(crank_rpm, axle_omega: nil)
|
119
|
+
new_axle_omega = DrivingPhysics.omega(crank_rpm) * self.ratio
|
120
|
+
if axle_omega.nil?
|
121
|
+
raise(ClutchDisengage, "cannot determine axle omega") if @clutch != 1.0
|
122
|
+
return new_axle_omega
|
96
123
|
end
|
124
|
+
diff = new_axle_omega - axle_omega
|
125
|
+
axle_omega + diff * @clutch
|
97
126
|
end
|
98
127
|
|
99
|
-
|
100
|
-
|
101
|
-
|
102
|
-
|
103
|
-
|
104
|
-
|
105
|
-
|
128
|
+
# take into account the old crank_rpm and @clutch
|
129
|
+
# crank will tolerate mismatch more than axle
|
130
|
+
def crank_rpm(axle_omega, crank_rpm: nil)
|
131
|
+
new_crank_rpm = DrivingPhysics.rpm(axle_omega) / self.ratio
|
132
|
+
if crank_rpm.nil?
|
133
|
+
raise(ClutchDisengage, "cannot determine crank rpm") if @clutch != 1.0
|
134
|
+
return new_crank_rpm
|
106
135
|
end
|
136
|
+
crank_rpm + (new_crank_rpm - crank_rpm) * @clutch
|
107
137
|
end
|
108
138
|
end
|
109
139
|
end
|
@@ -1,103 +1,258 @@
|
|
1
1
|
require 'driving_physics/disk'
|
2
2
|
|
3
3
|
module DrivingPhysics
|
4
|
+
def self.interpolate(x, xs:, ys:)
|
5
|
+
raise("Xs size #{xs.size}; Ys size #{ys.size}") unless xs.size == ys.size
|
6
|
+
raise("#{x} out of range") if x < xs.first or x > xs.last
|
7
|
+
xs.each.with_index { |xi, i|
|
8
|
+
return ys[i] if x == xi
|
9
|
+
if i > 0
|
10
|
+
last_x, last_y = xs[i-1], ys[i-1]
|
11
|
+
raise("xs out of order (#{xi} <= #{last_x})") unless xi > last_x
|
12
|
+
if x <= xi
|
13
|
+
proportion = Rational(x - last_x) / (xi - last_x)
|
14
|
+
return last_y + (ys[i] - last_y) * proportion
|
15
|
+
end
|
16
|
+
end
|
17
|
+
}
|
18
|
+
raise("couldn't find #{x} in #{xs.inspect}") # sanity check
|
19
|
+
end
|
20
|
+
|
21
|
+
class TorqueCurve
|
22
|
+
RPMS = [500, 1000, 1500, 2000, 2500, 3500, 5000, 6000, 7000, 7100]
|
23
|
+
TORQUES = [ 0, 70, 130, 200, 250, 320, 330, 320, 260, 0]
|
24
|
+
RPM_IDX = {
|
25
|
+
min: 0,
|
26
|
+
idle: 1,
|
27
|
+
redline: -2,
|
28
|
+
max: -1,
|
29
|
+
}
|
30
|
+
|
31
|
+
def self.validate_rpms!(rpms)
|
32
|
+
raise("rpms should be positive") if rpms.any? { |r| r < 0 }
|
33
|
+
rpms.each.with_index { |r, i|
|
34
|
+
if i > 0 and r <= rpms[i-1]
|
35
|
+
raise("rpms #{rpms.inspect} should increase")
|
36
|
+
end
|
37
|
+
}
|
38
|
+
rpms
|
39
|
+
end
|
40
|
+
|
41
|
+
def self.validate_torques!(torques)
|
42
|
+
raise("first torque should be zero") unless torques.first == 0
|
43
|
+
raise("last torque should be zero") unless torques.last == 0
|
44
|
+
raise("torques should be positive") if torques.any? { |t| t < 0 }
|
45
|
+
torques
|
46
|
+
end
|
47
|
+
|
48
|
+
def initialize(rpms: RPMS, torques: TORQUES)
|
49
|
+
if rpms.size != torques.size
|
50
|
+
raise("RPMs size #{rpms.size}; Torques size #{torques.size}")
|
51
|
+
end
|
52
|
+
@rpms = self.class.validate_rpms! rpms
|
53
|
+
@torques = self.class.validate_torques! torques
|
54
|
+
peak_torque = 0
|
55
|
+
idx = 0
|
56
|
+
@torques.each.with_index { |t, i|
|
57
|
+
if t > peak_torque
|
58
|
+
peak_torque = t
|
59
|
+
idx = i
|
60
|
+
end
|
61
|
+
}
|
62
|
+
@peak = idx
|
63
|
+
end
|
64
|
+
|
65
|
+
def peak
|
66
|
+
[@rpms[@peak], @torques[@peak]]
|
67
|
+
end
|
68
|
+
|
69
|
+
def to_s
|
70
|
+
@rpms.map.with_index { |r, i|
|
71
|
+
format("%s RPM %s Nm",
|
72
|
+
r.to_s.rjust(5, ' '),
|
73
|
+
@torques[i].round(1).to_s.rjust(4, ' '))
|
74
|
+
}.join("\n")
|
75
|
+
end
|
76
|
+
|
77
|
+
RPM_IDX.each { |name, idx| define_method(name) do @rpms[idx] end }
|
78
|
+
|
79
|
+
# interpolate based on torque curve points
|
80
|
+
def torque(rpm)
|
81
|
+
DrivingPhysics.interpolate(rpm, xs: @rpms, ys: @torques)
|
82
|
+
end
|
83
|
+
end
|
84
|
+
|
85
|
+
# represent all rotating mass as one big flywheel
|
4
86
|
class Motor
|
5
87
|
class Stall < RuntimeError; end
|
6
88
|
class OverRev < RuntimeError; end
|
7
|
-
class SanityCheck < RuntimeError; end
|
8
89
|
|
9
|
-
|
10
|
-
RPMS = [500, 1000, 1500, 2000, 2500, 3500, 5000, 6000, 7000, 7100]
|
11
|
-
ENGINE_BRAKING = 0.2 # 20% of the torque at a given RPM
|
90
|
+
CLOSED_THROTTLE = 0.01 # threshold for engine braking
|
12
91
|
|
13
|
-
|
14
|
-
|
15
|
-
|
92
|
+
# What percentage of the nominal torque at a given RPM would slow the
|
93
|
+
# motor when off throttle? 0% at 0 RPM; 50% at 7000 RPM
|
94
|
+
def self.engine_braking(rpm)
|
95
|
+
(rpm / 14_000.0).clamp(0, 0.5)
|
96
|
+
end
|
16
97
|
|
17
|
-
|
18
|
-
|
98
|
+
attr_reader :env, :torque_curve, :throttle
|
99
|
+
attr_accessor :fixed_mass, :spinner, :starter_torque
|
19
100
|
|
20
|
-
|
21
|
-
|
22
|
-
|
101
|
+
# Originally, torque_curve was a kwarg; but mruby currently has a bug
|
102
|
+
# where block_given? returns true in the presence of an unset default
|
103
|
+
# kwarg, or something like that.
|
104
|
+
# https://github.com/mruby/mruby/issues/5741
|
105
|
+
#
|
106
|
+
def initialize(env, torque_curve = nil)
|
107
|
+
@env = env
|
108
|
+
@torque_curve = torque_curve.nil? ? TorqueCurve.new : torque_curve
|
109
|
+
@throttle = 0.0 # 0.0 - 1.0 (0% - 100%)
|
23
110
|
|
24
|
-
|
25
|
-
@
|
26
|
-
fly.
|
27
|
-
fly.
|
28
|
-
fly.base_friction
|
29
|
-
fly.omega_friction =
|
111
|
+
@fixed_mass = 125
|
112
|
+
@flywheel = Disk.new(@env) { |fly|
|
113
|
+
fly.mass = 12
|
114
|
+
fly.radius = 0.20
|
115
|
+
fly.base_friction = 1.0 / 1_000
|
116
|
+
fly.omega_friction = 5.0 / 10_000
|
30
117
|
}
|
118
|
+
@crankshaft = Disk.new(@env) { |crank|
|
119
|
+
crank.mass = 25
|
120
|
+
crank.radius = 0.04
|
121
|
+
crank.base_friction = 1.0 / 1_000
|
122
|
+
crank.omega_friction = 5.0 / 10_000
|
123
|
+
}
|
124
|
+
|
31
125
|
@starter_torque = 500 # Nm
|
32
|
-
|
33
|
-
|
126
|
+
|
127
|
+
yield self if block_given?
|
128
|
+
end
|
129
|
+
|
130
|
+
def redline
|
131
|
+
@torque_curve.redline
|
132
|
+
end
|
133
|
+
|
134
|
+
def idle
|
135
|
+
@torque_curve.idle
|
34
136
|
end
|
35
137
|
|
36
138
|
def to_s
|
37
|
-
|
38
|
-
|
39
|
-
|
40
|
-
|
41
|
-
|
42
|
-
|
43
|
-
|
44
|
-
|
45
|
-
|
46
|
-
|
139
|
+
peak_rpm, peak_tq = *@torque_curve.peak
|
140
|
+
[format("Peak Torque: %d Nm @ %d RPM Redline: %d",
|
141
|
+
peak_tq, peak_rpm, @torque_curve.redline),
|
142
|
+
format(" Throttle: %s Mass: %.1f kg (%d kg fixed)",
|
143
|
+
self.throttle_pct, self.mass, @fixed_mass),
|
144
|
+
format(" Crankshaft: %s", @crankshaft),
|
145
|
+
format(" Flywheel: %s", @flywheel),
|
146
|
+
].join("\n")
|
147
|
+
end
|
148
|
+
|
149
|
+
def inertia
|
150
|
+
@crankshaft.rotational_inertia + @flywheel.rotational_inertia
|
151
|
+
end
|
152
|
+
|
153
|
+
def energy(omega)
|
154
|
+
@crankshaft.energy(omega) + @flywheel.energy(omega)
|
47
155
|
end
|
48
156
|
|
49
|
-
def
|
50
|
-
@
|
157
|
+
def friction(omega)
|
158
|
+
@crankshaft.rotating_friction(omega) +
|
159
|
+
@flywheel.rotating_friction(omega)
|
51
160
|
end
|
52
161
|
|
53
162
|
def mass
|
54
|
-
@
|
163
|
+
@fixed_mass + self.rotating_mass
|
164
|
+
end
|
165
|
+
|
166
|
+
def rotating_mass
|
167
|
+
@crankshaft.mass + @flywheel.mass
|
55
168
|
end
|
56
169
|
|
57
170
|
def throttle=(val)
|
58
|
-
|
59
|
-
raise(ArgumentError, "val #{val.inspect} should be between 0 and 1")
|
60
|
-
end
|
61
|
-
@throttle = val
|
171
|
+
@throttle = DrivingPhysics.unit_interval! val
|
62
172
|
end
|
63
173
|
|
64
|
-
|
174
|
+
def throttle_pct(places = 1)
|
175
|
+
format("%.#{places}f%%", @throttle * 100)
|
176
|
+
end
|
177
|
+
|
178
|
+
# given torque, determine crank alpha considering inertia and friction
|
65
179
|
def alpha(torque, omega: 0)
|
66
|
-
|
180
|
+
(torque + self.friction(omega)) / self.inertia
|
67
181
|
end
|
68
182
|
|
69
183
|
def implied_torque(alpha)
|
70
|
-
|
184
|
+
alpha * self.inertia
|
71
185
|
end
|
72
186
|
|
73
|
-
|
187
|
+
def output_torque(rpm)
|
188
|
+
self.torque(rpm) + self.friction(DrivingPhysics.omega(rpm))
|
189
|
+
end
|
190
|
+
|
191
|
+
# this is our "input torque" and it depends on @throttle
|
192
|
+
# here is where engine braking is implemented
|
74
193
|
def torque(rpm)
|
75
|
-
raise(Stall, "RPM #{rpm}") if rpm < @
|
76
|
-
raise(OverRev, "RPM #{rpm}") if rpm > @
|
77
|
-
|
78
|
-
|
79
|
-
|
80
|
-
torque = nil
|
81
|
-
|
82
|
-
# ew; there must be a better way
|
83
|
-
@rpms.each_with_index { |r, i|
|
84
|
-
tq = @torques[i]
|
85
|
-
if last_rpm <= rpm and rpm <= r
|
86
|
-
proportion = Rational(rpm - last_rpm) / (r - last_rpm)
|
87
|
-
torque = last_tq + (tq - last_tq) * proportion
|
88
|
-
break
|
89
|
-
end
|
90
|
-
last_rpm = r
|
91
|
-
last_tq = tq
|
92
|
-
}
|
93
|
-
raise(SanityCheck, "RPM #{rpm}") if torque.nil?
|
194
|
+
raise(Stall, "RPM #{rpm}") if rpm < @torque_curve.min
|
195
|
+
raise(OverRev, "RPM #{rpm}") if rpm > @torque_curve.max
|
196
|
+
|
197
|
+
# interpolate based on torque curve points
|
198
|
+
torque = @torque_curve.torque(rpm)
|
94
199
|
|
95
|
-
if (@throttle <=
|
96
|
-
|
97
|
-
-1 * torque * ENGINE_BRAKING
|
200
|
+
if (@throttle <= CLOSED_THROTTLE)
|
201
|
+
-1 * torque * self.class.engine_braking(rpm)
|
98
202
|
else
|
99
203
|
torque * @throttle
|
100
204
|
end
|
101
205
|
end
|
102
206
|
end
|
103
207
|
end
|
208
|
+
|
209
|
+
|
210
|
+
# TODO: starter motor
|
211
|
+
# Starter motor is power limited, not torque limited
|
212
|
+
# Consider:
|
213
|
+
# * 2.2 kW (3.75:1 gear reduction)
|
214
|
+
# * 1.8 kW (4.4:1 gear reduction)
|
215
|
+
# On a workbench, a starter will draw 80 to 90 amps. However, during actual
|
216
|
+
# start-up of an engine, a starter will draw 250 to 350 amps.
|
217
|
+
# from https://www.motortrend.com/how-to/because-theres-more-to-a-starter-than-you-realize/
|
218
|
+
|
219
|
+
# V - Potential, voltage
|
220
|
+
# I - Current, amperage
|
221
|
+
# R - Resistance, ohms
|
222
|
+
# P - Power, wattage
|
223
|
+
|
224
|
+
# Ohm's law: I = V / R (where R is held constant)
|
225
|
+
# P = I * V
|
226
|
+
# For resistors (where R is helod constant)
|
227
|
+
# = I^2 * R
|
228
|
+
# = V^2 / R
|
229
|
+
|
230
|
+
|
231
|
+
# torque proportional to A
|
232
|
+
# speed proportional to V
|
233
|
+
|
234
|
+
|
235
|
+
# batteries are rated in terms of CCA - cold cranking amps
|
236
|
+
|
237
|
+
# P = I * V
|
238
|
+
# V = 12 (up to 14ish)
|
239
|
+
# I = 300
|
240
|
+
# P = 3600 or 3.6 kW
|
241
|
+
|
242
|
+
|
243
|
+
# A starter rated at e.g. 2.2 kW will use more power on initial cranking
|
244
|
+
# Sometimes up to 500 amperes are required, and some batteries will provide
|
245
|
+
# over 600 cold cranking amps
|
246
|
+
|
247
|
+
|
248
|
+
# Consider:
|
249
|
+
|
250
|
+
# V = 12
|
251
|
+
# R = resistance of battery, wiring, starter motor
|
252
|
+
# L = inductance (approx 0)
|
253
|
+
# I = current through motor
|
254
|
+
# Vc = proportional to omega
|
255
|
+
|
256
|
+
# rated power - Vc * I
|
257
|
+
# input power - V * I
|
258
|
+
# input power that is unable to be converted to output power is wasted as heat
|
@@ -0,0 +1,48 @@
|
|
1
|
+
# This is only intended for use on mruby
|
2
|
+
# It is a workaround for Timer's use of Process in cli.rb
|
3
|
+
#
|
4
|
+
module DrivingPhysics
|
5
|
+
module CLI
|
6
|
+
# returns user input as a string
|
7
|
+
def self.prompt(msg = '')
|
8
|
+
print msg + ' ' unless msg.empty?
|
9
|
+
print '> '
|
10
|
+
$stdin.gets.chomp
|
11
|
+
end
|
12
|
+
|
13
|
+
# press Enter to continue, ignore input, return elapsed time
|
14
|
+
def self.pause(msg = '')
|
15
|
+
t = Timer.now
|
16
|
+
puts msg unless msg.empty?
|
17
|
+
puts ' [ Press Enter ]'
|
18
|
+
$stdin.gets
|
19
|
+
Timer.since(t)
|
20
|
+
end
|
21
|
+
end
|
22
|
+
|
23
|
+
module Timer
|
24
|
+
def self.now
|
25
|
+
Time.now
|
26
|
+
end
|
27
|
+
|
28
|
+
def self.since(t)
|
29
|
+
self.now - t
|
30
|
+
end
|
31
|
+
|
32
|
+
def self.elapsed(&work)
|
33
|
+
t = self.now
|
34
|
+
return yield, self.since(t)
|
35
|
+
end
|
36
|
+
|
37
|
+
# HH:MM:SS.mmm
|
38
|
+
def self.display(seconds: 0, ms: 0)
|
39
|
+
ms += (seconds * 1000).round if seconds > 0
|
40
|
+
DrivingPhysics.elapsed_display(ms)
|
41
|
+
end
|
42
|
+
|
43
|
+
def self.summary(start, num_ticks, paused = 0)
|
44
|
+
elapsed = self.since(start) - paused
|
45
|
+
format("%.3f s (%d ticks/s)", elapsed, num_ticks.to_f / elapsed)
|
46
|
+
end
|
47
|
+
end
|
48
|
+
end
|
@@ -0,0 +1,111 @@
|
|
1
|
+
module DrivingPhysics
|
2
|
+
# we will have a control loop
|
3
|
+
# SP - setpoint, this is the desired position
|
4
|
+
# PV(t) - process variable, this is the sensed position, varying over time
|
5
|
+
# e(t) - error, SP - PV
|
6
|
+
# CV(t) - control variable: the controller output
|
7
|
+
|
8
|
+
# for example, where to set the throttle to maintain 1000 RPM
|
9
|
+
# SP - 1000 RPM
|
10
|
+
# PV - current RPM
|
11
|
+
# CV - throttle position
|
12
|
+
|
13
|
+
class PIDController
|
14
|
+
HZ = 1000
|
15
|
+
TICK = Rational(1) / HZ
|
16
|
+
|
17
|
+
# Ziegler-Nichols method for tuning PID gain knobs
|
18
|
+
ZN = {
|
19
|
+
# Kp Ti Td Ki Kd
|
20
|
+
# Var: Ku Tu Tu Ku/Tu Ku*Tu
|
21
|
+
'P' => [0.500],
|
22
|
+
'PI' => [0.450, 0.800, nil, 0.540],
|
23
|
+
'PD' => [0.800, nil, 0.125, nil, 0.100],
|
24
|
+
'PID' => [0.600, 0.500, 0.125, 1.200, 0.075],
|
25
|
+
'PIR' => [0.700, 0.400, 0.150, 1.750, 0.105],
|
26
|
+
# less overshoot than standard PID below
|
27
|
+
'some' => [0.333, 0.500, 0.333, 0.666, 0.111],
|
28
|
+
'none' => [0.200, 0.500, 0.333, 0.400, 0.066],
|
29
|
+
}
|
30
|
+
|
31
|
+
# ultimate gain, oscillation
|
32
|
+
def self.tune(type, ku, tu)
|
33
|
+
record = ZN[type.downcase] || ZN[type.upcase] || ZN.fetch(type)
|
34
|
+
kp, ti, td, ki, kd = *record
|
35
|
+
kp *= ku if kp
|
36
|
+
ti *= tu if ti
|
37
|
+
td *= tu if td
|
38
|
+
ki *= (ku / tu) if ki
|
39
|
+
kd *= (ku * tu) if kd
|
40
|
+
{ kp: kp, ti: ti, td: td, ki: ki, kd: kd }
|
41
|
+
end
|
42
|
+
|
43
|
+
attr_accessor :kp, :ki, :kd, :dt, :setpoint,
|
44
|
+
:p_range, :i_range, :d_range, :o_range
|
45
|
+
attr_reader :measure, :error, :last_error, :sum_error
|
46
|
+
|
47
|
+
def initialize(setpoint, dt: TICK)
|
48
|
+
@setpoint, @dt, @measure = setpoint, dt, 0.0
|
49
|
+
|
50
|
+
# track error over time for integral and derivative
|
51
|
+
@error, @last_error, @sum_error = 0.0, 0.0, 0.0
|
52
|
+
|
53
|
+
# gain / multipliers for PID; tunables
|
54
|
+
@kp, @ki, @kd = 1.0, 1.0, 1.0
|
55
|
+
|
56
|
+
# optional clamps for PID terms and output
|
57
|
+
@p_range = (-Float::INFINITY..Float::INFINITY)
|
58
|
+
@i_range = (-Float::INFINITY..Float::INFINITY)
|
59
|
+
@d_range = (-Float::INFINITY..Float::INFINITY)
|
60
|
+
@o_range = (-Float::INFINITY..Float::INFINITY)
|
61
|
+
|
62
|
+
yield self if block_given?
|
63
|
+
end
|
64
|
+
|
65
|
+
def update(measure)
|
66
|
+
self.measure = measure
|
67
|
+
self.output
|
68
|
+
end
|
69
|
+
|
70
|
+
def measure=(val)
|
71
|
+
@measure = val
|
72
|
+
@last_error = @error
|
73
|
+
@error = @setpoint - @measure
|
74
|
+
if @error * @last_error <= 0 # zero crossing; reset the accumulated error
|
75
|
+
@sum_error = @error
|
76
|
+
else
|
77
|
+
@sum_error += @error
|
78
|
+
end
|
79
|
+
end
|
80
|
+
|
81
|
+
def output
|
82
|
+
(self.proportion +
|
83
|
+
self.integral +
|
84
|
+
self.derivative).clamp(@o_range.begin, @o_range.end)
|
85
|
+
end
|
86
|
+
|
87
|
+
def proportion
|
88
|
+
(@kp * @error).clamp(@p_range.begin, @p_range.end)
|
89
|
+
end
|
90
|
+
|
91
|
+
def integral
|
92
|
+
(@ki * @sum_error * @dt).clamp(@i_range.begin, @i_range.end)
|
93
|
+
end
|
94
|
+
|
95
|
+
def derivative
|
96
|
+
(@kd * (@error - @last_error) / @dt).clamp(@d_range.begin, @d_range.end)
|
97
|
+
end
|
98
|
+
|
99
|
+
def to_s
|
100
|
+
[format("Setpoint: %.3f Measure: %.3f",
|
101
|
+
@setpoint, @measure),
|
102
|
+
format("Error: %+.3f\tLast: %+.3f\tSum: %+.3f",
|
103
|
+
@error, @last_error, @sum_error),
|
104
|
+
format(" Gain:\t%.3f\t%.3f\t%.3f",
|
105
|
+
@kp, @ki, @kd),
|
106
|
+
format(" PID:\t%+.3f\t%+.3f\t%+.3f\t= %.5f",
|
107
|
+
self.proportion, self.integral, self.derivative, self.output),
|
108
|
+
].join("\n")
|
109
|
+
end
|
110
|
+
end
|
111
|
+
end
|
@@ -10,7 +10,7 @@ module DrivingPhysics
|
|
10
10
|
class Powertrain
|
11
11
|
attr_reader :motor, :gearbox
|
12
12
|
|
13
|
-
def initialize(motor
|
13
|
+
def initialize(motor:, gearbox:)
|
14
14
|
@motor = motor
|
15
15
|
@gearbox = gearbox
|
16
16
|
end
|
@@ -29,22 +29,17 @@ module DrivingPhysics
|
|
29
29
|
[t * o, t, o]
|
30
30
|
end
|
31
31
|
|
32
|
-
def axle_torque(rpm)
|
33
|
-
|
34
|
-
|
35
|
-
crank_torque = @motor.implied_torque(crank_alpha)
|
36
|
-
|
37
|
-
axle_alpha = @gearbox.alpha(@gearbox.axle_torque(crank_torque),
|
38
|
-
omega: @gearbox.axle_omega(rpm))
|
39
|
-
@gearbox.implied_torque(axle_alpha)
|
32
|
+
def axle_torque(rpm, axle_omega: nil)
|
33
|
+
@gearbox.output_torque(@motor.output_torque(rpm), rpm,
|
34
|
+
axle_omega: axle_omega)
|
40
35
|
end
|
41
36
|
|
42
|
-
def axle_omega(rpm)
|
43
|
-
@gearbox.axle_omega(rpm)
|
37
|
+
def axle_omega(rpm, axle_omega: nil)
|
38
|
+
@gearbox.axle_omega(rpm, axle_omega: axle_omega)
|
44
39
|
end
|
45
40
|
|
46
|
-
def crank_rpm(axle_omega)
|
47
|
-
@gearbox.crank_rpm(axle_omega)
|
41
|
+
def crank_rpm(axle_omega, crank_rpm: nil)
|
42
|
+
@gearbox.crank_rpm(axle_omega, crank_rpm: crank_rpm)
|
48
43
|
end
|
49
44
|
end
|
50
45
|
end
|
@@ -23,7 +23,8 @@ module DrivingPhysics
|
|
23
23
|
frontal_area: FRONTAL_AREA,
|
24
24
|
drag_cof: DRAG_COF,
|
25
25
|
air_density: AIR_DENSITY)
|
26
|
-
|
26
|
+
(speed ** 2) * (speed > 0 ? -1 : 1) *
|
27
|
+
0.5 * frontal_area * drag_cof * air_density
|
27
28
|
end
|
28
29
|
|
29
30
|
# opposes the direction of speed
|