#!/usr/bin/python2
#
# Blink the lights connected to the RomiPi
# The neopixel should turn red, yellow, then red
from romipi_astar.romipi_driver import AStar
import time
led_delay_s = 2.0
romi = AStar()
# turn on red LED
romi.leds(True, False, False)
romi.pixels(250, 0, 0)
print("LED RED")
print("PIXEL RED")
time.sleep(led_delay_s)
# turn on yellow LED
romi.leds(False, True, False)
romi.pixels(0, 250, 0)
print("LED YELLOW")
print("PIXEL GREEN")
time.sleep(led_delay_s)
# turn on green LED
romi.leds(False, False, True)
romi.pixels(0, 0, 250)
print("LED GREEN")
print("PIXEL BLUE")
time.sleep(led_delay_s)
# Move the robot forwards and backwards
from romipi_astar.romipi_driver import AStar
import time
motor_delay_s = 5.0
forward_speed_m_s = 0.25
stop_speed_m_s = 1.25
romi = AStar()
# int main()
# turn off all LEDs
romi.leds(False, False, False)
romi.pixels(0, 0, 0)
print("Displaying Circle Code")
var = input("Press s/S to stop and any key to continue: ") #input
while var != "s" and var != "S":
currentTime = time.time()
nextTime = currentTime + motor_delay_s
while currentTime < nextTime: #loops for prev time delay
romi.twist(forward_speed_m_s, stop_speed_m_s) # makes a circle
currentTime = time.time()
romi.twist(0.0, 0.0) # stop
var = input("Press s/S to stop and any key to continue: ") #input