import sys
from os import path
sys.path.append(path.abspath(path.join(path.dirname(__file__), '..')))

from Things.DifferentialDrive import DifferentialDrive
from Things.TouchSensor import TouchSensor
import time

# The robot's propulsion is a differential drive
bb2b = DifferentialDrive()
# It has a touch sensor at the front left of the roboid
touch_left = TouchSensor(parent = bb2b)
# and other one on the right
touch_right = TouchSensor(parent = bb2b)

# Do forever:
while True:
    # The left wheel turns forward when nothing is touched on the right side
    # and turn backward when the roboid hits something on the right
    wheel_speed_left = -600 if touch_right.touched_something else 600
    # The right wheel does the same, but instead is controlled by
    # touches on the left side
    wheel_speed_right = -600 if touch_left.touched_something else 600
    # When both sides are touching something, both wheels will turn backward
    # and the roboid will move backwards
    bb2b.SetWheelVelocity(wheel_speed_left, wheel_speed_right)

    # Update the roboid state
    bb2b.Update(True)
    # and sleep for 100ms
    time.sleep(100)