#include <p18f6585.h>

////////// Define pins to make program easier to read //////////
#define RdyIndex PORTBbits.RB1 // Output
#define NoPocketFound PORTBbits.RB2 // Output
#define JamOccured PORTBbits.RB3 // Output
#define MoveDone PORTBbits.RB5 // Output
#define MotorDir PORTBbits.RB6 // Output
#define MotorStep PORTBbits.RB7 // Output

#define SprocketSnsr PORTCbits.RC0 // High Speed Input
#define JamSnsr PORTCbits.RC5 // High Speed Input

#define CMDBit3 PORTDbits.RD0 // Input
#define CMDBit1 PORTDbits.RD1 // Input
#define CMDSet PORTDbits.RD2 // Input
#define CMDBit2 PORTDbits.RD3 // Input
#define CMDBit4 PORTDbits.RD6 // Input

#define DataBit2 PORTEbits.RE0 // Input
#define DataBit3 PORTEbits.RE1 // Input
#define DataBit4 PORTEbits.RE2 // Input
#define DataBit1 PORTEbits.RE3 // Input

////////// Variables //////////
unsigned char TapeMoving, MotionSegment, RampIndex;
unsigned int SegmentPulsesTraveled, Segment1Pulses, Segment2Pulses, PulsePeriod, OverflowCount;


#pragma udata RampTable
unsigned int RampTbl[100] = {
5000,253,126,84,63,51,42,36,32,28,25,23,21,19,18,17,16,15,14,13,13,12,11,11,11,10,
10,9,9,9,8,8,8,8,7,7,7,7,7,6,6,6,6,6,6,6,5,5,5,5,5,
5,5,5,5,5,5,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,3,3,3,3,
3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2};


////////// Function Prototypes //////////
void high_isr(void);


#pragma code interrupt_at_high_vector = 0x08
void interrupt_at_high_vector(void)
{
	_asm
	GOTO high_isr
	_endasm
}

#pragma code 
#pragma interrupt high_isr
void high_isr(void)
{
	if(PIR1bits.TMR2IF)
	{
		PIR1bits.TMR2IF = 0;
		TMR2 = 0xAA; // Reset timer 2 to overflow in 20.2 uS
		OverflowCount++;
		if(OverflowCount >= 10)
		{
			OverflowCount = 0;
			RampIndex++;
		}
		if(MotorStep) // If step signal is on
		{
			MotorStep = 0; // Turn step signal off
			SegmentPulsesTraveled++;
			OverflowCount = 0;
		}	
		else if(OverflowCount >= PulsePeriod)
		{
			MotorStep = 1; // Turn step signal on
		}
	}
	
}

void main(void)
{	
	Segment1Pulses = 194;
	Segment2Pulses = 194;

	
	TRISBbits.TRISB7 = 0; // Tape advance motor step
	TRISBbits.TRISB6 = 0; // Tape advamce motor direction
	TRISBbits.TRISB5 = 0; // Tape move done
	TRISCbits.TRISC5 = 1; // Jam sensor
	TRISCbits.TRISC1 = 1; // Advance tape signal <------------ Not sure of acutal pin
	TRISCbits.TRISC0 = 1; // Sprocket hole sensor

	TRISD &= 0x0E;
	TRISD = 0x00;
	
	TapeMoving = 0;
	
	RCONbits.IPEN = 0; // Disable interrupt priorities
	INTCON = 0xC0;
	PIE1bits.TMR2IE = 1; // Enable interrupt on timer 2 overflow
	T2CON = 0x00; // 1:1 Postscale, Timer off, 1:1 Prescale
	
	MotionSegment = 1;
	RampIndex = 0;	
	
	while(1)
	{
		if(CMDBit1 && CMDBit2 && CMDSet) // Check for move signal
		{
			while(MotionSegment == 1) // Acceleration phase of motion profile
			{	
				MotorDir = 1;
				MotorStep = 1;
				TMR2 = 0xAA; // Reset timer 2 to overflow in 20.2 uS
				T2CONbits.TMR2ON = 1; // Turn the timer on
				if(SegmentPulsesTraveled < Segment1Pulses)
				{
					OverflowCount = RampTbl[RampIndex];
				}
				else
				{
					MotionSegment = 2;
				}
			}
			while(MotionSegment == 2)
			{
				MotionSegment = 1; // Temporary for debug
			}
			
		}
		
		
		
		

	}
}








/*
		while(!(PORTDbits.RD1 && PORTDbits.RD2 && PORTDbits.RD3)); // Wait for move signal
	
		if(MotionSegment == 1) // Acceleration phase of motion profile
			{		
				if((SegmentPulsesTraveled < (970000*(.000025*.000025)*SegmentTime*SegmentTime*.5)) && (SegmentPulsesTraveled < 194))
				{
					PulseMotor = 1; // Move a step
					SegmentPulsesTraveled++;
				}
				else
				{
					MotionSegment = 2;
					SegmentTime = 0;
					SegmentPulsesTraveled = 0;
				}	
			}
			
			if(MotionSegment == 2) // Deceleration before constant velocity phase
			{		
				
				// After clock increments to about 35, start checking for sprocket sensor to transition
				// from low to high. If that happens or if the frequency drops below constant velocity freq,
				// set MotionSegment to 3.
				if(SegmentPulsesTraveled < (970000*SegmentTime*SegmentTime))
				{
					PORTBbits.RB7 = 1; // Turn step signal on
					SegmentPulsesTraveled++;
				}
				else
				{
					MotionSegment = 3;
					SegmentTime = 0;
				}	
			}
			if(MotionSegment == 3) // Constant velocity phase to look for far edge of sprocket hole
			{			
				// Check every clock increment for sprocket sensor. If transitioned from high to low, set 
				// MotionSegment to 4.		
				if(SegmentPulsesTraveled < (970000*SegmentTime*SegmentTime))
				{
					PORTBbits.RB7 = 1; // Turn step signal on
					SegmentPulsesTraveled++;
				}
				else
				{
					MotionSegment = 4;
					SegmentTime = 0;
				}	
			}
			if(MotionSegment == 4) // Deceleration phase after locating far edge of pocket hole
			{
				SegmentTime = 0;
				// This segment is to rapidly decelerate to a stop
			}
			TapeMoving = 0;
		}
}
*/