#!/usr/bin/env python
# -*- coding: utf-8 -*-
from rccontrol import Servo, Esc
import time

if __name__ == "__main__":
    servo = Servo(3)
    servo.set_max_angle(30) # 30 degrees
    servo.set_offset('left', 5) # 5 degrees
    servo.start()
    esc = Esc(5)
    print 'start calibrating:'
    esc.calibrate_forward()
    raw_input('press enter key')
    esc.calibrate_backward()
    raw_input('press enter key')
    esc.calibrate_neutral()
    raw_input('press enter key')
    print 'done.'
    esc.start()

    while True:
        val = raw_input('input value: ')
        print val
        if (val == 'q'):
            servo.stop()
            esc.stop()
            exit()
        try:
            type_, v_ = val.split(',')
            if type_ == 'f':
                print 'forward, speed', v_
                esc.forward(int(v_))
            elif type_ == 'b':
                print 'backward, speed', v_
                esc.backward(int(v_))
            elif type_ == 'l':
                print 'turn left, angle', v_
                servo.turn_left(int(v_))
            elif type_ == 'r':
                print 'turn right, angle', v_
                servo.turn_right(int(v_))
        except:
            continue
        time.sleep(0.1)