// ver1.c This program is to be compiled with the CCS C cimpiler
// http://www.ccsinfo.com

// 5-29-2001
// Increase deadband from 20 counts to 30 counts
// Moved the check for > 3000 out of timing section
// Reduced max PWM from 511 to 509

// 5-29-2001
// That didn't seem to work, so I am setting current pulse to last pulse
// if current pulse is > 3000

// 5-29-2001
// PWM is still wrapping around so I will reduce it to 500


// Port A usage
//
//   bit 0 - (Used on eval board as A/D pot)
//   bit 1 - 
//   bit 2 - 
//   bit 3 - 
//   bit 4 - 
//   bit 5 - 
//   bit 6 - (Not available on 28 pin part)
//   bit 7 - (Not available on 28 pin part)

// Port B usage
//
//   bit 0 - (Used on eval board for programming and interrupt button)
//   bit 1 - Channel 1 servo pulse input
//   bit 2 - Channel 2 servo pulse input
//   bit 3 - (Used on eval board for programming)
//   bit 4 - 
//   bit 5 - 
//   bit 6 - (Used on eval board for programming)
//   bit 7 - (Used on eval board for programming)

// Port C usage
//
//   bit 0 - MOSFET driver chip enables
//   bit 1 - Channel 2 PWM output
//   bit 2 - Channel 1 PWM output
//   bit 3 - Channel 2 reverse enable
//   bit 4 - Channel 2 forward enable
//   bit 5 - Channel 1 reverse enable
//   bit 6 - Channel 1 forward enable
//   bit 7 - Used for debug

#define DEADBAND 40
#define MAX_PULSE 2300
#define MIN_PULSE 1100
#define MID_PULSE 1690
#define MAX_PWM 500


#include <\picc\examples\16f876.h>

#fuses xt,nowdt,noprotect,nolvp

#use delay(clock=20000000)

#use fast_io(C)

#byte port_c=7


// Note:
//  short is 1 bit
//    int is 8 bits
//   long is 16 bits
// signed long is 16 bits

long kludge;
long swap_space;
long pulse1;
long last_pulse1;
long pulse2;
long last_pulse2;
signed long stick1;
signed long stick2;
short ch1_dir;		// Channel 1 direction flag
short ch2_dir;		// Channel 2 direction flag

	short stopped;



#int_timer1

// Timer1 overflow interrupt routine
//
// If timer1 ever overflows then we had loss of signal or the program got confused
// Turn off both channels if timer1 ever overflows

timer1_overflow() {


	output_bit(PIN_C0, 0);	// Disable MOSFET drivers
	stopped = 1;		// Indicate we have shut down


	kludge = 0;
	set_pwm1_duty(kludge);
	set_pwm2_duty(kludge);

	output_bit(PIN_C3, 0);
	output_bit(PIN_C4, 0);
	output_bit(PIN_C5, 0);
	output_bit(PIN_C6, 0);

	setup_timer_1(T1_DISABLED);

}



main()	{

	port_c = 0;		// Clear port C before the direction is set
	set_tris_c(0x00);	// Set port C to all outputs

	stopped = 1;		// Indicate MOSFET drivers are shut down

	setup_ccp1(CCP_PWM);   // Configure CCP1 module for PWM output
	setup_ccp2(CCP_PWM);   // Configure CCP2 module for PWM output

	kludge = 0;
	set_pwm1_duty(kludge);
	set_pwm2_duty(kludge);



	// Setup timer 2 for PWM period

	// The cycle time will be (1/clock)*4*t2div*(period+1)
	// In this program clock=20000000 and period=127 (below)
	// For the three possible selections the cycle time is:
	//     (1/20000000)*4*1*128 =  25.6 us or 39.062 khz
	//     (1/20000000)*4*4*128 = 102.4 us or 9.765 khz
	//     (1/20000000)*4*16*128= 409.6 us or 2.441 khz

	// So let's pick t2div of 16 so we get 409.6us or 2.441khz
	setup_timer_2(T2_DIV_BY_16, 127, 1);

/*
		// Setup timer 1 to measure incoming servo pulses
		setup_timer_1(T1_INTERNAL | T1_DIV_BY_4);

		// Enable all the driver chips
		delay_ms(1000); // 1 second delay before enabling MOSFET drivers
		output_bit(PIN_C0, 1);

		// Clear timer1 so we have 65ms before it times outs
		set_timer1(0);

		enable_interrupts(INT_TIMER1);	// Enable timer1 overflow interrupt
		enable_interrupts(GLOBAL);	// Main interrupt enable
*/


	while (true) {



		if (stopped) {

			disable_interrupts(GLOBAL);	// Main interrupt enable
			disable_interrupts(INT_TIMER1);	// Enable timer1 overflow interrupt

			stopped = 0;

			port_c = 0;		// Turn everything off

			// Enable all the driver chips
			delay_ms(1000); // 1 second delay before enabling MOSFET drivers
			output_bit(PIN_C0, 1);
	
			// Setup timer 1 to measure incoming servo pulses
			setup_timer_1(T1_INTERNAL | T1_DIV_BY_4);

			last_pulse1 = MID_PULSE;
			last_pulse2 = MID_PULSE;

			// Clear timer1 so we have 65ms before it times outs
			set_timer1(0);

			enable_interrupts(INT_TIMER1);	// Enable timer1 overflow interrupt
			enable_interrupts(GLOBAL);	// Main interrupt enable

		}


		output_bit(PIN_C7, 1);	// Indicate we are waiting for activity

// Measure the first pulse width

		// Clear timer1 so we have 65ms before it times outs
		set_timer1(0);

		// Wait for no pulse on channel 1 so we start in the middle of it
		while (input(PIN_B1)) {
		}



		// Clear timer1 so we have 65ms before it times outs
		set_timer1(0);

		// Wait for start of pulse on channel 1
		while (!input(PIN_B1)) {
		}

		// Timer1 is used to measure high period and to detect loss of pulse
		// Clear timer1 so we have 65ms before it times outs
		set_timer1(0);

		// Wait for end of pulse on channel 1
		while (input(PIN_B1)) {
		}

		pulse1 = get_timer1();



// Now measure the second pulse width

		// Clear timer1 so we have 65ms before it times outs
		set_timer1(0);

		// Wait for start of pulse on channel 2
		while (!input(PIN_B2)) {
		}

		// Timer1 is used to measure high period and to detect loss of pulse
		// Clear timer1 so we have 65ms before it times outs
		set_timer1(0);

		// Wait for end of pulse on channel 2
		while (input(PIN_B2)) {
		}

		pulse2 = get_timer1();




		// Check for out of range pulse width	
		if (pulse1 > MAX_PULSE)
			pulse1 = last_pulse1;
		if (pulse1 < MIN_PULSE)
			pulse1 = last_pulse1;


		// Check for out of range pulse width	
		if (pulse2 > MAX_PULSE)
			pulse2 = last_pulse2;
		if (pulse2 < MIN_PULSE)
			pulse2 = last_pulse2;


		last_pulse1 = pulse1;
		last_pulse2 = pulse2;



		// Clear timer1 so we have 65ms before it times outs
		set_timer1(0);

/*
		if (input(PIN_xx)) {
			output_bit(PIN_C7, 1);
			swap_space = pulse1;
			pulse1 = pulse2;
			pulse2 = swap_space;
		} else {
			output_bit(PIN_C7, 0);
		}
*/

		output_bit(PIN_C7, 0);


// Convert pulse width value to signed value and set direction flag
// Check for deadband around center joystick position
// If not in deadband, output new direction bit


if (!stopped) {

// First do channel 1

		// Convert to signed number with 0 at center of joystick travel
		stick1 = pulse1 - MID_PULSE; // Make it ~ +/-550

		// Take absolute value and set direction flag
		if (stick1 < 0) {
			pulse1 = -stick1;
			ch1_dir = 0;
		} else {
			pulse1 = stick1;
			ch1_dir = 1;
		}

		// Limit the max PWM value
		if (pulse1 > MAX_PWM) pulse1 = MAX_PWM;

		// Add some deadband
		if (pulse1 < DEADBAND) {
			pulse1 = 0;
			output_bit(PIN_C5, 0);
			output_bit(PIN_C6, 0);

		} else {
			output_bit(PIN_C5, ch1_dir);
			output_bit(PIN_C6, !ch1_dir);
		}




// Now do channel 2

		// Convert to signed number with 0 at center of joystick travel
		stick2 = pulse2 - MID_PULSE; // Make it ~ +/-550

		// Take absolute value and set direction flag
		if (stick2 < 0) {
			pulse2 = -stick2;
			ch2_dir = 0;
		} else {
			pulse2 = stick2;
			ch2_dir = 1;
		}

		// Limit the max PWM value
		if (pulse2 > MAX_PWM) pulse2 = MAX_PWM;

		// Add some deadband
		if (pulse2 < DEADBAND) {
			pulse2 = 0;
			output_bit(PIN_C3, 0);
			output_bit(PIN_C4, 0);
		} else {
			output_bit(PIN_C3, ch2_dir);
			output_bit(PIN_C4, !ch2_dir);
		}


		// This sets the time the pulse is high each cycle.
		// the high time will be:
		//  if value is LONG INT (16 bit value):
		//    value*(1/clock)*t2div
		//  if value is INT (8 bit value):
		//    value*4*(1/clock)*t2div
		// for example:
		//    a clock of 20Mhz
		//    a value of 30
		//    and t2div of 1 the high time is 6us
		// WARNING:  A value to high or low will
		//           prevent the output from changing.

		// So we are looking for a count of 512 for max PWM

		// Output both PWM values
		set_pwm1_duty(pulse1);
		set_pwm2_duty(pulse2);
}



	}
 }