# BtRobotOled.py

from raspibrick import *
from btpycom import *
from OLED1306 import OLED1306

def onStateChanged(state, msg):
    if state == "LISTENING":
        oled.println("Listening")
    elif state == "CONNECTED":
        oled.println("conn")
    elif state == "MESSAGE":
        go(msg)
        oled.println("go " + msg)
        
def go(msg):
    if msg == "FORWARD":
        gear.forward()
    elif msg == "BACKWARD":
        gear.backward()
    elif msg == "RIGHT":
        gear.rightArc(0.1)
    elif msg == "LEFT":
        gear.leftArc(0.1)
    elif msg == "STOP":
        gear.stop()

robot = Robot()
gear = Gear()
oled = OLED1306()
serviceName = "BTRobot"
server = BTServer(serviceName, stateChanged = onStateChanged)
while not isEscapeHit():
    Tools.delay(100)
server.terminate()
robot.exit()