######################## 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'

class Ezservo
  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] }
  @@direction_translation = {:forward => "P", :backward => "D"}

  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
    @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 0 time 20`
    ez
  end 

  def Ezservo.mini_rover
    ez = Ezservo.new nil, '/dev/tts/4'
    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 
        #self.encoder_position = starboard_dist + 500
        sleep 1
      end
      if port_dir == :backward && positions[1] < port_dist
        self.active_motor_num = "A"
        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 = 1
    syslog "in turn, moving starboard dir #{starboard_dir} #{starboard_dist.to_s}"
    send_message "#{starboard_dir}" << starboard_dist.to_s 
    self.active_motor_num = 2
    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
    positions = self.encoder_position
    sleep 5
    new_positions = self.encoder_position
    while (positions != new_positions && !self.stop_wait) do
      sleep 5
      positions = new_positions
      new_positions = self.encoder_position
      if self.log_moves
        update = "encoder positions: "
        new_positions.each do |pos|
          update << pos.to_s << ", "
        end
        syslog update
      end
    end
    self.stop_wait = false
  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

  #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 = Thread.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 = ""
    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 .3
    syslog "command sent #{command}"
    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
