require 'relay_board'
require 'ezservo'
require 'rover_utils'
require 'acm'
require 'control_system'

include LogHelper
#number of seconds to sleep before starting script
drop_sleep = 16800
#number of seconds to sleep between moves
move_sleep = 30
#number of seconds to sleep between sequences of moves
sequence_sleep = 900

syslog "sleeping #{move_sleep} seconds"
sleep drop_sleep

rb = RelayBoard.new nil, "/dev/relay"
rb.turn_on 1
sleep 1
ez = Ezservo.mini_rover
velocity = 500
ez.velocity = velocity
ez.log_moves = true
acm = Acm.new "/dev/acm"
acm.run

1.upto(3) do |count|
  syslog "beginning sequence #{count}"
  ez.forward 2000
  sleep move_sleep
  ez.forward 4000
  sleep move_sleep
  ez.forward 6000
  sleep move_sleep
  syslog "moving backwards"
  ez.back 2000
  syslog "sequence #{count} ended"
  velocity += 500
  ez.velocity = velocity
  sleep sequence_sleep
end

cs = ControlSystem.new ez acm
syslog "calibrating control system"
cs.calibrate_ticks_per_degree

syslog "executing sequence of turns"
ez.velocity = 500
cs.turn_to_heading 0
ez.velocity = 1000
ez.forward 1000
sleep move_sleep
ez.velocity = 500
cs.turn_to_heading 90
sleep move_sleep
ez.velocity = 1000
ez.forward 1000
sleep move_sleep
ez.velocity = 500
ez.turn_to_heading 0
sleep move_sleep
ez.velocity = 1000
ez.forward 1000

syslog "shutting down motor relay"
rb.turn_off 1
