// Lab 6
// file = damper.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 "timers.h"
#include <math.h>


void my_isr1(void)	// our first ISR, reads a value from the adc to change duty cycle
{
  	float dutyCycle = .5;	// our duty cycle variable
	float velocity = 0;
  	int pwmModule = 0, channel = 0;	// our PWM module as well as our adc channel
  	unsigned short posCount;
	static unsigned short posCount1 = 0x8000;
	static long last = 0x8000;
	static long templast = 0x8000;
	long wall = 0x7000;
  	static int my_pit_cntr = 0;	// interrupt counter
	float K = 2;
	float C = 1;
	
  	my_pit_cntr++;			// update counter for every interrupt handled
  	
	posCount = ReadFQD_pc();

	last = updateCounter(last, posCount, posCount1);
		
	MIOS1.MPIOSM32DR.R = last;
		
	if(last < wall)
	{  //if in wall, change the duty cycle
		velocity = (float)(last - templast)/.001;	// get velocity
		dutyCycle = (float)(last-wall)*0.00000225*K + (C*velocity)*0.00000225 + .5;
					// calculate duty cycle
	}
	else  		// no torque
	{
		dutyCycle = .5;
	}
	
	mpwmsmDutyCycle(pwmModule, dutyCycle);		// update duty cycle
	
	templast = last;					// update templast for velocity calcs
	posCount1 = posCount;				// update poscount1
  
  
}


void main(void)
{
	
	
	//unsigned short edgeTime = 0;
	unsigned short posCount, posCount1 = 0x8000;
	long last = 0x8000, templast;
	long wall = 0x7000;
	int pwmModule = 0;
	unsigned short newPeriod = 1000;
	float dutyCycle = .50;
	unsigned int loopcnt;  // Loop count for debug
	
	asm("	mfmsr 	r3");
	asm("	ori 	r3,r3,0x2000");
	asm("	mtmsr 	r3");
	
	
  	disable_watchdog ();      // Disable watchdog timer
  	init_speed (); 
  	init_miosgpio (); 
	
	init_fp ();  // Initialize floating point unit

	mcpsmInit();
	mpwmsmInit(pwmModule);
	
	init_pit(my_isr1);
	
	
	loopcnt = 0;

	mpwmsmPeriod(pwmModule, newPeriod);
	MIOS1.MPIOSM32DDR.R = 0xFFFF;
	
	init_FQD(0);
	
	enable_interrupts ();

	while(1)
	{
		loopcnt++;            // Up count while waiting for interrupts
	}

}