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 = 20380
#number of seconds to sleep between moves
move_sleep = 30
#number of seconds to sleep between sequences of moves
sequence_sleep = 600

syslog "sleeping #{drop_sleep} seconds"
sleep drop_sleep

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

1.upto(2) do |count|
  syslog "beginning sequence #{count}"
  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
  syslog "sequence #{count} ended"
  syslog "increasing velocity by 500"
  velocity += 500
  ez.velocity = velocity
  syslog "velocity is now #{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
syslog "turning to 0 degrees"
cs.turn_to_heading 0
ez.velocity = 1000
ez.forward 1000
sleep move_sleep
ez.velocity = 500
syslog "turning to 90 degrees"
cs.turn_to_heading 90
sleep move_sleep
ez.velocity = 1000
ez.forward 1000
sleep move_sleep
ez.velocity = 500
syslog "turning to 0 degrees"
ez.turn_to_heading 0
sleep move_sleep
ez.velocity = 1000
ez.forward 1000

syslog "shutting down motor relay"
rb.turn_off 0