driving_physics 0.0.2.1 → 0.0.3.1

data/demo/pid_controller.rb

@@ -0,0 +1,143 @@
+require 'device_control'
+require 'driving_physics/motor'
+require 'driving_physics/cli'
+
+include DrivingPhysics
+
+env = Environment.new { |e|
+  e.hz = CLI.prompt("What frequency? (Hz)", default: 1000).to_i
+}
+motor = Motor.new(env)
+puts env
+puts motor
+puts
+
+
+PID = DeviceControl::PIDController
+models = PID::ZN.keys.join('  ')
+
+puts "PID models: #{models}"
+pid_model = CLI.prompt("What PID model?", default: 'none')
+
+# Ku for 1000 RPM idle has been measured at 0.2
+# Tu is 1000 Hz / 2
+zn = PID.tune(pid_model, 0.2, 1/500r)
+puts format("kp: %.3f\tki: %.1f\tkd: %.8f", zn[:kp], zn[:ki], zn[:kd])
+puts
+
+# maintain arbitrary RPM via throttle adjustment
+pidc = DeviceControl::PIDController.new(motor.idle, dt: env.tick) { |p|
+  p.kp = zn[:kp]
+  p.ki = zn[:ki]
+  p.kd = zn[:kd]
+  p.p_range = (-1.00..1.00)
+  p.i_range = (-0.50..0.50)
+  p.d_range = (-0.25..0.25)
+  p.o_range = ( 0.00..1.00)
+  p.e_range = (-1.00..1.00)
+}
+# track the accumulated absolute error
+pidc_error = 0.0
+
+puts pidc
+puts
+
+duration = CLI.prompt("How long to run for? (seconds)", default: 10).to_f
+CLI.pause
+
+alpha = 0.0
+omega = 0.0
+
+rpm = 0
+status = :ignition
+
+(duration * env.hz + 1).to_i.times { |i|
+  puts Timer.display(seconds: i.to_f / env.hz)
+
+  # GIVEN:
+  # * rpm
+  # DETERMINE:
+  # * torque
+  # * new rpm
+
+  case status
+  when :ignition
+    torque = motor.starter_torque
+    puts "IGNITION: Starter torque %d Nm  %d RPM" % [torque, rpm]
+    status = :running if rpm > motor.idle
+  when :running
+    torque = motor.torque(rpm)
+    puts format("RUNNING: %.1f Nm @ %d RPM  Throttle: %s",
+                torque, rpm.round, motor.throttle_pct(3))
+    puts pidc
+    puts "Absolute error %.3f" % pidc_error
+  end
+
+  # update kinematics
+  alpha = motor.alpha(torque, omega: omega)
+  omega += alpha * env.tick
+  rpm = DrivingPhysics.rpm(omega)
+
+  puts format("Net Torque: %+.4f Nm   Friction: %+.4f Nm",
+              motor.implied_torque(alpha),
+              motor.friction(omega))
+
+  case status
+  when :ignition
+    # ok
+    CLI.pause if i % 50 == 0
+  when :running
+    # Update throttle position and RPM
+    # Throttle position is based on PID controller
+    # PID controller looks at current RPM
+    # Update to the new RPM at the very end of the loop
+
+    motor.throttle = pidc.update(rpm)
+    pidc_error += pidc.error.abs
+    error_pct = pidc.error.abs / pidc.setpoint.to_f
+
+    # prompt every so often
+    if (error_pct < 0.001) or
+      (i < 10_000 and i % 100 == 0) or
+      (i % 1000 == 0)
+      # ask about PID tunables; loop until an acceptable answer
+      loop {
+        puts
+        puts format("rpm %.1f\tsetpoint %d\tkp %.3f\tki %.3f\tkd %.3f",
+                    rpm, pidc.setpoint, pidc.kp, pidc.ki, pidc.kd)
+        str = CLI.prompt("Enter key and value, e.g. > setpoint 3500\n")
+        # empty answer is perfectly acceptable; exit the loop
+        break if str.empty?
+
+        # look for "key value" pairs
+        parts = str.split(' ').map(&:strip)
+        next unless parts.size == 2
+        begin
+          key, val = parts[0].downcase, parts[1].to_f
+        rescue => e
+          puts e
+          next
+        end
+
+        # update RPM or PID controller and exit the loop
+        if key == "rpm"
+          rpm = val
+          omega = DrivingPhysics.omega(rpm)
+        else
+          begin
+            pidc.send("#{key}=", val)
+          rescue => e
+            puts e
+            next
+          end
+        end
+        # reset error tracking on successful input
+        pidc_error = 0.0
+        pidc.sum_error = 0.0
+        puts
+        break
+      }
+    end
+  end
+  puts
+}

data/demo/powertrain.rb

@@ -8,7 +8,7 @@ include DrivingPhysics
 env = Environment.new
 motor = Motor.new(env)
 gearbox = Gearbox.new(env)
-powertrain = Powertrain.new(motor, gearbox)
+powertrain = Powertrain.new(motor: motor, gearbox: gearbox)
 motor.throttle = 1.0
 puts env
 puts powertrain

data/driving_physics.gemspec

@@ -1,12 +1,13 @@
 Gem::Specification.new do |s|
   s.name = 'driving_physics'
-  s.summary = "WIP"
+  s.summary = "Physics from first principles: mass, friction, rotation, etc"
   s.description = "WIP"
   s.authors = ["Rick Hull"]
   s.homepage = "https://github.com/rickhull/driving_physics"
   s.license = "LGPL-3.0"
 
   s.required_ruby_version = "> 2"
+  s.add_runtime_dependency 'device_control', '~> 0.3'
 
   s.version = File.read(File.join(__dir__, 'VERSION')).chomp
 

data/lib/driving_physics/car.rb

@@ -26,6 +26,14 @@ module DrivingPhysics
       @powertrain.motor.throttle = val
     end
 
+    def clutch
+      @powertrain.gearbox.clutch
+    end
+
+    def clutch=(val)
+      @powertrain.gearbox.clutch = val
+    end
+
     def gear
       @powertrain.gearbox.gear
     end
@@ -38,9 +46,9 @@ module DrivingPhysics
       @powertrain.gearbox.top_gear
     end
 
-    # force opposing speed
+    # force opposing speed; depends on speed**2 but use speed and speed.abs
     def air_force(speed)
-      -0.5 * @frontal_area * @cd * @env.air_density * speed ** 2
+      -0.5 * @frontal_area * @cd * @env.air_density * speed * speed.abs
     end
 
     # force of opposite sign to omega
@@ -60,6 +68,7 @@ module DrivingPhysics
     # force of opposite sign to force
     def tire_inertial_force(force)
       mag = force.abs
+      return 0.0 if mag < 0.001
       sign = force / mag
       force_loss = 0
       5.times {
@@ -86,8 +95,8 @@ module DrivingPhysics
       ].join("\n")
     end
 
-    def drive_force(rpm)
-      @tire.force(@powertrain.axle_torque(rpm))
+    def drive_force(rpm, axle_omega: nil)
+      @tire.force @powertrain.axle_torque(rpm, axle_omega: axle_omega)
     end
 
     def tire_speed(rpm)

data/lib/driving_physics/cli.rb

@@ -1,10 +1,14 @@
+require 'driving_physics/timer'
+
 module DrivingPhysics
   module CLI
     # returns user input as a string
-    def self.prompt(msg = '')
-      print msg + ' ' unless msg.empty?
+    def self.prompt(msg = '', default: nil)
+      print "#{msg} " unless msg.empty?
+      print "(#{default}) " unless default.nil?
       print '> '
-      $stdin.gets.chomp
+      input = $stdin.gets.chomp
+      input.empty? ? default.to_s : input
     end
 
     # press Enter to continue, ignore input, return elapsed time
@@ -16,36 +20,4 @@ module DrivingPhysics
       Timer.since(t)
     end
   end
-
-  module Timer
-    if defined? Process::CLOCK_MONOTONIC
-      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/cockpit.rb

@@ -0,0 +1,167 @@
+require 'driving_physics/car'
+
+module DrivingPhysics
+  class Cockpit
+    CLUTCH_MIN = 0.1
+    REV_MATCH = 0.02
+    REV_SLIP = 0.1
+    MODE = {
+      normal: 0,
+      sport: 1,
+      sport_plus: 2,
+    }
+    MODE_LABELS = MODE.invert
+
+    def self.mode_label(idx)
+      MODE_LABELS.fetch(idx)
+    end
+
+    # return [:status, recommended clutch, proportional difference]
+    def self.rev_match(old_rpm, new_rpm)
+      diff = new_rpm - old_rpm
+      proportion = diff.to_f / old_rpm
+      clutch = [1.0 - proportion.abs, CLUTCH_MIN].max
+      if proportion.abs <= REV_MATCH
+        [:match, 1.0, proportion]
+      elsif proportion.abs <= REV_SLIP
+        [:slip, clutch, proportion]
+      else
+        [:mismatch, clutch, proportion]
+      end
+    end
+
+    def self.generate
+      e = Environment.new
+      t = Tire.new(e)
+      m = Motor.new(e)
+      g = Gearbox.new(e)
+      p = Powertrain.new(motor: m, gearbox: g)
+      c = Car.new(tire: t, powertrain: p)
+      self.new(c)
+    end
+
+    attr_reader :car, :brake_pedal, :steering_wheel, :mode, :mode_label
+    attr_accessor :min_rpm, :max_rpm
+
+    def initialize(car)
+      @car = car
+      @brake_pedal = 0.0    # to 1.0
+      @steering_wheel = 0.0 # -1.0 to +1.0
+      @min_rpm = @car.powertrain.motor.idle + 1000
+      @max_rpm = @car.powertrain.motor.redline
+
+      self.mode = 0             # normal
+
+      yield self if block_given?
+    end
+
+    def clutch_pedal
+      1.0 - @car.clutch
+    end
+
+    def clutch_pct
+      self.clutch_pedal * 100
+    end
+
+    # clutch pedal has inverse relationship with the clutch itself
+    # clutch pedal DISENGAGES the clutch; pedal at 0.0 means clutch at 1.0
+    def clutch_pedal=(val)
+      @car.clutch = 1.0 - DrivingPhysics.unit_interval!(val)
+    end
+
+    def brake_pct
+      self.brake_pedal * 100
+    end
+
+    # TODO: implement @car.brake or something
+    def brake_pedal=(val)
+      @brake_pedal = DrivingPhysics.unit_interval!(val)
+    end
+
+    def throttle_pedal
+      @car.throttle
+    end
+
+    def throttle_pct
+      self.throttle_pedal * 100
+    end
+
+    def throttle_pedal=(val)
+      @car.throttle = DrivingPhysics.unit_interval!(val)
+    end
+
+    def steering_pct
+      self.steering_wheel * 100
+    end
+
+    # TODO: implement @car.steering_wheel
+    def steering_wheel=(val)
+      DrivingPhysics.unit_interval!(val.abs)
+      @steering_wheel = val
+    end
+
+    def gear
+      @car.gear
+    end
+
+    def gear=(val)
+      @car.gear = val
+    end
+
+    def mode=(val)
+      @mode = MODE_LABELS[val] ? val : MODE.fetch(val)
+      @mode_label = Cockpit.mode_label(@mode)
+    end
+
+    def to_s
+      [format("Clutch: %d%%  Brake: %d%%  Throttle: %d%%",
+              self.clutch_pct, self.brake_pct, self.throttle_pct),
+       format("Wheel: %d%%  Gear: %d  Mode: [%d] %s",
+              self.steering_pct, self.gear, self.mode, self.mode_label)
+      ].join("\n")
+    end
+
+    # return :normal, :sport, :sport_plus based on pedal positions
+    def pedal_mode
+      if self.brake_pedal < 0.4 and self.throttle_pedal < 0.5
+        MODE[:normal]
+      elsif self.brake_pedal < 0.7 and self.throttle_pedal < 0.8
+        MODE[:sport]
+      else
+        MODE[:sport_plus]
+      end
+    end
+
+    def rpm_range
+      if @mode < MODE[:sport_plus] and self.pedal_mode == MODE[:sport_plus]
+        mode = @mode + 1
+      elsif @mode == MODE[:sport_plus] and self.pedal_mode == MODE[:normal]
+        mode = MODE[:sport]
+      else
+        mode = @mode
+      end
+
+      case mode
+      when MODE[:normal]
+        [@min_rpm, [@min_rpm + 3000, @max_rpm - 1000].min]
+      when MODE[:sport]
+        [@min_rpm + 1000, @max_rpm - 1000]
+      when MODE[:sport_plus]
+        [@min_rpm + 1500, @max_rpm]
+      end
+    end
+
+    def choose_gear(rpm)
+      min, max = *self.rpm_range
+      gear = self.gear
+
+      if rpm < min and gear > 1
+        gear - 1
+      elsif rpm > max and gear < @car.top_gear
+        gear + 1
+      else
+        gear
+      end
+    end
+  end
+end

data/lib/driving_physics/disk.rb

@@ -80,31 +80,6 @@ module DrivingPhysics
       tangential.to_f / radius
     end
 
-    # vectors only
-    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
-
-    # vectors only
-    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
-
     attr_reader :env
     attr_accessor :radius, :width, :density, :base_friction, :omega_friction
 
@@ -130,6 +105,11 @@ module DrivingPhysics
       @normal_force
     end
 
+    # E = (1/2) (I) (omega^2)
+    def energy(omega)
+      0.5 * self.rotational_inertia * omega ** 2
+    end
+
     def alpha(torque, omega: 0, normal_force: nil)
       (torque - self.rotating_friction(omega, normal_force: normal_force)) /
         self.rotational_inertia

data/lib/driving_physics/environment.rb

@@ -11,6 +11,8 @@ module DrivingPhysics
       @air_temp = AIR_TEMP
       @air_density = AIR_DENSITY
       @petrol_density = PETROL_DENSITY
+
+      yield self if block_given?
     end
 
     def hz=(int)

data/lib/driving_physics/gearbox.rb

@@ -13,16 +13,30 @@ 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|
@@ -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