require "rover"
require "navigator"
require "ezservo"

include LogHelper
#number of seconds to sleep before starting script
drop_sleep = 5
#number of seconds to sleep before starting to move
bottom_sleep = 5
#number of seconds to sleep between moves
move_sleep = 5
#number of seconds to sleep between sequences of moves
sequence_sleep = 5

syslog "sleeping #{drop_sleep} seconds"
sleep drop_sleep
syslog "starting up current meter"
Rover.instance.acm.run
sleep bottom_sleep

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 1000
sleep move_sleep
syslog "moving 4000 counts"
ez.forward 1000
sleep move_sleep
syslog "moving 6000 counts"
ez.forward 1000
sleep move_sleep
syslog "moving backwards"
ez.back 1000
syslog "velocity is now #{velocity}"
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

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

sleep 60
syslog "stopping current meter"
Rover.instance.acm.close