//Basic configureation of chip periferials
 #pragma config OSC = INTIO2, WDT = OFF, LVP = OFF
 #include <p18f1320.h>

//setup pins for PWM input
#define ReceiverPin PORTBbits.RB3
#define ReceiverTris TRISBbits.TRISB3

//PWM capture variables
unsigned int PWM1RiseTime = 0; //timer value at rising edge capture
unsigned int PWM1FallTime = 0; //timer value at falling edge capture
unsigned int PWM1Width = 0; //calculated width

unsigned int CH1_width = 0;
unsigned int CH2_width = 0;
unsigned int CH3_width = 0;
unsigned int PWMGap = 0; //calculated gap between pulses

char PWM1Edge = 1; //edge currently being monitored 1 = rising, 0 = falling 

//button variables
unsigned char button_1_debounce_wait = 0;

unsigned char calibration_mode = 0;
#define mode_operate 0
#define mode_CH1_high 1
#define mode_CH1_med 2
#define mode_CH1_low 3
#define mode_CH2_high 4
#define mode_CH2_med 5
#define mode_CH2_low 6


unsigned int limit_CH1_high = 2381;
unsigned int limit_CH1_med = 3307;
unsigned int limit_CH1_low = 4286;

unsigned int limit_CH2_high = 2022;
unsigned int limit_CH2_med = 2946;
unsigned int limit_CH2_low = 3983;
unsigned int CH_temp = 0;

//motor control variables
#define Motor_PWM_Rez 16 //number if different speeds possible forward and reverse
#define center_buffer 20 //this is the fraction of the range before movement starts

unsigned char Motor_Phase = 0;//as it cycles this will time the motors

unsigned int CH1_range = 2000;
unsigned char Motor_A_Speed = 0; //this is the speed of motor A, at %100 it will equal the rezolution
unsigned char CH1_forward_increment = 10;//the width of range for each speed output
unsigned char CH1_reverse_increment = 10;

unsigned int CH2_range = 2000;
unsigned char Motor_B_Speed = 0; //this is the speed of motor A, at %100 it will equal the rezolution
unsigned char CH2_forward_increment = 10;//the width of range for each speed output
unsigned char CH2_reverse_increment = 10;



typedef struct
  {
  unsigned motor_A_Direction: 1;
  unsigned motor_B_Direction: 1;
  unsigned button_1_last_state: 1;

  }BITS;

  unsigned char motor_A_inverted = 1;//this related to calibration
  unsigned char motor_B_inverted = 1;
  unsigned char motor_calibration_needed = 1;

volatile BITS Bits; 

//timing variables
unsigned char slow_count = 0; //this is used to create the scaled timer for slower events

//set up interrupt
void low_ISR(void);//prototype
#pragma code low_vector = 0x08 //0X08 IS LOW 0X18 IS HIGH
void low_interrupt (void){
_asm goto low_ISR _endasm
}
#pragma code
#pragma interrupt low_ISR

void main(void)
{	
	OSCCON = 0x72; //8MHz clock
	while(!OSCCONbits.IOFS); //Wait for OSC to become stable 

	//configure timer1
	PIR1bits.TMR1IF = 0; //clears the timer 1 interupt flag
	T1CONbits.TMR1ON = 1; //turn on timer
	T1CONbits.T1CKPS1 = 0; //set prescaler
	T1CONbits.T1CKPS0 = 0; //set prescaler


	//setup timer2
	PIR1bits.TMR2IF = 0; //clears the timer 2 interupt flag
	PIE1bits.TMR2IE = 1; //enable the interrupt
	PR2 = 199;
	T2CON = 0b00000100; //(-)always 0 (----) postscale (-)on/off (--) prescale

	//configure CCP1
	CCP1CON = 0b0000101; //configure CCP1 for capture, rising edge
	INTCONbits.PEIE=1;		//enable peripheral interrupts
    PIE1bits.CCP1IE=1;		//enabled CCP1 interrupt
    INTCONbits.GIE=1;		//enable branching to interrupt
	ReceiverTris = 1; //set RB3 for input so the capture can work.
	TRISBbits.TRISB2 = 1; //set rb2 for in so it can be used to differentiate channels
	
	//configure ports
	ADCON1 = 0xff; //all digital
	INTCON2bits.RBPU = 0; //port b weak pullups on
	
	//these will be motor outputs
	TRISAbits.TRISA0 = 0;
	#define Motor_Pin_A1 LATAbits.LATA0
	TRISAbits.TRISA1 = 0;
	#define Motor_Pin_A2 LATAbits.LATA1
	TRISAbits.TRISA2 = 0;
	#define Motor_Pin_B1 LATAbits.LATA2
	TRISAbits.TRISA3 = 0;
	#define Motor_Pin_B2 LATAbits.LATA3

	//these will be indicator outputs
	TRISAbits.TRISA6 = 0;
	TRISAbits.TRISA7 = 0;
	
	//this will be the servo signal input
	TRISBbits.TRISB0 = 1;
	
	//initially calibrate the RC ranges
	motor_calibration_needed = 1;

	while(1)
	{
	}
}



void low_ISR(void)
{		
	

	//Timer 2 flag (currently set to interrupt at 10Khz)
	if(PIR1bits.TMR2IF == 1)
	{
		PIR1bits.TMR2IF = 0; //clears the timer 1 interupt flag
		
		//withen this function executes at 100Hz***
		slow_count ++;
		if(slow_count > 100) 
		{
			slow_count = 1;//reset count for next time
			
			//Handle Calibration Button
			if(button_1_debounce_wait > 0){button_1_debounce_wait --;}
			if(PORTBbits.RB0 == 0){
				if(Bits.button_1_last_state == 0 && button_1_debounce_wait == 0)//button just pressed  
				{
					button_1_debounce_wait = 10;//set debounce count
					calibration_mode++;
					if(calibration_mode > 6){calibration_mode = 0;}
				}
				Bits.button_1_last_state = 1;
			}
			else
			{
				Bits.button_1_last_state = 0;
			}
			//end of calibration button

			//Handle Led Mode Indicators
			if(calibration_mode == mode_operate)
			{
				LATAbits.LATA6 = 0;
				LATAbits.LATA7 = 0;

				if(motor_calibration_needed == 1)
				{
					motor_calibration_needed = 0; //clear flag

					//recalculate calibration variables for CH1
					if(limit_CH1_low < limit_CH1_high)//speed increases as number increases
					{
						motor_A_inverted = 0;
					}
					else//speed decreases as number increases
					{
						//swap them so high is the greater value
						CH_temp = limit_CH1_low;
						limit_CH1_low = limit_CH1_high;
						limit_CH1_high = CH_temp;
		
						motor_A_inverted = 1;
					}
		
					CH1_range = limit_CH1_high-limit_CH1_low;
					CH1_forward_increment = (limit_CH1_high-limit_CH1_med -((limit_CH1_high-limit_CH1_med)/center_buffer))/Motor_PWM_Rez;
					CH1_reverse_increment = (limit_CH1_med-limit_CH1_low -((limit_CH1_med-limit_CH1_low)/center_buffer))/Motor_PWM_Rez;
				}

					//recalculate calibration variables for CH2
					if(limit_CH2_low < limit_CH2_high)//speed increases as number increases
					{
						motor_B_inverted = 0;
					}
					else//speed decreases as number increases
					{
						//swap them so high is the greater value
						CH_temp = limit_CH2_low;
						limit_CH2_low = limit_CH2_high;
						limit_CH2_high = CH_temp;
		
						motor_B_inverted = 1;
					}
		
					CH2_range = limit_CH2_high-limit_CH2_low;
					CH2_forward_increment = (limit_CH2_high-limit_CH2_med -((limit_CH2_high-limit_CH2_med)/center_buffer))/Motor_PWM_Rez;
					CH2_reverse_increment = (limit_CH2_med-limit_CH2_low -((limit_CH2_med-limit_CH2_low)/center_buffer))/Motor_PWM_Rez;
			

			}
			//end of led mode indicators

			//calibration
			if(calibration_mode == mode_CH1_high)
			{
				LATAbits.LATA6 = 0;
				LATAbits.LATA7 = 1;

				limit_CH1_high = CH1_width;

			}
			if(calibration_mode == mode_CH1_med)
			{
				LATAbits.LATA6 = 1;
				LATAbits.LATA7 = 1;

				limit_CH1_med = CH1_width;
			}
			if(calibration_mode == mode_CH1_low)
			{
				LATAbits.LATA6 = 1;
				LATAbits.LATA7 = 0;

				limit_CH1_low = CH1_width;
			}
			if(calibration_mode == mode_CH2_high)
			{
				LATAbits.LATA6 = 0;
				LATAbits.LATA7 = 1;

				limit_CH2_high = CH2_width;
			}
			if(calibration_mode == mode_CH2_med)
			{
				LATAbits.LATA6 = 1;
				LATAbits.LATA7 = 1;
		
				limit_CH2_med = CH2_width;
			}
			if(calibration_mode == mode_CH2_low)
			{
				LATAbits.LATA6 = 1;
				LATAbits.LATA7 = 0;
		
				limit_CH2_low = CH2_width;
		
				motor_calibration_needed = 1;
			}

			//calculate motor speed A
			Motor_A_Speed = 0;
			if(CH1_width > limit_CH1_med+((limit_CH1_high-limit_CH1_med)/center_buffer))//upper range
			{
				Motor_A_Speed = (CH1_width-limit_CH1_med -((limit_CH1_high-limit_CH1_med)/center_buffer))/CH1_forward_increment;
				Bits.motor_A_Direction = motor_A_inverted;
			}
			if(CH1_width < limit_CH1_med-((limit_CH1_med-limit_CH1_low)/center_buffer))//lower range
			{	
				Motor_A_Speed = (limit_CH1_med-CH1_width -((limit_CH1_med-limit_CH1_low)/center_buffer))/CH1_reverse_increment;
				Bits.motor_A_Direction = !motor_A_inverted;
			}

			//calculate motor speed B
			Motor_B_Speed = 0;
			if(CH2_width > limit_CH2_med+((limit_CH2_high-limit_CH2_med)/center_buffer))//upper range
			{
				Motor_B_Speed = (CH2_width-limit_CH2_med -((limit_CH2_high-limit_CH2_med)/center_buffer))/CH2_forward_increment;
				Bits.motor_B_Direction = motor_B_inverted;
			}
			if(CH2_width < limit_CH2_med-((limit_CH2_med-limit_CH2_low)/center_buffer))//lower range
			{	
				Motor_B_Speed = (limit_CH2_med-CH2_width -((limit_CH2_med-limit_CH2_low)/center_buffer))/CH2_reverse_increment;
				Bits.motor_B_Direction = !motor_B_inverted;
			}
			//end of calculating motor speed

		}//end of 100hz section

		//contol pulses to motor
		Motor_Phase++;
		if(Motor_Phase > Motor_PWM_Rez){Motor_Phase = 1;}
		
		//Motor A
		if(Motor_A_Speed >= Motor_Phase && Motor_A_Speed < 20){
			if(Bits.motor_A_Direction == 0){
				Motor_Pin_A1 = 1;
				Motor_Pin_A2 = 0;
			}
			if(Bits.motor_A_Direction == 1){
				Motor_Pin_A1 = 0;
				Motor_Pin_A2 = 1;
			}
		}
		else{
			Motor_Pin_A1 = 0;
			Motor_Pin_A2 = 0;
		}

		//Motor B
		if(Motor_B_Speed >= Motor_Phase && Motor_B_Speed < 20){
			if(Bits.motor_B_Direction == 0){
				Motor_Pin_B1 = 1;
				Motor_Pin_B2 = 0;
			}
			if(Bits.motor_B_Direction == 1){
				Motor_Pin_B1 = 0;
				Motor_Pin_B2 = 1;
			}
		}
		else{
			Motor_Pin_B1 = 0;
			Motor_Pin_B2 = 0;
		}

	}//end of timer interrupt


	//ccp interrupt
	if(PIR1bits.CCP1IF == 1)
	{	
		PIR1bits.CCP1IF = 0; //clear the flag
		if(PWM1Edge == 1)//if detecting rising
		{
			CCP1CON = 0b0000100;//switch to detect falling edge
			PWM1Edge = 0;//switch to indicate falling edge is next
			PWMGap = CCPR1 - PWM1FallTime; //calculate gap between pulse starts
			PWM1RiseTime = CCPR1;//save the low timer value for the rise time
			
			
			if(PWMGap < 10000){CH2_width = PWMGap;}
			

		}
		else //if detecting falling
		{
			CCP1CON = 0b0000101;//switch to detect rising edge
			PWM1Edge = 1;//switch to indicate rising edge is next
			PWM1Width = CCPR1 - PWM1RiseTime; //(pwm rise time is the time that the pwm rise occured)
			PWM1FallTime = CCPR1;//save the low timer value for the fall time

			if(PWMGap > 10000){CH1_width = PWM1Width;}
			if(PWMGap < 10000){CH3_width = PWM1Width;}
		}

		
	}
}