######################## rack.rb  henthorn@mbari.org ###########################
#
#
################################################################################

require 'ezservo'

class Rack < Ezservo

  def initialize config, portname='/dev/com1' 
    unless config
      config = SerialPort::Configuration.default
      #default baud is 38.4k, let's change it to 9600
      config.baud = 9600
    end
    super(config, portname)
    
    #  Set H-E limit switches
    #
    send_message("f1R")   # set home flag and limit polarity
    send_message("n2R")   # enable limit switches
    send_message("s2R")   # store null program for use in finding insert switch
    send_message("V3000R")
  end

  def reinit
    #  Set H-E limit switches
    #
    encoder_position
    send_message("n2R")   # enable limit switches
    send_message("f1R")   # set home flag and limit polarity
    send_message("s2R")   # store null program for use in finding insert switch
    velocity=3000
  end

  #configuration steps that are needed for testing in the pit
  def Rack.lab_test
    r = Rack.new nil, '/dev/ezservo'
    r.start_listener
    r.active_motor_num = "1"
    `stty -F /dev/ezservo min 1 time 0`
    r
  end

  
  def Rack.rover
    r = Rack.new nil, '/dev/rack'
    #r = Rack.new nil, '/dev/ezservo'
    r.start_listener
    r.active_motor_num = "1"
    `stty -F /dev/rack min 1 time 0`
    r
  end 

  def home
    self.send_message("Z30000R")
    if (self.wait_for_move_to_complete)
      self.forward 250
    end
    return self.wait_for_move_to_complete(1)
  end


  def insert_chamber
    self.send_message("gP100S11e2G0R")
    if (self.wait_for_move_to_complete(1))
      self.stop
      return true
    else
      return false
    end
  end
 

end