driving_physics 0.0.1.2 → 0.0.3.1

data/demo/mruby/motor.rb

@@ -0,0 +1,140 @@
+
+# next fun idea: PID controller
+
+include DrivingPhysics
+
+env = Environment.new
+motor = Motor.new(env)
+puts env
+puts motor
+puts
+
+puts "Rev it up!"
+motor.throttle = 1.0
+[:torque, :power].each { |run|
+  puts
+  puts run.to_s.upcase + ':'
+
+  800.upto(7000) { |rpm|
+    next unless rpm % 200 == 0
+    omega = DrivingPhysics.omega(rpm)
+    torque = motor.torque(rpm)
+    case run
+    when :torque
+      count = (torque.to_f / 10).round
+      char = 'T'
+      val = torque.round.to_s.rjust(5, ' ')
+      fmt = "%s Nm"
+    when :power
+      power = DrivingPhysics.power(torque, omega)
+      kw = power.to_f / 1000
+      count = (kw / 5).round
+      char = 'P'
+      val = kw.round(1).to_s.rjust(5, ' ')
+      fmt = "%s kW"
+    else
+      raise "unknown"
+    end
+    puts format("%s RPM: #{fmt}\t%s",
+                rpm.to_s.rjust(4, ' '),
+                val,
+                char * count)
+  }
+}
+
+puts
+puts "Now, the simulation begins..."
+puts "---"
+puts "* Spin the motor up to #{motor.idle} RPM with the starter motor."
+puts "* Rev it up with the throttle."
+puts "* Let it die."
+CLI.pause
+
+alpha = 0.0
+omega = 0.0
+theta = 0.0
+
+duration = 60
+
+status = :ignition
+rpm = 0
+
+paused = 0.0
+num_ticks = duration * env.hz + 1
+start = Timer.now
+
+num_ticks.times { |i|
+  # this is an input torque; alpha is determined after inertia and friction
+  torque = case status
+           when :ignition
+             motor.starter_torque
+           when :running, :off_throttle, :idling
+             motor.torque(rpm)
+           else
+             0
+           end
+
+  # Motor#alpha incorporates inertia and friction
+  alpha = motor.alpha(torque, omega: omega)
+  omega += alpha * env.tick
+  theta += omega * env.tick
+
+  # prevent silly oscillations due to ticks or tiny floating point errors
+  omega = 0 if omega < 0.0001
+
+  net_torque = motor.implied_torque(alpha)
+  rpm = DrivingPhysics.rpm(omega)
+  power = DrivingPhysics.power(net_torque, omega)
+
+  if rpm > motor.idle and status == :ignition
+    status = :running
+    flag = true
+  end
+
+  if rpm > 7000 and status == :running
+    status = :off_throttle
+    motor.throttle = 0.0
+    flag = true
+  end
+
+  if rpm < 1000 and status == :off_throttle
+    status = :idling
+    motor.throttle = 0.06
+    flag = true
+  end
+
+  # we don't need no PID control
+  if status == :idling
+    if rpm.round <= 999
+      motor.throttle += (1.0 - motor.throttle) * 0.005
+    elsif rpm.round == 1000
+      motor.throttle += (rand - 0.5) * 0.0005
+    else
+      motor.throttle -= motor.throttle * 0.005
+    end
+  end
+
+  if flag or
+    (i < 10) or
+    (i < 100 and i % 10 == 0) or
+    (i < 1000 and i % 100 == 0) or
+    (i < 10_000 and i % 500 == 0) or
+    (i % 5000 == 0) or
+    (status == :idling and i % 100 == 0)
+    puts Timer.display(ms: i)
+    puts format("Throttle: %.1f%%", motor.throttle * 100)
+    puts format("%d RPM  %.1f Nm (%d Nm)  %.1f kW   Friction: %.1f Nm",
+                DrivingPhysics.rpm(omega),
+                net_torque,
+                torque,
+                power / 1000,
+                motor.spinner.rotating_friction(omega))
+    puts format("%.3f rad/s/s  %.2f rad/s  %.1f rad", alpha, omega, theta)
+    puts
+
+    paused += CLI.pause if flag
+    flag = false
+  end
+}
+
+puts Timer.summary(start, num_ticks, paused)

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)
@@ -103,7 +112,7 @@ module DrivingPhysics
     end
 
     def corner_mass
-      Rational(self.total_mass) / @num_tires
+      self.total_mass / @num_tires
     end
 
     # per tire

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

@@ -1,5 +1,5 @@
 require 'driving_physics/environment'
-require 'driving_physics/vector_force'
+#require 'driving_physics/vector_force'
 
 module DrivingPhysics
   # radius is always in meters
@@ -80,41 +80,16 @@ 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
 
     def initialize(env)
       @env = env
-      @radius  = 350/1000r # m
-      @width   = 200/1000r # m
+      @radius  = 0.35
+      @width   = 0.2
       @density = DENSITY
-      @base_friction  = 5/100_000r  # constant resistance to rotation
-      @omega_friction = 5/100_000r  # scales with omega
+      @base_friction  = 5.0/100_000  # constant resistance to rotation
+      @omega_friction = 5.0/100_000  # scales with omega
       yield self if block_given?
     end
 
@@ -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)