driving_physics 0.0.1.2 → 0.0.3.1

data/lib/driving_physics/gearbox.rb

@@ -13,22 +13,36 @@ module DrivingPhysics
   # Likewise, 1st gear is a _smaller_ gear ratio than 3rd
   class Gearbox
     class Disengaged < RuntimeError; end
+    class ClutchDisengage < Disengaged; end
 
     RATIOS = [1/5r, 2/5r, 5/9r, 5/7r, 1r, 5/4r]
     FINAL_DRIVE = 11/41r # 1/3.73
+    REVERSE = -1
+    REVERSE_RATIO = -1/10r
 
-    attr_accessor :gear, :ratios, :final_drive, :spinner, :fixed_mass
+    attr_accessor :ratios, :final_drive, :spinner, :fixed_mass
+    attr_reader :gear, :clutch
+
+    def self.gear_interval!(gear, min: REVERSE, max:)
+      if gear < min or gear > max
+        raise(ArgumentError, format("gear %s should be between %d and %d",
+                                    gear.inspect, min, max))
+      end
+      raise(ArgumentError, "gear should be an integer") if !gear.is_a? Integer
+      gear
+    end
 
     def initialize(env)
       @ratios = RATIOS
       @final_drive = FINAL_DRIVE
-      @gear = 0 # neutral
+      @gear = 0     # neutral
+      @clutch = 1.0 # fully engaged (clutch pedal out)
 
       # represent all rotating mass
       @spinner = Disk.new(env) { |m|
         m.radius = 0.15
-        m.base_friction = 5/1000r
-        m.omega_friction = 15/100_000r
+        m.base_friction = 5.0/1000
+        m.omega_friction = 15.0/100_000
         m.mass = 15
       }
       @fixed_mass = 30 # kg
@@ -36,6 +50,10 @@ module DrivingPhysics
       yield self if block_given?
     end
 
+    def clutch=(val)
+      @clutch = DrivingPhysics.unit_interval! val
+    end
+
     # given torque, apply friction, determine alpha
     def alpha(torque, omega: 0)
       @spinner.alpha(torque + @spinner.rotating_friction(omega))
@@ -53,57 +71,69 @@ module DrivingPhysics
       @fixed_mass + @spinner.mass
     end
 
+    def gear=(val)
+      @gear = self.class.gear_interval!(val, max: self.top_gear)
+    end
+
     def top_gear
       @ratios.length
     end
 
     def to_s
-      [format("Ratios: %s", @ratios.inspect),
+      [self.inputs,
+       format("Ratios: %s", @ratios.inspect),
        format(" Final: %s  Mass: %.1f kg  Rotating: %.1f kg",
               @final_drive.inspect, self.mass, @spinner.mass),
       ].join("\n")
     end
 
+    def inputs
+      format("Gear: %d  Clutch: %.1f%%", @gear, @clutch * 100)
+    end
+
     def ratio(gear = nil)
       gear ||= @gear
-      raise(Disengaged, "Cannot determine gear ratio") if @gear == 0
-      @ratios.fetch(gear - 1) * @final_drive
+      case gear
+      when REVERSE
+        REVERSE_RATIO * @final_drive
+      when 0
+        raise(Disengaged, "Cannot determine gear ratio")
+      else
+        @ratios.fetch(gear - 1) * @final_drive
+      end
     end
 
     def axle_torque(crank_torque)
-      crank_torque / self.ratio
+      crank_torque * @clutch / self.ratio
     end
 
-    def axle_omega(crank_rpm)
-      DrivingPhysics.omega(crank_rpm) * self.ratio
+    def output_torque(crank_torque, crank_rpm, axle_omega: nil)
+      axle_alpha = self.alpha(self.axle_torque(crank_torque),
+                              omega: self.axle_omega(crank_rpm,
+                                                     axle_omega: axle_omega))
+      self.implied_torque(axle_alpha)
     end
 
-    def crank_rpm(axle_omega)
-      DrivingPhysics.rpm(axle_omega) / self.ratio
-    end
-
-    def match_rpms(old_rpm, new_rpm)
-      diff = new_rpm - old_rpm
-      proportion = diff.to_f / old_rpm
-      if proportion.abs < 0.01
-        [:ok, proportion]
-      elsif proportion.abs < 0.1
-        [:slip, proportion]
-      elsif @gear == 1 and new_rpm < old_rpm and old_rpm <= 1500
-        [:get_rolling, proportion]
-      else
-        [:mismatch, proportion]
+    # take into account the old axle_omega and @clutch
+    def axle_omega(crank_rpm, axle_omega: nil)
+      new_axle_omega = DrivingPhysics.omega(crank_rpm) * self.ratio
+      if axle_omega.nil?
+        raise(ClutchDisengage, "cannot determine axle omega") if @clutch != 1.0
+        return new_axle_omega
       end
+      diff = new_axle_omega - axle_omega
+      axle_omega + diff * @clutch
     end
 
-    def next_gear(rpm, floor: 2500, ceiling: 6400)
-      if rpm < floor and @gear > 1
-        @gear - 1
-      elsif rpm > ceiling and @gear < self.top_gear
-        @gear + 1
-      else
-        @gear
+    # take into account the old crank_rpm and @clutch
+    # crank will tolerate mismatch more than axle
+    def crank_rpm(axle_omega, crank_rpm: nil)
+      new_crank_rpm = DrivingPhysics.rpm(axle_omega) / self.ratio
+      if crank_rpm.nil?
+        raise(ClutchDisengage, "cannot determine crank rpm") if @clutch != 1.0
+        return new_crank_rpm
       end
+      crank_rpm + (new_crank_rpm - crank_rpm) * @clutch
     end
   end
 end

data/lib/driving_physics/motor.rb

@@ -1,103 +1,258 @@
 require 'driving_physics/disk'
 
 module DrivingPhysics
+  def self.interpolate(x, xs:, ys:)
+    raise("Xs size #{xs.size}; Ys size #{ys.size}") unless xs.size == ys.size
+    raise("#{x} out of range") if x < xs.first or x > xs.last
+    xs.each.with_index { |xi, i|
+      return ys[i] if x == xi
+      if i > 0
+        last_x, last_y = xs[i-1], ys[i-1]
+        raise("xs out of order (#{xi} <= #{last_x})") unless xi > last_x
+        if x <= xi
+          proportion = Rational(x - last_x) / (xi - last_x)
+          return last_y + (ys[i] - last_y) * proportion
+        end
+      end
+    }
+    raise("couldn't find #{x} in #{xs.inspect}") # sanity check
+  end
+
+  class TorqueCurve
+    RPMS    = [500, 1000, 1500, 2000, 2500, 3500, 5000, 6000, 7000, 7100]
+    TORQUES = [  0,   70,  130,  200,  250,  320,  330,  320,  260,    0]
+    RPM_IDX = {
+      min: 0,
+      idle: 1,
+      redline: -2,
+      max: -1,
+    }
+
+    def self.validate_rpms!(rpms)
+      raise("rpms should be positive") if rpms.any? { |r| r < 0 }
+      rpms.each.with_index { |r, i|
+        if i > 0 and r <= rpms[i-1]
+          raise("rpms #{rpms.inspect} should increase")
+        end
+      }
+      rpms
+    end
+
+    def self.validate_torques!(torques)
+      raise("first torque should be zero") unless torques.first == 0
+      raise("last torque should be zero") unless torques.last == 0
+      raise("torques should be positive") if torques.any? { |t| t < 0 }
+      torques
+    end
+
+    def initialize(rpms: RPMS, torques: TORQUES)
+      if rpms.size != torques.size
+        raise("RPMs size #{rpms.size}; Torques size #{torques.size}")
+      end
+      @rpms = self.class.validate_rpms! rpms
+      @torques = self.class.validate_torques! torques
+      peak_torque = 0
+      idx = 0
+      @torques.each.with_index { |t, i|
+        if t > peak_torque
+          peak_torque = t
+          idx = i
+        end
+      }
+      @peak = idx
+    end
+
+    def peak
+      [@rpms[@peak], @torques[@peak]]
+    end
+
+    def to_s
+      @rpms.map.with_index { |r, i|
+        format("%s RPM %s Nm",
+               r.to_s.rjust(5, ' '),
+               @torques[i].round(1).to_s.rjust(4, ' '))
+      }.join("\n")
+    end
+
+    RPM_IDX.each { |name, idx| define_method(name) do @rpms[idx] end }
+
+    # interpolate based on torque curve points
+    def torque(rpm)
+      DrivingPhysics.interpolate(rpm, xs: @rpms, ys: @torques)
+    end
+  end
+
+  # represent all rotating mass as one big flywheel
   class Motor
     class Stall < RuntimeError; end
     class OverRev < RuntimeError; end
-    class SanityCheck < RuntimeError; end
 
-    TORQUES = [  0,   70,  130,  200,  250,  320,  330,  320,  260,    0]
-    RPMS    = [500, 1000, 1500, 2000, 2500, 3500, 5000, 6000, 7000, 7100]
-    ENGINE_BRAKING = 0.2 # 20% of the torque at a given RPM
+    CLOSED_THROTTLE = 0.01 # threshold for engine braking
 
-    attr_reader :env, :throttle
-    attr_accessor :torques, :rpms, :fixed_mass,
-                  :spinner, :starter_torque, :idle_rpm
+    # What percentage of the nominal torque at a given RPM would slow the
+    # motor when off throttle?    0% at 0 RPM; 50% at 7000 RPM
+    def self.engine_braking(rpm)
+      (rpm / 14_000.0).clamp(0, 0.5)
+    end
 
-    def initialize(env)
-      @env = env
+    attr_reader :env, :torque_curve, :throttle
+    attr_accessor :fixed_mass, :spinner, :starter_torque
 
-      @torques = TORQUES
-      @rpms = RPMS
-      @fixed_mass = 125
+    # Originally, torque_curve was a kwarg; but mruby currently has a bug
+    # where block_given? returns true in the presence of an unset default
+    # kwarg, or something like that.
+    # https://github.com/mruby/mruby/issues/5741
+    #
+    def initialize(env, torque_curve = nil)
+      @env          = env
+      @torque_curve = torque_curve.nil? ? TorqueCurve.new : torque_curve
+      @throttle     = 0.0  # 0.0 - 1.0 (0% - 100%)
 
-      # represent all rotating mass as one big flywheel
-      @spinner = Disk.new(@env) { |fly|
-        fly.radius =  0.25 # m
-        fly.mass   = 75    # kg
-        fly.base_friction  = 5/1000r
-        fly.omega_friction = 2/10_000r
+      @fixed_mass = 125
+      @flywheel = Disk.new(@env) { |fly|
+        fly.mass = 12
+        fly.radius = 0.20
+        fly.base_friction = 1.0 / 1_000
+        fly.omega_friction = 5.0 / 10_000
       }
+      @crankshaft = Disk.new(@env) { |crank|
+        crank.mass = 25
+        crank.radius = 0.04
+        crank.base_friction = 1.0 / 1_000
+        crank.omega_friction = 5.0 / 10_000
+      }
+
       @starter_torque = 500  # Nm
-      @idle_rpm       = 1000 # RPM
-      @throttle       = 0.0  # 0.0 - 1.0 (0% - 100%)
+
+      yield self if block_given?
+    end
+
+    def redline
+      @torque_curve.redline
+    end
+
+    def idle
+      @torque_curve.idle
     end
 
     def to_s
-      ary = [format("Throttle: %.1f%%", @throttle * 100)]
-      ary << "Torque:"
-      @rpms.each_with_index { |r, i|
-        ary << format("%s Nm %s RPM",
-                      @torques[i].round(1).to_s.rjust(4, ' '),
-                      r.to_s.rjust(5, ' '))
-      }
-      ary << format("Mass: %.1f kg  Fixed: %d kg", self.mass, @fixed_mass)
-      ary << format("Rotating: %s", @spinner)
-      ary.join("\n")
+      peak_rpm, peak_tq = *@torque_curve.peak
+      [format("Peak Torque: %d Nm @ %d RPM  Redline: %d",
+              peak_tq, peak_rpm, @torque_curve.redline),
+       format("   Throttle: %s  Mass: %.1f kg  (%d kg fixed)",
+              self.throttle_pct, self.mass, @fixed_mass),
+       format(" Crankshaft: %s", @crankshaft),
+       format("   Flywheel: %s", @flywheel),
+      ].join("\n")
+    end
+
+    def inertia
+      @crankshaft.rotational_inertia + @flywheel.rotational_inertia
+    end
+
+    def energy(omega)
+      @crankshaft.energy(omega) + @flywheel.energy(omega)
     end
 
-    def rotational_inertia
-      @spinner.rotational_inertia
+    def friction(omega)
+      @crankshaft.rotating_friction(omega) +
+        @flywheel.rotating_friction(omega)
     end
 
     def mass
-      @spinner.mass + @fixed_mass
+      @fixed_mass + self.rotating_mass
+    end
+
+    def rotating_mass
+      @crankshaft.mass + @flywheel.mass
     end
 
     def throttle=(val)
-      if val < 0.0 or val > 1.0
-        raise(ArgumentError, "val #{val.inspect} should be between 0 and 1")
-      end
-      @throttle = val
+      @throttle = DrivingPhysics.unit_interval! val
     end
 
-    # given torque, determine crank alpha after inertia and friction
+    def throttle_pct(places = 1)
+      format("%.#{places}f%%", @throttle * 100)
+    end
+
+    # given torque, determine crank alpha considering inertia and friction
     def alpha(torque, omega: 0)
-      @spinner.alpha(torque + @spinner.rotating_friction(omega))
+      (torque + self.friction(omega)) / self.inertia
     end
 
     def implied_torque(alpha)
-      @spinner.implied_torque(alpha)
+      alpha * self.inertia
     end
 
-    # interpolate based on torque curve points
+    def output_torque(rpm)
+      self.torque(rpm) + self.friction(DrivingPhysics.omega(rpm))
+    end
+
+    # this is our "input torque" and it depends on @throttle
+    # here is where engine braking is implemented
     def torque(rpm)
-      raise(Stall, "RPM #{rpm}") if rpm < @rpms.first
-      raise(OverRev, "RPM #{rpm}") if rpm > @rpms.last
-
-      last_rpm = 99999
-      last_tq = -1
-      torque = nil
-
-      # ew; there must be a better way
-      @rpms.each_with_index { |r, i|
-        tq = @torques[i]
-        if last_rpm <= rpm and rpm <= r
-          proportion = Rational(rpm - last_rpm) / (r - last_rpm)
-          torque = last_tq + (tq - last_tq) * proportion
-          break
-        end
-        last_rpm = r
-        last_tq = tq
-      }
-      raise(SanityCheck, "RPM #{rpm}") if torque.nil?
+      raise(Stall, "RPM #{rpm}") if rpm < @torque_curve.min
+      raise(OverRev, "RPM #{rpm}") if rpm > @torque_curve.max
+
+      # interpolate based on torque curve points
+      torque = @torque_curve.torque(rpm)
 
-      if (@throttle <= 0.05) and (rpm > @idle_rpm * 1.5)
-        # engine braking: 20% of torque
-        -1 * torque * ENGINE_BRAKING
+      if (@throttle <= CLOSED_THROTTLE)
+        -1 * torque * self.class.engine_braking(rpm)
       else
         torque * @throttle
       end
     end
   end
 end
+
+
+# TODO: starter motor
+# Starter motor is power limited, not torque limited
+# Consider:
+# * 2.2 kW (3.75:1 gear reduction)
+# * 1.8 kW  (4.4:1 gear reduction)
+# On a workbench, a starter will draw 80 to 90 amps. However, during actual
+# start-up of an engine, a starter will draw 250 to 350 amps.
+# from https://www.motortrend.com/how-to/because-theres-more-to-a-starter-than-you-realize/
+
+# V - Potential, voltage
+# I - Current, amperage
+# R - Resistance, ohms
+# P - Power, wattage
+
+# Ohm's law: I = V / R (where R is held constant)
+# P = I * V
+# For resistors (where R is helod constant)
+#   = I^2 * R
+#   = V^2 / R
+
+
+# torque proportional to A
+# speed proportional to V
+
+
+# batteries are rated in terms of CCA - cold cranking amps
+
+# P = I * V
+# V = 12 (up to 14ish)
+# I = 300
+# P = 3600 or 3.6 kW
+
+
+# A starter rated at e.g. 2.2 kW will use more power on initial cranking
+# Sometimes up to 500 amperes are required, and some batteries will provide
+# over 600 cold cranking amps
+
+
+# Consider:
+
+# V = 12
+# R = resistance of battery, wiring, starter motor
+# L = inductance (approx 0)
+# I = current through motor
+# Vc = proportional to omega
+
+# rated power - Vc * I
+# input power - V * I
+# input power that is unable to be converted to output power is wasted as heat

data/lib/driving_physics/mruby.rb

@@ -0,0 +1,48 @@
+# This is only intended for use on mruby
+# It is a workaround for Timer's use of Process in cli.rb
+#
+module DrivingPhysics
+  module CLI
+    # returns user input as a string
+    def self.prompt(msg = '')
+      print msg + ' ' unless msg.empty?
+      print '> '
+      $stdin.gets.chomp
+    end
+
+    # press Enter to continue, ignore input, return elapsed time
+    def self.pause(msg = '')
+      t = Timer.now
+      puts msg unless msg.empty?
+      puts '     [ Press Enter ]'
+      $stdin.gets
+      Timer.since(t)
+    end
+  end
+
+  module Timer
+    def self.now
+      Time.now
+    end
+
+    def self.since(t)
+      self.now - t
+    end
+
+    def self.elapsed(&work)
+      t = self.now
+      return yield, self.since(t)
+    end
+
+    # HH:MM:SS.mmm
+    def self.display(seconds: 0, ms: 0)
+      ms += (seconds * 1000).round if seconds > 0
+      DrivingPhysics.elapsed_display(ms)
+    end
+
+    def self.summary(start, num_ticks, paused = 0)
+      elapsed = self.since(start) - paused
+      format("%.3f s (%d ticks/s)", elapsed, num_ticks.to_f / elapsed)
+    end
+  end
+end

data/lib/driving_physics/pid_controller.rb

@@ -0,0 +1,111 @@
+module DrivingPhysics
+  # we will have a control loop
+  # SP    - setpoint, this is the desired position
+  # PV(t) - process variable, this is the sensed position, varying over time
+  #  e(t) - error, SP - PV
+  # CV(t) - control variable: the controller output
+
+  # for example, where to set the throttle to maintain 1000 RPM
+  # SP - 1000 RPM
+  # PV - current RPM
+  # CV - throttle position
+
+  class PIDController
+    HZ = 1000
+    TICK = Rational(1) / HZ
+
+    # Ziegler-Nichols method for tuning PID gain knobs
+    ZN = {
+      #            Kp     Ti     Td     Ki     Kd
+      #     Var:   Ku     Tu     Tu    Ku/Tu  Ku*Tu
+      'P'  =>   [0.500],
+      'PI' =>   [0.450, 0.800,   nil, 0.540],
+      'PD' =>   [0.800,   nil, 0.125,   nil, 0.100],
+      'PID' =>  [0.600, 0.500, 0.125, 1.200, 0.075],
+      'PIR' =>  [0.700, 0.400, 0.150, 1.750, 0.105],
+      # less overshoot than standard PID below
+      'some' => [0.333, 0.500, 0.333, 0.666, 0.111],
+      'none' => [0.200, 0.500, 0.333, 0.400, 0.066],
+    }
+
+    # ultimate gain, oscillation
+    def self.tune(type, ku, tu)
+      record = ZN[type.downcase] || ZN[type.upcase] || ZN.fetch(type)
+      kp, ti, td, ki, kd = *record
+      kp *= ku if kp
+      ti *= tu if ti
+      td *= tu if td
+      ki *= (ku / tu) if ki
+      kd *= (ku * tu) if kd
+      { kp: kp, ti: ti, td: td, ki: ki, kd: kd }
+    end
+
+    attr_accessor :kp, :ki, :kd, :dt, :setpoint,
+                  :p_range, :i_range, :d_range, :o_range
+    attr_reader :measure, :error, :last_error, :sum_error
+
+    def initialize(setpoint, dt: TICK)
+      @setpoint, @dt, @measure = setpoint, dt, 0.0
+
+      # track error over time for integral and derivative
+      @error, @last_error, @sum_error = 0.0, 0.0, 0.0
+
+      # gain / multipliers for PID; tunables
+      @kp, @ki, @kd = 1.0, 1.0, 1.0
+
+      # optional clamps for PID terms and output
+      @p_range = (-Float::INFINITY..Float::INFINITY)
+      @i_range = (-Float::INFINITY..Float::INFINITY)
+      @d_range = (-Float::INFINITY..Float::INFINITY)
+      @o_range = (-Float::INFINITY..Float::INFINITY)
+
+      yield self if block_given?
+    end
+
+    def update(measure)
+      self.measure = measure
+      self.output
+    end
+
+    def measure=(val)
+      @measure = val
+      @last_error = @error
+      @error = @setpoint - @measure
+      if @error * @last_error <= 0  # zero crossing; reset the accumulated error
+        @sum_error = @error
+      else
+        @sum_error += @error
+      end
+    end
+
+    def output
+      (self.proportion +
+       self.integral +
+       self.derivative).clamp(@o_range.begin, @o_range.end)
+    end
+
+    def proportion
+      (@kp * @error).clamp(@p_range.begin, @p_range.end)
+    end
+
+    def integral
+      (@ki * @sum_error * @dt).clamp(@i_range.begin, @i_range.end)
+    end
+
+    def derivative
+      (@kd * (@error - @last_error) / @dt).clamp(@d_range.begin, @d_range.end)
+    end
+
+    def to_s
+      [format("Setpoint: %.3f  Measure: %.3f",
+              @setpoint, @measure),
+       format("Error: %+.3f\tLast: %+.3f\tSum: %+.3f",
+              @error, @last_error, @sum_error),
+       format(" Gain:\t%.3f\t%.3f\t%.3f",
+              @kp, @ki, @kd),
+       format("  PID:\t%+.3f\t%+.3f\t%+.3f\t= %.5f",
+              self.proportion, self.integral, self.derivative, self.output),
+      ].join("\n")
+    end
+  end
+end

data/lib/driving_physics/powertrain.rb

@@ -10,7 +10,7 @@ module DrivingPhysics
   class Powertrain
     attr_reader :motor, :gearbox
 
-    def initialize(motor, gearbox)
+    def initialize(motor:, gearbox:)
       @motor = motor
       @gearbox = gearbox
     end
@@ -29,22 +29,17 @@ module DrivingPhysics
       [t * o, t, o]
     end
 
-    def axle_torque(rpm)
-      crank_alpha = @motor.alpha(@motor.torque(rpm),
-                                 omega: DrivingPhysics.omega(rpm))
-      crank_torque = @motor.implied_torque(crank_alpha)
-
-      axle_alpha = @gearbox.alpha(@gearbox.axle_torque(crank_torque),
-                                  omega: @gearbox.axle_omega(rpm))
-      @gearbox.implied_torque(axle_alpha)
+    def axle_torque(rpm, axle_omega: nil)
+      @gearbox.output_torque(@motor.output_torque(rpm), rpm,
+                             axle_omega: axle_omega)
     end
 
-    def axle_omega(rpm)
-      @gearbox.axle_omega(rpm)
+    def axle_omega(rpm, axle_omega: nil)
+      @gearbox.axle_omega(rpm, axle_omega: axle_omega)
     end
 
-    def crank_rpm(axle_omega)
-      @gearbox.crank_rpm(axle_omega)
+    def crank_rpm(axle_omega, crank_rpm: nil)
+      @gearbox.crank_rpm(axle_omega, crank_rpm: crank_rpm)
     end
   end
 end

data/lib/driving_physics/scalar_force.rb

@@ -23,7 +23,8 @@ module DrivingPhysics
                             frontal_area: FRONTAL_AREA,
                             drag_cof: DRAG_COF,
                             air_density: AIR_DENSITY)
-      -1 * 0.5 * frontal_area * drag_cof * air_density * speed ** 2
+      (speed ** 2) * (speed > 0 ? -1 : 1) *
+        0.5 * frontal_area * drag_cof * air_density
     end
 
     # opposes the direction of speed