######################## ezservo.rb achase@mbari.org ###########################
#
# Provide control of the All Motion EZSV23 servo motor controller over a RS-485
# half duplex mult-drop bus.
#
# The OEM communication protocol is the only protocol supported at this time.
# It has the same functionality as the DT protocol, but supports checksums as
# well
#
################################################################################

require 'rover_environment'
require 'posix/serialport'
require 'rover_utils'
require 'rover_thread'
require 'monitor'

class Ezservo < Monitor
  include LogHelper
  attr_reader :thread, :response_packets, :serial_port
  attr_accessor :active_motor_num, :log_moves, :stop_wait
  @@cr = 0xD #ascii carriage return
  #this hash maps the motor banks to the actual motors each bank controls
  #note that only motor 1 is mapped to the global address "_"
  @@motor_banks = {"A" => [1,2], "C" => [3,4], "E" => [5,6], "G" => [7,8], \
                  "I" => [9,':'], "K" => [';','<'], "M" => ['=','>'], \
                  "O" => ['?','@'], "Q" => [1,2,3,4], "U" => [5,6,7,8], \
                  "Y" => ['9',':',';','<'], "]" => ['=','>','?','@'], \
                  "_" => [1] }
                  
  #NOTE: This should probably come from a config file
  STARBOARD_MOTOR_NUM = 2
  PORT_MOTOR_NUM = 1

  def initialize config, portname='/dev/com1' 
    super()
    unless config
      config = SerialPort::Configuration.default
      #default baud is 38.4k, let's change it to 9600
      config.baud = 9600
    end
    @serial_port = Posix::SerialPort.new portname, config
    @response_packets = Array.new
    #the default motor number
    self.active_motor_num = "A"
    self.encoder_position = 2000
    self.log_moves = false
    self.stop_wait = false
  end

  #configuration steps that are needed for testing at my (andrew's) desk
  def Ezservo.lab_test
    ez = Ezservo.new nil, '/dev/ttyS1'
    ez.start_listener
    ez.proportional_gain = 1000
    ez
  end
  
  def Ezservo.rover
    ez = Ezservo.new nil, '/dev/ezservo'
    ez.start_listener
    ez.active_motor_num = "A"
    `stty -F /dev/ezservo min 1 time 0`
    ez
  end 

  def Ezservo.mini_rover
    ez = Ezservo.new nil, '/dev/ttyAM0'
    ez.start_listener
    ez.active_motor_num = "A"
   # `stty -F /dev/ezservo min 0 time 20`
    ez
  end 

###################### Operational Commands ####################################
#
# commands:
#   stop_command -- stops the currently executing string of commands sent to the
#                   servo, but does not stop the command actually executing
#   halt, stop -- aliases for stop_command
#   reset! -- Resets the servo, returning it to the same state as on power-up 
#   move_abs [int count] -- Moves the motor to the specified encoder count
#   forward [int count] -- Moves the motor forward "count" encoder counts
#   back [int count] -- Moves the motor backward "count" encoder counts
#   recover -- Recover Servo, turns the servo on or off to current encoder
#              position. Useful in the case where an overload error has occurred.
#              Typically this command is used in a recovery script triggered by
#              the n512 mode.
# read only values:
#   current_status? -- Retrieves the current error message
#   commanded_position? -- Retrieves the position the servo has been commanded to 
#   message -- The last message sent back from the servo
#   error_mesage -- Error from the last response from the servo
# read/write values:
#   encoder_position -- Setting this value will change the encoder position 
#     without moving the motor
#   proportional_gain, integral_gain, differential_gain -- PID Loop variables
#   velocity
# write only values:
#   current [0-100 percent] -- Sets the max current allowed during a move
#   acceleration [0-65000] -- Sets acceleration in (encoder ticks/32.768) per
#              second squared
#   overload_retry_count [1-65000] -- Only executes when n512 and n1024 modes
#              are activated, after exceeding the retry count stored program
#              12 is executed
#   overload_timeout [1-25000 milliseconds] -- Time to wait before shutting down
#              when an overload condition has been detected.
#
################################################################################
  #returns the ready/busy status as well as any error conditions
  def move_abs position
    send_message "A" << position.to_s << "R"
  end

  #a blocking move command that will block until the move is complete
  def forward amount
    syslog "moving #{amount}"
    send_message "P" << amount.to_s << "R"
    wait_for_move_to_complete
  end
  
  #a blocking move command that will block until the move is complete
  def back amount
    send_message "D" << amount.to_s << "R"
    wait_for_move_to_complete
  end
  
  #turn the two motors simultaneously with optionally different
  #values for distance and different values for direction
  #Legal values for direction are :forward and :backward
  def turn starboard_dir, starboard_dist, port_dir, port_dist
    syslog "received command to move starboard #{starboard_dir} - #{starboard_dist}" <<
           " and port #{port_dir} - #{port_dist}"
    old_active = self.active_motor_num
    #first, check the encoder position to make sure any backward moves
    #won't go past zero
    if starboard_dir == :backward || port_dir == :backward
      self.active_motor_num = "A"
      syslog "querying encoder positions"
      positions = self.encoder_position
      syslog "encoder positions are: #{positions[0]} and #{positions[1]}"
      if starboard_dir == :backward && positions[0] < starboard_dist
        self.active_motor_num = "A"
        #BUG! The starboard motor controller can not handle this 
        #20061130 This bug should now be fixed.
        #the motor controller firmware should now be updated and this should work
        syslog "Setting starboard motors position forward to accomadate for upcoming " +
               "backward move"
        self.encoder_position = starboard_dist + 500
        sleep 1
      end
      if port_dir == :backward && positions[1] < port_dist
        self.active_motor_num = "A"
        syslog "Setting port motors position forward to accomadate for upcoming " +
               "backward move"
        self.encoder_position = port_dist + 500
        sleep 1
      end
    end
    starboard_dir = direction_translation(starboard_dir)
    port_dir = direction_translation(port_dir)
    self.active_motor_num = STARBOARD_MOTOR_NUM
    syslog "in turn, moving starboard dir #{starboard_dir} #{starboard_dist.to_s}"
    send_message "#{starboard_dir}" << starboard_dist.to_s 
    self.active_motor_num = PORT_MOTOR_NUM
    syslog "in turn, moving port dir #{port_dir} #{port_dist.to_s}"
    send_message "#{port_dir}" << port_dist.to_s
    self.active_motor_num = "A"
    send_message "R"
    wait_for_move_to_complete
    self.active_motor_num = old_active
  end
  
  def current_status?
    packets = do_query "Q" 
    packets.each do |packet|
      packet.error_message
    end
  end
  
  def wait_for_move_to_complete sleep_interval = 5
    self.stop_wait = false
    
    sleep 1
    syslog "beginning wait_for_move_to_complete"
    positions = self.encoder_position
    sleep 5
    new_positions = self.encoder_position
    
    update = "position: "
    new_positions.each do |pos|
      update << pos.to_s << ", "
    end
      
    while (positions != new_positions && !self.stop_wait) do
      sleep sleep_interval
      old_update = update
      positions = new_positions
      new_positions = self.encoder_position
      update = "positions: "
      new_positions.each do |pos|
        update << pos.to_s << ", "
      end
      syslog old_update + "| " + update
    end
    
    if (self.stop_wait)
      syslog " wait was stopped"
    else
      syslog " positions are equal"
    end
    
    if self.has_error
      syslog "error detected while waiting for move to complete"
      syslog "error: #{self.error_message}"
    end
    self.stop_wait = false
    syslog "move has completed"
  end
  
  #translate from a direction symbol (:forward or :backward) to the
  #corresponding ezservo direction (P or D)
  def direction_translation direction
    if direction == :forward
      return "P"
    elsif direction == :backward
      return "D"
    else
      syslog "ERROR: Bad direction received in direction_translation function" 
      syslog "Direction symbol received: #{direction}"
      syslog "Returning :forward direction rather than throwing an error."
      return "P"
    end
  end

  def commanded_position?
    numeric_response_query "?0"
  end
  def encoder_position
    numeric_response_query "?8"
  end
  #change position without moving
  def encoder_position= new_position
    send_message "z" << new_position.to_s << "R"
  end
  def proportional_gain
    numeric_response_query "?w"
  end
  def proportional_gain= new_gain
    send_message "w" << new_gain.to_s << "R"
  end
  def integral_gain
    numeric_response_query "?x"
  end
  def integral_gain= new_gain
    send_message "x" << new_gain.to_s << "R"
  end
  def differential_gain
    numeric_response_query "?y"
  end
  def differential_gain= new_gain
    send_message "y" << new_gain.to_s << "R"
  end
  def velocity= new_velocity
    send_message "V" << new_velocity.to_s << "R"
  end
  def velocity
    numeric_response_query "?2"
  end
  def acceleration= new_acceleration
    send_message "L" << new_acceleration.to_s << "R"
  end
  def current= new_current
    send_message "m" << new_current.to_s << "R"
  end
  def overload_retry_count= new_count
    send_message "au" << new_count.to_s << "R"
  end
  def overload_timeout= new_timeout
    send_message "u" << new_timeout.to_s << "R"
  end
  #stop the command currently executing. This stop request is likely
  #to be ignored...
  def stop_command
    old_active = self.active_motor_num
    self.active_motor_num = "A"
    send_message "T"
    self.active_motor_num = old_active
    self.stop_wait = true
  end
  alias halt stop_command
  alias stop stop_command 
  #perform a processor reset
  def reset!
    send_message "ar5073R"
  end
  #NOTE: This needs to be tested, the numeric value can be 1 or 0
  def recover
    send_message "r1R"
  end
  
  #get the most recent error message
  def error_message
    response_packets[-1].error_message
  end
  
  def has_error
    response_packets[-1].error_code != 0
  end

  #get the message segment of the last response_packet
  def message
    response_packets[-1].message
  end

########################## Supporting Commands ################################
  #warning, this is a blocking call...
  def get_response_packet
      collector = ""
      #use this variable to track when a response is coming across the wire
      @receiving_response = false
      while true
        c = @serial_port.getc
        @receiving_response = true
        collector << c
        if collector[-1] == 3
          #remove the RS485 line turn around character, this character
          #should be 255, but that could be corrupted due to line turn around
          #transients, so just test that the first character isn't the 'start'
          #character
          if collector[0] != 2
            collector = collector[1..-1]
          end
          if collector[0] != 2
            syslog "WARNING: start character not seen at beginning of response"
          end
          #grab the checksum...
          checksum = @serial_port.getc
          if Ezservo.compute_checksum(collector) != checksum
            syslog "WARNING: Checksums do not match..."
          end
          packet = ResponsePacket.new collector
          return packet
        end
      end
  end

  def start_listener
    @thread = RoverThread.new do
      while true
        packet = get_response_packet
        unless packet.error_code == 0
          syslog "Error returned from servo: #{packet.error_message}"
        end
        @response_packets.push packet
      end
    end
  end

  def kill_listener
    @thread.kill
  end

  #format a message to the servo using the (more complex) OEM
  #protocol of the EZ Servo.
  #start character is Ctrl-B, end character is Ctrl-C
  #See appendix 4 (page 21) of the EZ Servo Communications Protocol Document
  #for a full description of the OEM protocol.
  OEM_START_CHAR = 0x2
  OEM_END_CHAR = 0x3
  def send_message message
    command = ""
    self.synchronize do
      command << OEM_START_CHAR 
      command << @active_motor_num.to_s	#message address
      command << "1"			#sequence character
      command << message
      command << OEM_END_CHAR 
      command << Ezservo.compute_checksum(command)
      @serial_port.write command
      #sleep a bit so that another command doesn't come along
      #after this one and collide with it.
      sleep 1
      syslog "command sent #{command}"
    end
    command
  end

  def Ezservo.compute_checksum string
    checksum = 0
    string.each_byte do |byte|
      checksum = checksum ^ byte
    end
    checksum
  end


  #Returns the most recent response packet after sending the query to the
  #motor
  #
  #This method assumes a single threaded mode of operation, with the only
  #other thread operating on the serial port being the listner thread.
  def do_query(query_string)
    unless thread && thread.alive?
      syslog "listener thread not currently active. Starting listener..."
      self.start_listener
    end
    #spin the wheels while the line clears
    count = 0
    while @receiving_response && count < 10 do
      sleep(0.2)
      count += 1
    end
    @receiving_response = false
    responses = response_packets.size
    response_counter = responses
    originally_active_motor = self.active_motor_num
    syslog "sending a query to motor bank: #{originally_active_motor}"
    motors_to_query = @@motor_banks[self.active_motor_num] ||= [self.active_motor_num]
    motors_to_query.each do |motor|
      self.active_motor_num = motor
      send_message query_string
      syslog "query sent to motor #{motor}"
      #wait until the new response has been added to the stack
      while response_counter == response_packets.size do
        sleep(0.2)
      end
      response_counter += 1 
    end
    self.active_motor_num = originally_active_motor
    #this will return all the packets that have been added to the array
    #since the last time the arrays size was queried.
    response_packets.slice(responses..-1)
  end

  def numeric_response_query message
    packets = do_query message 
    packets.collect! {|x| x.message.to_i }
    packets
  end
end

################################################################################
# EZ Servos respond to commands by sending messages addressed to the master
# device (\0), following the device address is a "status character" which is
# a byte that encodes various information in each of it's bytes. the bits are:
# 7 - reserved
# 6 - always set
# 5 - ready bit -- Set wehn EZ Servo is ready to accept a command
# 4 - reserved
# 3-0 combined to form an error code from 0 to 15
# The error codes are as follows:
# 0 = no error                  1 = InitError   2 = Bad Command (illegal command)
# 3 = Bad Operand (range error) 4 = n/a         5 = Communications Error (internal)
# 6 = n/a                       7 = Not Inited  8 = n/a
# 9 = Overload Error (physical system could not keep up with command position)
# 10= n/a                       11= Move not allowed
# 12-14 = n/a                   15= Command Overflow
#
# For more detailed documentation see appendix 7 of the ez servo command set
################################################################################
class ResponsePacket
  ERROR_NAMES = ["no error", "init error", "bad command", "bad operand", "n/a", \
                 "communications error", "n/a", "not initialzed", "n/a", \
                 "overload error", "n/a", "move not allowed", "n/a", "n/a", "n/a", \
                 "command overflow"]

  def initialize response_packet_string
    @response_packet_string = response_packet_string
  end

  def to_s
    "readybit = " + is_ready_bit_set?.to_s + " error message = " + error_message
  end  
    
  def status_character
    status_regex = Regexp.new("" << Ezservo::OEM_START_CHAR << "0(.).*")
    match = status_regex.match @response_packet_string
    #the first index into match gets the regex () match, the second index
    #takes the first (and only) character's byte value from the string
    return match[1][0] || 5
  end

  def is_ready_bit_set? 
    if self.status_character & 2**5 != 0
      true
    else
      false
    end
  end

  def message
    #the . after the 0 is the status_character
    message_regex = Regexp.new("" << Ezservo::OEM_START_CHAR << "0.(.*)" << \
                               Ezservo::OEM_END_CHAR)
    match = message_regex.match @response_packet_string
    match[1]
  end

  def error_code
    self.status_character & 15
  end

  def error_message 
    ERROR_NAMES[self.error_code]
  end
end