Omniwheel Bot Program for a Bot with 2 Joysticks

###############################################################################
# Description: Virtual Device Module that controls an Omniwheel
# Bot using a joystick.  Assumes a Wicked Device V1 Motor Shield or an
# Adafruit V2 Motor Shield.
# Modified original code so uses a second joystick - causes bot to rotate.
###############################################################################
require "thread"

module VdOmniWheelBotController

  private

  #############################################################################
  # Create and return the thread that will control the Omniwheel Bot.
  # This thread polls the event queue and uses the events it finds to control
  # the Bot.
  #############################################################################
  def bot_thread
    return Thread.new do
      begin
        @x1, @y1, @x2, @y2 = nil,nil, nil, nil # init joystick values

        # main loop - process joystick events and control the Bot
        loop do
          event = @event_queue.deq
          if val = event["joy_x"]
            @x1 = val
          elsif val = event["joy_y"]
            @y1 = val
          # check for values from our 2nd joystick
          elsif val = event["joy2_x"]
            @x2 = val
          elsif val = event["joy2_y"]
            # we don't use this value - available for a new function
            @y2 = val
          else
            p "unexpected event: #{event.inspect}"
          end
          # if have all x & ys from joysticks, can run things
          if @x1 && @y1 && @x2
            x1, y1, x2 = normalize(@x1, @y1, @x2)
            # set the motor speeds
            set_speed(x1, y1, x2)
          end
        end
      rescue Exception
        # say why we crashed in the log
        p "Error: bot thread has terminated: #{$!}"
      end
    end
  end

  #############################################################################
  # Takes raw joystick values and normalizes them to work with the Bot.
  # This Bot code wants pwm values in the range -100 to +100.
  # Uses the first samples to calibrate the joysticks.
  #############################################################################
  def normalize(x1, y1, x2)
    if !@calibrated
      @calibrated = true
      @x1_center, @y1_center, @x2_center = x1, y1, x2
    end
    # subtract out the calibration
    x1, y1, x2 = (x1 - @x1_center), (y1 - @y1_center), (x2 - @x2_center)
    # ignore small deviations when idling (noise)
    (x1.abs == 1) && (x1 = 0)
    (y1.abs == 1) && (y1 = 0)
    (x2.abs == 1) && (x2 = 0)
    # scale value so full range and return the result
    [4 * x1, 4 * y1, 4 * x2]
  end

  #############################################################################
  # Set the bot's speed and direction based on an (x1,y1,x2) vector.  x2 is
  # the x coordinate of a 2nd joystick - we use it for rotation.
  # Works for Wicked Motor Shield.
  # Speed magnitude will be between 0 and 100.
  #############################################################################
  def wickeddevice_set_speed(x1,y1,x2)
    # set the shift_reg and m1-3 variables
    # initialize to hard stop
    shift_reg = 0xffff
    m1 = m2 = m3 = 0
    # see if moving
    if x1 != 0 || y1 != 0 || x2 != 0
      # moving
      angle, mag = get_angle_and_mag(x1, y1)

      # get motor speeds, given speed and direction (angle)
      # add in rotation
      m1 = (Math.cos(angle) * mag).round - x2
      m2 = (Math.cos(angle + Math::PI*2/3) * mag).round - x2
      m3 = (Math.cos(angle + Math::PI*4/3) * mag).round - x2

      # set direction in the shift register - on Wicked Device Motor Shield
      shift_reg = 0
      shift_reg |= 0x2000 if m1 < 0
      shift_reg |= 0x800 if m2 < 0
      shift_reg |= 0x200 if m3 < 0

      # make motor speeds positive integers
      m1 = m1.abs
      m2 = m2.abs
      m3 = m3.abs
    end

    # send information to the bot, if it's changed
    generate_event("shift_reg" => shift_reg) if @shift_reg != shift_reg
    generate_event("m1_pwm"=>m1) if m1 != @m1_pwm
    generate_event("m2_pwm"=>m2) if m2 != @m2_pwm
    generate_event("m3_pwm"=>m3) if m3 != @m3_pwm

    # remember what we've sent
    @shift_reg = shift_reg
    @m1_pwm = m1
    @m2_pwm = m2
    @m3_pwm = m3
  end

  #############################################################################
  # Set the bot's speed and direction based on an (x1,y1,x2) vector.  x2 is
  # the x coordinate of a 2nd joystick - we use it for rotation.
  # Works for Adafruit Motor Shield.
  # Speed magnitude will be between 0 and 100.
  #############################################################################
  def adafruit_set_speed(x1,y1,x2)
    # set the shift_reg and m1-3 variables
    # init with hard stop
    m1_pwm, m1_in1, m1_in2 = 0, "off", "off"
    m2_pwm, m2_in1, m2_in2 = 0, "off", "off"
    m3_pwm, m3_in1, m3_in2 = 0, "off", "off"
    # see if moving
    if x1 != 0 || y1 != 0 || x2 != 0
      angle, mag = get_angle_and_mag(x1, y1)

      # get motor speeds, given speed and direction (angle)
      # add in rotation
      m1 = (Math.cos(angle) * mag).round - x2
      m2 = (Math.cos(angle + Math::PI*2/3) * mag).round - x2
      m3 = (Math.cos(angle + Math::PI*4/3) * mag).round - x2

      # set direction
      m1_in1, m1_in2 = "on", "off" if m1 > 0
      m1_in1, m1_in2 = "off", "on" if m1 < 0
      m2_in1, m2_in2 = "on", "off" if m2 > 0
      m2_in1, m2_in2 = "off", "on" if m2 < 0
      m3_in1, m3_in2 = "on", "off" if m3 > 0
      m3_in1, m3_in2 = "off", "on" if m3 < 0
      # set speeds
      m1_pwm, m2_pwm, m3_pwm = m1.abs, m2.abs, m3.abs
    end

    # update terminals, if changes
    generate_event("m1_pwm" => m1_pwm) if m1_pwm != @m1_pwm
    generate_event("m1_in1" => m1_in1) if m1_in1 != @m1_in1
    generate_event("m1_in2" => m1_in2) if m1_in2 != @m1_in2
    generate_event("m2_pwm" => m2_pwm) if m2_pwm != @m2_pwm
    generate_event("m2_in1" => m2_in1) if m2_in1 != @m2_in1
    generate_event("m2_in2" => m2_in2) if m2_in2 != @m2_in2
    generate_event("m3_pwm" => m3_pwm) if m3_pwm != @m3_pwm
    generate_event("m3_in1" => m3_in1) if m3_in1 != @m3_in1
    generate_event("m3_in2" => m3_in2) if m3_in2 != @m3_in2

    # remember
    @m1_pwm, @m1_in1, @m1_in2 = m1_pwm, m1_in1, m1_in2
    @m2_pwm, @m2_in1, @m2_in2 = m2_pwm, m2_in1, m2_in2
    @m3_pwm, @m3_in1, @m3_in2 = m3_pwm, m3_in1, m3_in2
  end

  #############################################################################
  # Get and angle and magnitude vector given an (x,y) point.
  # Returns an array with the angle and magnitude
  #############################################################################
  def get_angle_and_mag(x, y)
    angle = Math.atan2(y,x)
    # get magnitude
    original_mag = (x**2 + y**2)**(0.5)

    # normalize joystick to a circle, given x/y's follow a square
    new_a = angle.abs
    new_a = Math::PI - new_a if new_a >= Math::PI/2
    new_a = Math::PI/2 - new_a if new_a >= Math::PI/4
    mag = [original_mag * Math.cos(new_a), 100].min # 100 max
    [angle, mag]
  end


  #############################################################################
  # Virtual device method for processing terminal events.
  # Inputs are a hash where keys are terminal names and values are terminal
  # values.
  # Keep this method short and fast, so we don't hold up the callback thread.
  #
  # Return value must be an event hash.  Return an empty hash if no event,
  # and a terminal/value hash (like the inputs hash) for creating events.
  #############################################################################
  def respond_to_inputs(inputs, outputs = {})

    # put all events of interest into our event queue
    # bot thread will read them out when it can
    inputs.each do |key, val|
      if ["joy_x", "joy_y", "joy2_x", "joy2_y"].include?(key)
        @event_queue.enq({key=>val})
      else
        if terminal_dids.include?(key)
          # ok, we generated something we don't care about
        else
          print "Unexpected input: #{key.inspect}\n" # log this
        end
      end
    end

    outputs # return nothing - no events generated from here
  end

  #############################################################################
  # Define the terminals that a Wicked Device Shield uses.
  #############################################################################
  def wickeddevice_terminal_dids
    %W(joy_x joy_y joy2_x joy2_y m1_pwm m2_pwm m3_pwm shift_reg)
  end

  #############################################################################
  # Define the terminals that an Adafruit Shield uses.
  #############################################################################
  def adafruit_terminal_dids
    %W(joy_x joy_y joy2_x joy2_y
      m1_pwm m1_in1 m1_in2
      m2_pwm m2_in1 m2_in2
      m3_pwm m3_in1 m3_in2)
  end

  #############################################################################
  # Set up for an Adafruit motor shield.
  # Define Adafruit specific methods as generic ones.
  #############################################################################
  def adafruit_shield_init
    class << self
      alias set_speed adafruit_set_speed
      alias terminal_dids adafruit_terminal_dids
    end
    controller_init()
  end

  #############################################################################
  # Set up for a Wicked Device motor shield.
  # Define Wicked Device specific methods as generic ones.
  #############################################################################
  def wickeddevice_shield_init
    class << self
      alias set_speed wickeddevice_set_speed
      alias terminal_dids wickeddevice_terminal_dids
    end
    controller_init()
  end

  #############################################################################
  # Generic initialization routine for setting up this controller.
  #############################################################################
  def controller_init
    @event_queue = Queue.new()
    set_speed(0,0,0) # stop, initialize instance variables
    # start up bot thread
    @bot_thread = bot_thread()
  end 

end # module