PhidgetMotorControl.py

"""
It's worth pointing out that python complex numbers are great for storing positions,
sizes, momentum, and polar/rect calculations.  See the cmath module for details.

"""

from random import random
import sys
import math
from cmath import polar, rect
import pygame
import pygame.time
from pygame.locals import *
from os.path import join, dirname, abspath
import time
pygame.init()

###############################################################################
def ConvertToInstance(cls):
	"""A class decorator which converts a class to an instance of the class"""
	return cls()

def G2S(Pos):
	"""Convert game coords to screen coords"""
	return Pos + ScreenCenter

def S2G(Pos):
	"""Convert screen coords to game coords"""
	return Pos - ScreenCenter

def C2T(C):
	"""Complex number to tuple of integers"""
	return (int(C.real), int(C.imag) )

def T2C(T):
	"""Complex number to tuple of integers"""
	return complex(T[0], T[1])

def DST(Pos1, Pos2):
	"""Absolute distance between two coords"""
	return polar(Pos2 - Pos1)[0]

def ANG(Pos1, Pos2):
	"""Angle from Pos1 to Pos2"""
	return polar(Pos2 - Pos1)[1]

def DST_ANG(Pos1, Pos2):
	"""Tuple of DST,ANG from Pos1 to Pos2"""
	return polar(Pos2 - Pos1)

def RND(min,max):
	return int(random()*(max-min)+min)

def COL(Thing1, Thing2):
	return polar(Thing2.Position - Thing1.Position)[0] < Thing1.Radius + Thing2.Radius


###############################################################################
@ConvertToInstance
class Input:
	def __init__(self):
		self.Up 		= False	#or True
		self.Down		= False #or True
		self.Left 		= False #or True
		self.Right 		= False	#or True
		self.Space 		= False	#or True
		self.Escape 	= False	#or True
		self.Mouse1 	= None 	#or complex of screen position
		self.Mouse3 	= None 	#or complex of screen position
	
	def Process(self):
		for event in pygame.event.get():
			if event.type == pygame.QUIT:
				raise SystemExit()
		
			elif event.type in (pygame.KEYDOWN, pygame.KEYUP):
				t = (event.type, event.dict['key'])
				if t in self.KeyMap:
					self.KeyMap[t](self, event)

			elif event.type in (pygame.MOUSEBUTTONDOWN, pygame.MOUSEBUTTONUP):
				t = (event.type, event.dict['button'])
				if t in self.MouseMap:
					self.MouseMap[t](self, event)
			else:
				pass

	def Up_DN(self, event):
		self.Up = True
	def Up_UP(self, event):
		self.Up = False
		
	def Down_DN(self, event):
		self.Down  = True
	def Down_UP(self, event):
		self.Down = False
	
	def Left_DN(self, event):
		self.Left = True
	def Left_UP(self, event):
		self.Left = False
	
	def Right_DN(self, event):
		self.Right = True
	def Right_UP(self, event):
		self.Right = False
	
	def Space_DN(self, event):
		self.Space = True
	def Space_UP(self, event):
		self.Space = False
	
	def Escape_DN(self, event):
		self.Escape = True
	def Escape_UP(self, event):
		self.Escape = False

	KeyMap = {
		(pygame.KEYDOWN, 	pygame.K_UP)	: Up_DN,
		(pygame.KEYUP, 		pygame.K_UP)	: Up_UP,	
		(pygame.KEYDOWN, 	pygame.K_DOWN)	: Down_DN,
		(pygame.KEYUP, 		pygame.K_DOWN)	: Down_UP,
		(pygame.KEYDOWN, 	pygame.K_LEFT)	: Left_DN,
		(pygame.KEYUP, 		pygame.K_LEFT)	: Left_UP,
		(pygame.KEYDOWN, 	pygame.K_RIGHT)	: Right_DN,
		(pygame.KEYUP, 		pygame.K_RIGHT)	: Right_UP,
		(pygame.KEYDOWN, 	pygame.K_SPACE)	: Space_DN,
		(pygame.KEYUP, 		pygame.K_SPACE)	: Space_UP,
		(pygame.KEYDOWN, 	pygame.K_ESCAPE): Escape_DN,
		(pygame.KEYUP,	 	pygame.K_ESCAPE): Escape_UP,
		}
	
	def Mouse1_DN(self, event):
		self.Mouse1 = complex(event.dict['pos'][0], event.dict['pos'][1])
	def Mouse1_UP(self, event):
		self.Mouse1 = None
	
	def Mouse3_DN(self, event):
		self.Mouse3 = complex(event.dict['pos'][0], event.dict['pos'][1])
	def Mouse3_UP(self, event):
		self.Mouse3 = None

	MouseMap = {
		(pygame.MOUSEBUTTONDOWN, 	1)		: Mouse1_DN, 
		(pygame.MOUSEBUTTONUP, 		1)		: Mouse1_UP, 
		(pygame.MOUSEBUTTONDOWN, 	3)		: Mouse3_DN, 
		(pygame.MOUSEBUTTONUP, 		3)		: Mouse3_UP, 
		}

###############################################################################


Running 		= True
Path 			= abspath(dirname(__file__))
FPS				= 60
ScreenSize 		= complex(1000,700)
Clock 			= pygame.time.Clock()
Ticks			= 0
TitleFont		= pygame.font.SysFont("Courier New", 18, True, False)
ScreenCenter	= ScreenSize / 2.0
Screen 			= pygame.display.set_mode(C2T(ScreenSize))


###############################################################################

# Initialize the screen

class Thingy():
	def __init__(self):
		self.Position = complex(0,0)
		self._Moved = False
	def Move(self, Direction):
		self.MoveTo(self.Position + Direction)
	def MoveTo(self, Position):
		oldpos = self.Position
		self.Position = Position
		self._Moved = True	
		
		if self.M0() > 100 or self.M0() < -100 or self.M1() > 100 or self.M1() < -100:
			self.Position = oldpos

	def Moved(self):
		if self._Moved:
			self._Moved = False
			return True
		else:
			return False

	def Tick(self):
		pass
	def Draw(self):
		pygame.draw.circle(Screen, (255,255,255), C2T(G2S(self.Position)), 100)

	@property
	def X(self):
		return self.Position.real

	def M0(self):
		return 0 - (int(self.Position.imag/3 + self.Position.real/3))

	def M1(self):
		return 0 + (int(self.Position.imag/3 - self.Position.real/3)) 

Thing = Thingy()

################################################################################
"""Copyright 2010 Phidgets Inc.
This work is licensed under the Creative Commons Attribution 2.5 Canada License. 
To view a copy of this license, visit http://creativecommons.org/licenses/by/2.5/ca/
"""

__author__ = 'Adam Stelmack'
__version__ = '2.1.7'
__date__ = 'May 17 2010'

#Basic imports
from ctypes import *
import sys
#Phidget specific imports
from Phidgets.PhidgetException import PhidgetErrorCodes, PhidgetException
from Phidgets.Events.Events import AttachEventArgs, DetachEventArgs, ErrorEventArgs, CurrentChangeEventArgs, InputChangeEventArgs, VelocityChangeEventArgs
from Phidgets.Devices.MotorControl import MotorControl
#import methods for sleeping thread
from time import sleep

import re

#Create an motorcontrol object
try:
	motorControl = MotorControl()
except RuntimeError as e:
	print("Runtime Exception: %s" % e.details)
	print("Exiting....")
	exit(1)

#Information Display Function
def displayDeviceInfo():
	print("|------------|----------------------------------|--------------|------------|")
	print("|- Attached -|-			  Type			  -|- Serial No. -|-  Version -|")
	print("|------------|----------------------------------|--------------|------------|")
	print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (motorControl.isAttached(), motorControl.getDeviceName(), motorControl.getSerialNum(), motorControl.getDeviceVersion()))
	print("|------------|----------------------------------|--------------|------------|")

#Event Handler Callback Functions
def motorControlAttached(e):
	attached = e.device
	print("MotorControl %i Attached!" % (attached.getSerialNum()))

def motorControlDetached(e):
	detached = e.device
	print("MotorControl %i Detached!" % (detached.getSerialNum()))

def motorControlError(e):
	source = e.device
	print("Motor Control %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))

def motorControlCurrentChanged(e):
	source = e.device
	print("Motor Control %i: Motor %i Current Draw: %f" % (source.getSerialNum(), e.index, e.current))

def motorControlInputChanged(e):
	source = e.device
	print("Motor Control %i: Input %i State: %s" % (source.getSerialNum(), e.index, e.state))
	global v0, v1
	
	if e.index == 1 and e.state == True:
	  v0 = -v0
	  v1 = -v1
	  print("Setting Velocity of Motor %i to %f" % (0,v0))
	  motorControl.setVelocity(0, v0)
	  print("Setting Velocity of Motor %i to %f" % (1,v1))
	  motorControl.setVelocity(1, v1)
 


def motorControlVelocityChanged(e):
	source = e.device
	print("Motor Control %i: Motor %i Current Velocity: %f" % (source.getSerialNum(), e.index, e.velocity))

#Main Program Code
try:
	motorControl.setOnAttachHandler(motorControlAttached)
	motorControl.setOnDetachHandler(motorControlDetached)
	motorControl.setOnErrorhandler(motorControlError)
	motorControl.setOnCurrentChangeHandler(motorControlCurrentChanged)
	motorControl.setOnInputChangeHandler(motorControlInputChanged)
	motorControl.setOnVelocityChangeHandler(motorControlVelocityChanged)
except PhidgetException as e:
	print("Phidget Exception %i: %s" % (e.code, e.details))
	print("Exiting....")
	exit(1)

print("Opening phidget object....")

try:
	motorControl.openPhidget()
except PhidgetException as e:
	print("Phidget Exception %i: %s" % (e.code, e.details))
	print("Exiting....")
	exit(1)

print("Waiting for attach....")

try:
	motorControl.waitForAttach(10000)
except PhidgetException as e:
	print("Phidget Exception %i: %s" % (e.code, e.details))
	try:
		motorControl.closePhidget()
	except PhidgetException as e:
		print("Phidget Exception %i: %s" % (e.code, e.details))
		print("Exiting....")
		exit(1)
	print("Exiting....")
	exit(1)
else:
	displayDeviceInfo()


motorControl.setVelocity(0, 0)
motorControl.setVelocity(1, 0)

oldpos = None

###############################################################################
while Running:

	# Process input loop
	Input.Process()
	Thing.Tick()

	# Handle input states
	if Input.Up:			
		Thing.Move(complex(0,-10))
	
	if Input.Down: 		
		Thing.Move(complex(0,10))
	
	if Input.Left: 		
		Thing.Move(complex(-10,0))
	
	if Input.Right: 		
		Thing.Move(complex(10,0))
	
	if Input.Escape:		
		Running = False

	if Input.Space:
		Thing.MoveTo(complex(0,0))

	if pygame.mouse.get_pressed()[0]:
		newpos = pygame.mouse.get_pos()
		if oldpos != newpos:
			oldpos = newpos
			Thing.MoveTo(S2G(T2C(newpos)))

	try:
		if Thing.Moved():
			print "Setting Velocity to " + str(Thing.M0()) + " and " + str(Thing.M1())
			motorControl.setVelocity(0, Thing.M0())
			motorControl.setVelocity(1, Thing.M1())
			
	except PhidgetException as e:
		print("Phidget Exception %i: %s" % (e.code, e.details))
	
	
	#Clear the screen
	Screen.fill((0,0,0))
	
	# Draw the screen
	pygame.draw.line(Screen, (0,0,255), C2T(G2S(complex(0,-1000))), C2T(G2S(complex(0,+1000))))
	pygame.draw.line(Screen, (0,0,255), C2T(G2S(complex(-1000,0))), C2T(G2S(complex(+1000,0))))

	Thing.Draw()
	
	# Render the title
	Text = "Phidget Control 001"
	surface = TitleFont.render(Text, True, (128,128,255))
	Screen.blit(surface, (0,0))

	# Flip the buffer
	pygame.display.flip()

	# Delay...
	Clock.tick(FPS)


	


print("Closing...")
try:
	motorControl.setVelocity(0, 0)
	motorControl.setVelocity(1, 0)
	motorControl.closePhidget()
except PhidgetException as e:
	print("Phidget Exception %i: %s" % (e.code, e.details))
	print("Exiting....")
	exit(1)

print("Done.")
exit(0)