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

#include "..\include\m_mios1.h"	// including our lab1 header file
#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>
#include "..\include\mpc555hw.h"

volatile float m = .07559;


//#include 

void my_isr1(void)	// our first ISR, reads a value from the adc to change duty cycle
{
  	float dutyCycle = .5;	// our duty cycle variable
  	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;
	long x1, x2;
	static long x1prev = 0x8000;
	static long x2prev = 0x0;
  	static int my_pit_cntr = 0;	// interrupt counter
	float K = 3;
	
	float T = .001;		//  PIT period
	float torque; 
	
  	my_pit_cntr++;			// update counter for every interrupt handled
  	
	posCount = ReadFQD_pc();

	last = updateCounter(last, posCount, posCount1);
		
	MIOS1.MPIOSM32DR.R = last;
	
	x1 = x1prev + T * x2prev;
	x2 = x2prev + T * (-(K / m) * x1prev + (K/m) * last);
	torque = (float)K * (x1-last);
	dutyCycle = (float)(.5 + torque * 0.00000225);
		
	mpwmsmDutyCycle(pwmModule, dutyCycle);
	
	x1prev = x1;
	x2prev = x2;
	posCount1 = posCount;
  
  
}


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
	char byte_in;
	
	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

	//sysclockInit();
	//uimbInit();
	
	mcpsmInit();
	mpwmsmInit(pwmModule);
	
	init_pit(my_isr1);
	
	loopcnt = 0;

	mpwmsmPeriod(pwmModule, newPeriod);
	MIOS1.MPIOSM32DDR.R = 0xFFFF;
	
	init_FQD(0);
	
	 initCom(1);
	 puts("\n\n\n\n\n\n\n\n\rSerial Output Enabled.");

	enable_interrupts();

	while(1)
	{
		loopcnt++;            // Up count while waiting for interrupts
		if (SerialReady() == 0)
  		{
    		byte_in = GetSByte();
    		switch (byte_in)
    		{
        		case '-':
				m = (float)m - .05;
				break;
       			case '+':
				m = (float)m + .05;
				break;
			}
		}
	}

}