require "rover"
require "control_system"

def doMove
  puts("Starting the Movement phase")
  Rover.instance.acm.run
  relay = Rover.instance.relay
  relay.motorsOn
  motors = Rover.instance.motors
  motors.close

  relay.motorsOn
  sleep(3)
  Rover.instance.motors = Ezservo.rover
  motors = Rover.instance.motors
  motors.proportional_gain = 1000
  motors.integral_gain = 0
  motors.differential_gain = 2500
  motors.velocity = 2000
  control_system = ControlSystem.new motors, Rover.instance.acm
  puts "moving 1500 counts at 180 heading"
  control_system.forward 180, 1500
  puts "first move complete"
  sleep 10
  puts "moving 1500 counts at 270 heading"
  control_system.forward 270, 1500
  puts "second move complete"
  sleep 10
  puts "moving 1500 counts at 0 heading"
  control_system.forward 0, 1500
  puts("Movement done!")
  relay.motorsOff
end



############################# Mission Main ######################


puts("Starting mission!")

launchTime = 5
puts("Launch phase, sleep for #{launchTime} seconds")
sleep(launchTime)

Rover.instance.relay.v12On
doMove
Rover.instance.relay.v12Off