######################## 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)
    
    self.start_listener
    #  Set H-E limit switches
    #
    self.active_motor_num = 2 
    #first send a message to flush the buffer
    send_message("badR")
    send_message("f1R")   # set home flag and limit polarity
    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
    send_message("V3000R")
  end

  def reinit
    #  Set H-E limit switches
    #
    #first send a message to flush the buffer
    #because there's gonna be junk in it if the rack was not
    #on when this object was first initialized
    send_message("badR")
    send_message("f1R")   # set home flag and limit polarity
    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
    send_message("V3000R")
  end

  #configuration steps that are needed for testing in the pit
  def Rack.lab_test
    r = Rack.new nil, '/dev/ezservo'
    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.active_motor_num = "2"
    `stty -F /dev/rack min 1 time 0`
    r
  end 

  def home
    self.send_message("Z30000R")
    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