// Lab 6
// file = spring.c
// Chuck Stauffer and Kevin Ziemer
// Haptic

#include "..\include\fqd.h"
#include "mpwmsm.h"
#include "mcpsm.h"
#include "..\include\m_mios1.h"
#include "clock.h"
#include "uimb.h"
#include "..\include\m_qadc.h"
#include "qadc64.h"
#include "..\include\mpc555.h"

//#include 

void main(void)
{
	unsigned short posCount, posCount1 = 0x8000;	// initial position
	long last = 0x8000, templast;
	long wall = 0x7000;				// Position of the wall
	int pwmModule = 0;
	unsigned short newPeriod = 1000;
	float dutyCycle = .50, torque_factor;
	float K = 144000;	// K is the torque value after 1 full wheel revolution
	

	asm("	mfmsr 	r3");					// set up registers
	asm("	ori 	r3,r3,0x2000");
	asm("	mtmsr 	r3");

	mcpsmInit();
	mpwmsmInit(pwmModule);

	mpwmsmPeriod(pwmModule, newPeriod);		// Set up period
	MIOS1.MPIOSM32DDR.R = 0xFFFF;			// I/O

	
	init_FQD(0);
	while(1)
	{
		posCount = ReadFQD_pc();			// Get position

		last = updateCounter(last, posCount, posCount1);	// Update last
		
		MIOS1.MPIOSM32DR.R = last;			// output position to leds
		
		if(last < wall)		// if in wall, change duty cycle
		{
			// Duty Cycle is calculated using the spring constant K.  K is
			// assumed to be entered as the amount of torque after 1 full wheel
			// revolution.
			dutyCycle = (((last-wall)/4096.0)*K + 2750.0)/5500.0;
		}
		else				// no torque
		{
			dutyCycle = .5;
		}
		mpwmsmDutyCycle(pwmModule, dutyCycle);	// update duty cycle

		posCount1 = posCount;					// update old position counter
	}

}