#!/usr/bin/env python
# driving demo for ROBOT magazine
# Michael E Ferguson
# www.blunderingbotics.com

from roboard import *
from Tkinter import *
import threading, sys, time

SCREEN_SIZE = 300.0

# parked speeds
LEFT_CENTER = 1510
RIGHT_CENTER = 1575

class miniBot(Tk):
    def __init__(self):
        Tk.__init__(self) 
        print "Minibot drive demo"
        self.config(bg = "#BBC6A1")         # make window nice green color
        self.resizable(0,0)
        self.protocol("WM_DELETE_WINDOW",self.end);
        # setup our main window
        self.title('Minibot Demo')

        Button(self,text="Forward",command=self.goForward).grid(row=0,column=1,pady=5,padx=5)
        Button(self,text="Left",command=self.turnLeft).grid(row=1,column=0,pady=5,padx=5)
        Button(self,text="Stop",command=self.stop).grid(row=1,column=1,pady=5,padx=5)
        Button(self,text="Right",command=self.turnRight).grid(row=1,column=2,pady=5,padx=5)
        Button(self,text="Backward",command=self.goBackward).grid(row=2,column=1,pady=5,padx=5)
        self.left = 0
        self.right = 0

        self.rbd = Roboard()
        self.rbd.ServoInit(RCSERVO_USECHANNEL2 + RCSERVO_USECHANNEL3)
        self.rbd.EnterPWMMode()

        self.shutdown = False
        t = threading.Thread(target=self.servoLoop)
        t.start()
        try:    
            self.mainloop()
        except KeyboardInterrupt:
            self.end()

    # Exit strategy
    def end(self):
        self.shutdown = True
        self.destroy()
        sys.exit(0)  

    def goForward(self):
        self.left = 200
        self.right = 240
    def goBackward(self):
        self.left = -200
        self.right = -240
    def stop(self):
        self.left = 0
        self.right = 0
    def turnLeft(self):
        self.left = 0
        self.right = 0
        self.rbd.SendPulse(2,LEFT_CENTER-50, 100)
        self.rbd.SendPulse(3,RIGHT_CENTER-100, 100)
    def turnRight(self):
        self.left = 0
        self.right = 0
        self.rbd.SendPulse(2,LEFT_CENTER+100,100)
        self.rbd.SendPulse(3,RIGHT_CENTER+50,100)

    def servoLoop(self):
        while not self.shutdown:    
            if self.right != 0:
                self.rbd.SendPulse(3, RIGHT_CENTER - self.right, 10)
            if self.left != 0:
                self.rbd.SendPulse(2, LEFT_CENTER + self.left, 10)
            while self.rbd.IsPWMCompleted(2) == False or self.rbd.IsPWMCompleted(3) == False:
                time.sleep(0.01)
        self.rbd.ServoClose()

if __name__ == "__main__":
    app = miniBot()