#########################################################################
#
#  Rover Supervisor - top level executive for Benthic Rover
#
#########################################################################

require "rover"   # Rover object created here
require "navigator"
require "ezservo"

include LogHelper

rv = Rover.instance

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Launch State ~~~~~~~~~~~~~~~~~~~~~~~~
# Begin mission, start with Launch state
#
syslog "_ Begin Launch State"

# Start servers needed for dive (acoustic modem, etc.)
#
# syslog "_ Starting acoustic modem"
# rv.am.run


# Launch state is simply sleeping for a proscribed duration long
# enough to reach benthic bliss
#
syslog "_ Sleeping #{rv.launch_duration} seconds"
sleep  rv.launch_duration

#
syslog "_ End Launch State"

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~


#~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Movement State ~~~~~~~~~~~~~~~~~~~~~~~
#
syslog "_ Begin Movement State"
syslog "_ Starting current meter"
rv.cm.run
sleep rv.cm_listen   # allow current meter to accumulate data

# Start Movement phase
#
rv.mv.run
while (rv.mv.moving?) do
  syslog "_ Waiting for move to finish"
  sleep rv.mv.wait_interval
  
  unless (
end



#############################################################################3
Rover.instance.relay.turn_on 1
Rover.instance.relay.turn_on 2
ez = Ezservo.rover
velocity = 500
ez.velocity = velocity
ez.log_moves = true

syslog "moving 2000 counts"
ez.forward 2000
sleep move_sleep
syslog "moving 4000 counts"
ez.forward 4000
sleep move_sleep
syslog "moving 6000 counts"
ez.forward 6000
sleep move_sleep
syslog "moving backwards"
ez.back 2000
ez.kill_listener
ez.serial_port.close
ez = nil
sleep sequence_sleep

nv = Navigator.new
syslog "using navigator to move forward 10 meters at 0 heading"
nv.forward 0, 6

sleep sequence_sleep

syslog "using navigator to move forward 10 meters at 90 heading"
nv.forward 90, 6

sleep sequence_sleep

syslog "moving 10 meters on 180 degree heading"
nv.forward 180, 6

sleep 3600
syslog "stopping current meter"
Rover.instance.acm.close
Rover.instance.relay.turn_off 1
Rover.instance.relay.turn_off 2