# Motor Controller
# Example application for using motors.
#
# Copyright (c) 2014 Charles Weir.  Shared under the MIT Licence.
 
import sys, os # Python path kludge - omit these 2 lines if BrickPython is installed.
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(sys.argv[0]))))
 
from BrickPython.TkApplication import TkApplication
from BrickPython.Motor import PIDSetting
from BrickPython.Sensor import Sensor
import logging
 
class MotorControllerApp(TkApplication):
    '''Application to control a lego NXT motor on port A as a servo motor.
 
    Type keystrokes into the application window to make the motor move:
    Numbers 0-9 make it move forward the corresponding number of quarter-turns.
    Lower case letters a-p make it go backward the corresponding number of quarter turns.
    Capital letter A stops the motor.
    Capital letters B-G make it go forward at a constant speed.  B is a quarter turn per second, C a half turn, and so on.
    Letters xyz and XYZ adjust the settings for the PID Servo Motor algorithm:
    * X,x increase and decrease the 'distance multiplier' - the P setting.
    * Y,y increase and decrease the 'speed multiplier' - the D setting (I think).
    * Z,z increase and decrease the 'Integrated distance multimplier' - the I setting.
    '''
 
    def __init__(self):
        TkApplication.__init__(self, {'1': Sensor.ULTRASONIC_CONT })
        self.pidSetting = PIDSetting()
 
    def rotate(self, degrees):
        'Rotate motor A through *degrees*'
        self.stopAllCoroutines()
        motor = self.motor('A')
        logging.info( "Rotating motor A %d degrees" % degrees )
        co = motor.moveTo( motor.position() + degrees*2 )
        self.addActionCoroutine( co )
 
    def setSpeed(self, speed):
        'Set the speed of motor A'
        self.stopAllCoroutines()
        motor = self.motor('A')
        logging.info( "Speed for motor A %.2f" % speed )
        co = motor.runAtConstantSpeed( speed )
        self.addActionCoroutine( co )
 
    def onKeyPress(self, event):
        'Handle user keystroke'
        if TkApplication.onKeyPress(self, event):
            return
 
        char = event.char
        if char in "0123456789":
            # rotate forwards:
            self.rotate(90 * (ord(char) - ord("0")))
        elif char in "abcdefghijklmnop":
            # rotate reverse:
            self.rotate(-90 * (ord(char) - ord("a") + 1))
        elif char in "ABCDEFG":
            self.setSpeed(180 * (ord(char) - ord("A") ))
 
        # Adjust PID settings:
        elif char in "xyzXYZ":
            if char == 'x':
                self.pidSetting.distanceMultiplier *= 1.1
            elif char == 'X':
                self.pidSetting.distanceMultiplier /= 1.1
            elif char == 'y':
                self.pidSetting.speedMultiplier *= 1.1
            elif char == 'Y':
                self.pidSetting.speedMultiplier /= 1.1
            elif char == 'z':
                self.pidSetting.integratedDistanceMultiplier *= 1.1
            elif char == 'Z':
                self.pidSetting.integratedDistanceMultiplier /= 1.1
            logging.info( "%r" % (self.pidSetting) )
            self.motor('A').setPIDSetting( self.pidSetting )
 
if __name__ == "__main__":
    logging.basicConfig(format='%(message)s', level=logging.INFO) # Logging is a simple print
    logging.info( "Starting" )
    app = MotorControllerApp()
    app.mainloop()