///////////////////////////////////////////////////////////////////////////////
// Skin Stretch Device Control
//
// Control code to position the skin stretch device to a rotation 
// angle specified by the user in degrees.
//
// ***For Shensei motor and driver number: 55520
//
// Pete Shull     04 March 2009
///////////////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "phidget21.h"
#include "time.h"
#include <cmath> 
#include "windows.h"
#include <stdio.h>
#include <string.h>


//Display InterfaceKit properties 
int display_properties(CPhidgetInterfaceKitHandle phid)
{
	int serialNo, version, numInputs, numOutputs, numSensors, triggerVal, ratiometric, i;
	const char* ptr;

	CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
	CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
	CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);

	CPhidgetInterfaceKit_getInputCount(phid, &numInputs);
	CPhidgetInterfaceKit_getOutputCount(phid, &numOutputs);
	CPhidgetInterfaceKit_getSensorCount(phid, &numSensors);
	CPhidgetInterfaceKit_getRatiometric(phid, &ratiometric);

	printf("%s\n", ptr);
	printf("Serial Number: %10d\nVersion: %8d\n", serialNo, version);
	printf("# Digital Inputs: %d\n# Digital Outputs: %d\n", numInputs, numOutputs);
	printf("# Sensors: %d\n", numSensors);
	printf("Ratiometric: %d\n", ratiometric);

	for(i = 0; i < numSensors; i++)
	{
		CPhidgetInterfaceKit_getSensorChangeTrigger (phid, i, &triggerVal);

		printf("Sensor#: %d > Sensitivity Trigger: %d\n", i, triggerVal);
	}

	return 0;
}
double enterRotationAngle()
{
	bool valid_entry;
	double rot_angle;
	
	rot_angle = 0;

	valid_entry = false;
	do{
		printf("\nEnter rotation (deg): ");
		scanf ("%lf",&rot_angle);  
		if((rot_angle > -80) && (rot_angle < 80))
			valid_entry = true;
		else
			printf("Invalid Entry! Rotation angle must be between -80deg to 80deg\n\n");

	}while(valid_entry == false);

	return (double)rot_angle;
}
bool continueLooping()
{
	bool valid_entry, continue_loop;
	char continue_str[80];

	valid_entry = false;

	do{
		printf("Keep going? ('y' or 'n') ");
		scanf("%s",continue_str);
		if(strcmp(continue_str,"y") == 0)
		{
			valid_entry = true;
			continue_loop = true;
		}
		else if(strcmp(continue_str,"n") == 0)
		{
			valid_entry = true;
			continue_loop = false;
		}
		else
		{
			valid_entry = false;
			printf("Invalid Entry! Please type: 'y' to keep going and 'n' to quit\n\n");
		}
	}while(valid_entry == false);

	return continue_loop;
}

void setMotorSpd(double speed_volts, int direction, CPhidgetInterfaceKitHandle phid)
{
	int i = 0, binary_out, remain, state;
	double Vmax;
	int digital_output_arr[8];

	// Zero out arrays
	for(i=0;i<8;i++)
		digital_output_arr[i] = 0;
	i = 0;

	// Make sure speed_volts is in the valid range
	Vmax = 3.2;
	if(speed_volts > Vmax)
		speed_volts = Vmax;
	else if(speed_volts < 0)
		speed_volts = 0;

	// Convert from speed_volts to a binary digital output array	
	binary_out = (int)((255 * speed_volts)/Vmax);
	while(binary_out > 0)
	{
		remain = binary_out % 2;
		binary_out = binary_out / 2;
		digital_output_arr[i++] = remain;
	} 
	 
	// Send speed digital output to Phidgets board
	for(i=0;i<8;i++)
	{
		state = digital_output_arr[i];
		
		// Flip ones and zeros
		if(state == 0)
			state = 1;
		else
			state = 0;

		CPhidgetInterfaceKit_setOutputState(phid,i,state);
	}

	// Send direction digital output to Phidgets board
	if(direction == 1)
	{
		CPhidgetInterfaceKit_setOutputState(phid,15,0);
		CPhidgetInterfaceKit_setOutputState(phid,14,1);
	}
	else if(direction == -1)
	{
		CPhidgetInterfaceKit_setOutputState(phid,15,1);
		CPhidgetInterfaceKit_setOutputState(phid,14,0);
	}
	else
	{
		CPhidgetInterfaceKit_setOutputState(phid,15,1);
		CPhidgetInterfaceKit_setOutputState(phid,14,1);
	}    
}

int skin_stretch_device()
{
	int result, i, direction;
	const char *err;
	double speed_volts;
	bool continue_loop;
	
	double Ts, time_len, dest_tolerance, kp, rot_angle, rot_angle_total;
	double pos, diff;
	int num_points;
	int encoder_count;
	double pos_arr[500];

	/****************************************************************/
	// Initialize devices
	// Declare and open interface kit 
	CPhidgetInterfaceKitHandle ifKit_handle = 0;
	CPhidgetInterfaceKit_create(&ifKit_handle);
	CPhidget_open((CPhidgetHandle)ifKit_handle, -1);

	// Declare and open encoder 
	CPhidgetEncoderHandle encoder_handle = 0;
	CPhidgetEncoder_create(&encoder_handle);
	CPhidget_open((CPhidgetHandle)encoder_handle, -1);

	// Wait for the interface kit device to be attached
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit_handle, 2500)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem attaching Interface Kit: %s\n", err);
		return 0;
	}
	printf("[Interface Kit attached]\n");

	// Wait for the encoder device to be attached
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)encoder_handle, 2500)))
	{
		CPhidget_getErrorDescription(result, &err);
		printf("Problem attaching Encoder: %s\n", err);
		return 0;
	}
	printf("[Encoder attached]\n\n");

	/****************************************************************/
	// Run control code
	Ts = 0.01; //sec
	time_len = 2; //sec
	num_points = (int)(time_len/Ts);
        
	dest_tolerance = 0.1; //deg
	kp = 0.5;
	continue_loop = true;
	rot_angle = 0;
	rot_angle_total = 0;

	while(continue_loop)
	{
		// User enters a desired rotation angle in degrees
		rot_angle = enterRotationAngle();
		rot_angle_total += rot_angle;

		Sleep(25); // Sleep for 25ms

		// Start closed-loop control to position skin stretch device
		for(i=0;i<num_points;i++)
		{
			Sleep(10); // Sleep for 10ms, running loop at 100Hz

			// Get current location
			CPhidgetEncoder_getPosition(encoder_handle, 0, &encoder_count);
			pos = (double)encoder_count * ((double)360)/((double)5000); //convert encoder counts to deg
			pos_arr[i] = pos;

			// Control calcs
			diff = rot_angle_total - pos;
			if(abs(diff) < dest_tolerance)
			{
				speed_volts = 0;
				direction = 0;
			}
			else
			{
				speed_volts = abs(kp*diff);
				if(diff >= 0)
					direction = 1;
				else
					direction = -1;
			}

			setMotorSpd(speed_volts,direction,ifKit_handle);
		}
		
		// Shut motor off
		setMotorSpd(0,0,ifKit_handle);

		// User decides whether to invoke another rotation or to stop
		continue_loop = continueLooping();
	}

	// Clean up
	setMotorSpd(0,0,ifKit_handle);
	CPhidget_close((CPhidgetHandle)ifKit_handle);
	CPhidget_delete((CPhidgetHandle)ifKit_handle);
	CPhidget_close((CPhidgetHandle)encoder_handle);
	CPhidget_delete((CPhidgetHandle)encoder_handle);
	return 0;
}

int _tmain(int argc, _TCHAR* argv[])
{
	skin_stretch_device();
	return 0;
}