#! /usr/bin/env mpscript
#
#  This script tests USB access by moving the first motor of a Phidget
#  USB motor controller from Phidgets, Inc.  The script assumes that the
#  Phidget controller has already had its firmware uploaded.  Running the
#  test script 'mphidget' found in the 'test' directory of the main MX
#  package is sufficient to do that.  However, if the MX test script
#  'mphidget' is still running when you invoke the current script, the
#  find_device_by_order() method will fail, since the device is already
#  in use by the 'mphidget' script.
#
#  Since this script accesses USB devices that are not handled by 'hotplug',
#  it is necessary to run this script as 'root' on Linux.
#

import struct
import time

def get_position( phidget ):

	status_string = phidget.bulk_read( 0x82, 16, 1.0 )

	unpack_format = "%iB" % len( status_string )

	status_tuple = struct.unpack( unpack_format, status_string )

	position = status_tuple[0] + ( status_tuple[1] << 8 ) \
			+ ( status_tuple[2] << 16 ) + ( status_tuple[3] << 24 )

	position = int( position )

	return position

def move( phidget, destination ):

	speed = 1000
	acceleration = 500

	destination = int( destination )

	move_string = struct.pack( "16B", 0, 0, \
				( acceleration & 0xff ), \
				( (acceleration >> 8) & 0xff ), \
				0, 0, 0, 0, \
				( destination & 0xff ), \
				( (destination >> 8) & 0xff ), \
				( (destination >> 16) & 0xff ), \
				( (destination >> 24) & 0xff ), \
				( speed & 0xff ), \
				( (speed >> 8) & 0xff ), \
				0, 0 )

	phidget.bulk_write( 0x01, move_string, 1.0 )

def main( record_list, argv ):

	libusb = record_list.get_record( 'libusb' )

	phidget = libusb.find_device_by_order( 0x6c2, 0x46, 0, 1, 0, 0, 0 )

	old_position = get_position( phidget )

	destination = argv[0]

	print( "Moving to", destination )

	move( phidget, destination )

	while (1):
		position = get_position( phidget )

		print( position )

		if ( position == old_position ):
			break

		old_position = position

		time.sleep(1.0)

	print( "Move complete." )
		
	phidget.delete_device()