driving_physics 0.0.2.1 → 0.0.3.1

data/lib/driving_physics/motor.rb

@@ -1,96 +1,204 @@
 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.0/1000
-        fly.omega_friction = 2.0/10_000
+      @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
+
+    def throttle_pct(places = 1)
+      format("%.#{places}f%%", @throttle * 100)
     end
 
-    # given torque, determine crank alpha after inertia and friction
+    # 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 torque(rpm)
-      raise(Stall, "RPM #{rpm}") if rpm < @rpms.first
-      raise(OverRev, "RPM #{rpm}") if rpm > @rpms.last
+    def output_torque(rpm)
+      self.torque(rpm) + self.friction(DrivingPhysics.omega(rpm))
+    end
 
-      last_rpm, last_tq, torque = 99999, -1, nil
+    # 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 < @torque_curve.min
+      raise(OverRev, "RPM #{rpm}") if rpm > @torque_curve.max
 
-      @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, last_tq = r, tq
-      }
-      raise(SanityCheck, "RPM #{rpm}") if torque.nil?
+      # 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

data/lib/driving_physics/mruby.rb

@@ -1,3 +1,6 @@
+# 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

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

data/lib/driving_physics/timer.rb

@@ -0,0 +1,34 @@
+module DrivingPhysics
+  module Timer
+    # don't use `defined?` with mruby
+    if (Process::CLOCK_MONOTONIC rescue false)
+      def self.now
+        Process.clock_gettime Process::CLOCK_MONOTONIC
+      end
+    else
+      def self.now
+        Time.now
+      end
+    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/vector_force.rb

@@ -64,6 +64,29 @@ module DrivingPhysics
     dir
   end
 
+  def self.torque_vector(force, radius)
+    if !force.is_a?(Vector) or force.size != 2
+      raise(ArgumentError, "force must be a 2D vector")
+    end
+    if !radius.is_a?(Vector) or radius.size != 2
+      raise(ArgumentError, "radius must be a 2D vector")
+    end
+    force = Vector[force[0], force[1], 0]
+    radius = Vector[radius[0], radius[1], 0]
+    force.cross(radius)
+  end
+
+  def self.force_vector(torque, radius)
+    if !torque.is_a?(Vector) or torque.size != 3
+      raise(ArgumentError, "torque must be a 3D vector")
+    end
+    if !radius.is_a?(Vector) or radius.size != 2
+      raise(ArgumentError, "radius must be a 2D vector")
+    end
+    radius = Vector[radius[0], radius[1], 0]
+    radius.cross(torque) / radius.dot(radius)
+  end
+
   module VectorForce
     #
     # Resistance Forces

data/lib/driving_physics.rb

@@ -1,37 +1,36 @@
+#
+# Units: metric
+#
+# distance: meter
+# velocity: meter / sec
+# acceleration: meter / sec^2
+# volume: Liter
+# temperature: Celsius
+#
 module DrivingPhysics
-  #
-  # Units: metric
-  #
-  # distance: meter
-  # velocity: meter / sec
-  # acceleration: meter / sec^2
-  # volume: Liter
-  # temperature: Celsius
-  #
+  # runtime check; this returns false by default
+  # Vector is not currently/easily available in mruby
+  def self.has_vector?
+    Vector rescue false
+  end
 
-  #
   # environmental defaults
-  #
   HZ = 1000
   TICK = Rational(1) / HZ
   G = 9.8               # m/s^2
   AIR_TEMP = 25         # deg c
   AIR_DENSITY = 1.29    # kg / m^3
-  PETROL_DENSITY = 0.71 # kg / L   TODO: move to car.rb
+  PETROL_DENSITY = 0.71 # kg / L
 
-  #
   # defaults for resistance forces
-  #
-  FRONTAL_AREA = 2.2  # m^2, based roughly on 2000s-era Chevrolet Corvette
-  DRAG_COF = 0.3      # based roughly on 2000s-era Chevrolet Corvette
-  DRAG = 0.4257       # air_resistance at 1 m/s given above numbers
-  ROT_COF = 12.771    # if rotating resistance matches air resistance at 30 m/s
-  ROT_CONST = 0.05    # N opposing drive force / torque
-  ROLL_COF = 0.01     # roughly: street tires on concrete
+  FRONTAL_AREA =  2.2    # m^2, based roughly on 2000s-era Chevrolet Corvette
+  DRAG_COF     =  0.3    # based roughly on 2000s-era Chevrolet Corvette
+  DRAG         =  0.4257 # air_resistance at 1 m/s given above numbers
+  ROT_COF      = 12.771  # if rot matches air at 30 m/s
+  ROT_CONST    =  0.05   # N opposing drive force / torque
+  ROLL_COF     =  0.01   # roughly: street tires on concrete
 
-  #
   # constants
-  #
   SECS_PER_MIN = 60
   MINS_PER_HOUR = 60
   SECS_PER_HOUR = SECS_PER_MIN * MINS_PER_HOUR
@@ -70,4 +69,11 @@ module DrivingPhysics
     alias_method(:omega, :accum)
     alias_method(:theta, :accum)
   end
+
+  def self.unit_interval!(val)
+    if val < 0.0 or val > 1.0
+      raise(ArgumentError, "val #{val.inspect} should be between 0 and 1")
+    end
+    val
+  end
 end