// NOTE - MODIFICATIONS TO INIT FILE
//; Feature=UART1 - UART 1 configuration
//	MOV #0x0033, W0                        ; UART1 baud rate generator
//	MOV W0, U1BRG
//; B7=WAKE B6=LPBACK B5=ABAUD B4=RXINV B5=BRGH B2:1=PDSEL1:0 B0=STSEL
//	MOV #0x8000, W0
//	MOV W0, U1MODE							<== changed from 8300
//; B14=JTSAGEN B13=GCP B12=GWRP B11=DEBUG B10=COE B7=FWDTEN B6=WINDIS B4=FWPSA B3:0=WDTPS3:0
//	config __CONFIG1, 0x3D1F
//

// board 1 is normal, board 2 uses IC1 for us capture
// 10-15-2009
// Changed to 40khz transducers.
// Now running IR from timer 5 interupt.
// Timer 2 is 20khz for motor drive
// Timer 3 is 40khz for sonar
// Timer 5 is 38khz IR LED drive
//
// Timer 1 is still time base.
// Timer 4 is still Sonar reply timer

#define BOARD_ID	2

#include	<p24Fxxxx.h> 
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <timer.h>         // Timer library functions
#include <spi.h>
#include <math.h>
#include <adc.h>
#include <incap.h>
#include <uart.h>
#include <outcompare.h>

#define byte unsigned char
#define word unsigned int
#define dword unsigned long

#define US_SAMPLES_QUADRENT 12
#define US_QUADRENTS 4
#define US_DEAD_TIME 4000
#define PING_LENGTH 18

//***************************************
// timer freq = 32,000,000 / pr value	*
// pr value = 32,000,000 / desired freq	*
// 16,000,000 / 1000 = 32000			*
// 16,000,000 / 38,000 = 421			*
// 16,000,000 / 24,000 = 666			*

// 16,000,000 / 56,000 = 286			*
// 16,000,000 / 40,000 = 400			*
// 16,000,000 / 38,000 = 421			*
// 16,000,000 / 36,700 = 436			*
// 16,000,000 / 36,000 = 444			*
// 16,000,000 / 33,000 = 485			*
// 16,000,000 / 30,000 = 533			*

//***************************************
#define US_RELOAD 384
#define US_DC (US_RELOAD / 2)
#define IR_RELOAD 202
#define IR_DC (IR_RELOAD / 2)
#define DRIVE_RELOAD 769
#define DRIVE_DC (DRIVE_RELOAD / 2)


#define LED1 LATEbits.LATE0
#define LED2 LATEbits.LATE1
#define LED3 LATGbits.LATG0

#define IR_LED_I_1 LATEbits.LATE5
#define IR_LED_I_2 LATEbits.LATE6


// IR = 38khz = 842
// US = 24khz = 1333

#define	STEP_PH_A LATBbits.LATB2

#define	STEP_PH_B LATBbits.LATB3

#define	STEP_PH_C LATBbits.LATB4

#define	STEP_PH_D LATBbits.LATB5

#define	US_SELECT_A LATDbits.LATD14
#define	US_SELECT_B LATDbits.LATD15
#define	US_SELECT_C LATFbits.LATF4
#define	US_SELECT_D LATFbits.LATF5


#define	L_DRIVE_R	LATFbits.LATF6
#define	L_DRIVE_F	LATFbits.LATF7

#define	R_DRIVE_R	LATGbits.LATG2
#define	R_DRIVE_F	LATGbits.LATG3

#define	R_PWM	OC1RS
#define	L_PWM	OC2RS
#define	US_PWM	OC3RS

#define	IR_DRIVE   LATDbits.LATD4

#define	IR_SENSE_1 PORTBbits.RB8
#define	IR_SENSE_2 PORTBbits.RB9
#define	IR_SENSE_3 PORTBbits.RB10
#define	IR_SENSE_4 PORTBbits.RB11
#define	IR_SENSE_5 PORTBbits.RB12
#define	IR_SENSE_6 PORTBbits.RB13
#define	IR_SENSE_7 PORTBbits.RB14
#define	IR_SENSE_8 PORTBbits.RB15

#define	AUX_IN1 PORTEbits.RE2
#define	AUX_IN2 PORTEbits.RE3
#define	AUX_IN3 PORTEbits.RE4

#define	REMOTE_ENABLE AUX_IN1

//****************************************
// Function prototypes
//****************************************
extern void VisualInitialization(void);
void left_drive(int speed);
void right_drive(int speed);
void dump_dist_array(void);
void process_USB(byte rxchar);
byte get_USB_timeout(int timeout);
void send_edges(void);
void scan_edges(void);
void check_enabled(void);

//****************************************
// Global variables
//****************************************
volatile word ping_count = 0;
volatile word distance[180];
//volatile word distance[48];
volatile byte edge_detect[8];
volatile static byte us_sel = 0;
volatile static int step_ctr = 12;
volatile static byte step_dir = 0;
volatile static byte this_us_sel = 0;
volatile static byte ping_recieved = 0;
volatile static word us_threshold = 138;
volatile static byte remote_enabled = 0;
volatile static byte last_remote_enabled = 0;
volatile static int	right_speed = 0;
volatile static int	left_speed = 0;

// Timer 1 time base interupt
void __attribute__((__interrupt__,auto_psv)) _T1Interrupt( void )
{
volatile static word itimer = 1000;
volatile static word StepTimer = 50;

volatile static byte this_state = 0;

	_T1IF = 0;
	if(--itimer == 0)
		{
			itimer = 1000;
			LED3 = !LED3;
		}
		
//	StepTimer = 0;
	if(--StepTimer == 0)
		{
			StepTimer = 75;
			this_state = 0;
			us_sel = 0;
			if(remote_enabled == 0)
				{
					STEP_PH_A = 0;
					STEP_PH_B = 0;
					STEP_PH_C = 0;
					STEP_PH_D = 0;
					US_SELECT_A = 0;
					US_SELECT_B = 0;
					US_SELECT_C = 0;
					US_SELECT_D = 0;
					return;
				}
			if(STEP_PH_A)
				{
					if(step_dir ){STEP_PH_A = 0; STEP_PH_B = 1; STEP_PH_C = 0; STEP_PH_D = 0;}
					else {STEP_PH_A = 0; STEP_PH_B = 0; STEP_PH_C = 0; STEP_PH_D = 1;}
				}
			else if(STEP_PH_B)
				{
					if(step_dir ){STEP_PH_A = 0; STEP_PH_B = 0; STEP_PH_C = 1; STEP_PH_D = 0;}
					else {STEP_PH_A = 1; STEP_PH_B = 0; STEP_PH_C = 0; STEP_PH_D = 0;}
				}
			else if(STEP_PH_C)
				{
					if(step_dir ){STEP_PH_A = 0; STEP_PH_B = 0; STEP_PH_C = 0; STEP_PH_D = 1;}
					else {STEP_PH_A = 0; STEP_PH_B = 1; STEP_PH_C = 0; STEP_PH_D = 0;}
				}
			else if(STEP_PH_D)
				{
					if(step_dir ){STEP_PH_A = 1; STEP_PH_B = 0; STEP_PH_C = 0; STEP_PH_D = 0;}
					else {STEP_PH_A = 0; STEP_PH_B = 0; STEP_PH_C = 1; STEP_PH_D = 0;}
				}
			else
				{
					STEP_PH_A = 1;
					STEP_PH_B = 0;
					STEP_PH_C = 0;
					STEP_PH_D = 0;
				}
			if(step_dir)
				{
					step_ctr++;
 					if((step_ctr >= US_SAMPLES_QUADRENT) || (_RE4 == 1))
						{
							step_dir = 0;
							StepTimer = 150;
							step_ctr = US_SAMPLES_QUADRENT;
						}
				}
			else
				{
					step_ctr--;
					if(step_ctr <= 0)
						{
							step_dir = 1;
							StepTimer = 150;
							step_ctr = 0;
						}
				}
		}
	if(remote_enabled == 0) return;
// enable one of the sonars
	this_state++;
	if(this_state == 1)
		{
			if(us_sel == 0)
				{
					US_SELECT_A = 1;
					US_SELECT_B = 0;
					US_SELECT_C = 0;
					US_SELECT_D = 0;
				}
			else if(us_sel == 1)
				{
					US_SELECT_A = 0;
					US_SELECT_B = 1;
					US_SELECT_C = 0;
					US_SELECT_D = 0;
				}
			else if(us_sel == 2)
				{
					US_SELECT_A = 0;
					US_SELECT_B = 0;
					US_SELECT_C = 1;
					US_SELECT_D = 0;
				}
			else if(us_sel == 3)
				{
					US_SELECT_A = 0;
					US_SELECT_B = 0;
					US_SELECT_C = 0;
					US_SELECT_D = 1;
				}
		}
// after 3 ms, send the pulse, start the timer
	else if(this_state == 3)
		{
			if(ping_recieved == 0)distance[((step_ctr))+(this_us_sel*US_SAMPLES_QUADRENT)] = 65535;
			this_us_sel = us_sel;
			us_sel++;
			ping_count = PING_LENGTH;		// allow 12 cycles
			OC4RS = US_DC;					// start the PWM
			TMR4 = 0;						// and reset the reply timer
			ping_recieved = 0;
//-----------			US_PWM = 400;
#if BOARD_ID == 1
			_IC2IF = 0;
			_IC2IE = 1;
			EnableIntIC2;
#elif BOARD_ID == 2
			_IC1IF = 0;
			_IC1IE = 1;
			EnableIntIC1;
#endif
		}
	else if(this_state == 5)
		{
			US_PWM = us_threshold;
		}
	else if(this_state >= 10) this_state = 10;
}

// Timer 2 US ping send interupt
void __attribute__((__interrupt__,auto_psv)) _T2Interrupt( void )
{
	_T2IF = 0;
	if(ping_count > 0)
		{
			if (--ping_count == 0) OC4RS = 0;		// stop this ping
		}
}

//--------------------------------------------------------------------------------------------

// Input Capture 2 US ping reply interupt
void __attribute__((__interrupt__,auto_psv)) _IC2Interrupt( void )
{
// REMOVED Timer 4/5 will have the number of 62ns counts since the ping was transmitted
// Timer 4 will have the number of 62ns counts since the ping was transmitted
volatile word timer;
    timer = TMR4;                        		// Copy Timer4 into timer low 16bit 
	_IC2IF = 0;
	if(timer > US_DEAD_TIME)
		{
			_IC2IE = 0;
			DisableIntIC2;
			distance[((step_ctr))+(this_us_sel*US_SAMPLES_QUADRENT)] = timer;
			ping_recieved = 1;
		}
}
//--------------------------------------------------------------------------------------------


// Timer 5 IR source interupt
void __attribute__((__interrupt__,auto_psv)) _T5Interrupt( void )
{
	_T5IF = 0;
	IR_DRIVE = !IR_DRIVE;
}

//--------------------------------------------------------------------------------------------

// Input Capture 1 US ping reply interupt
void __attribute__((__interrupt__,auto_psv)) _IC1Interrupt( void )
{
// REMOVED Timer 4/5 will have the number of 62ns counts since the ping was transmitted
// Timer 4 will have the number of 62ns counts since the ping was transmitted
volatile word timer;
    timer = TMR4;                        		// Copy Timer4 into timer low 16bit 
	_IC1IF = 0;
	if(timer > US_DEAD_TIME)
		{
			_IC1IE = 0;
			DisableIntIC2;
			distance[((step_ctr))+(this_us_sel*US_SAMPLES_QUADRENT)] = timer;
			ping_recieved = 1;
		}
}
//--------------------------------------------------------------------------------------------

int main(void)
{
dword	x;
byte	locb;

	VisualInitialization();
	AD1PCFG = 0xffff;
	ODCB = 0;
	TRISA = 0x0000;
	TRISB = 0xff00;
	TRISC = 0x0000;
#if BOARD_ID == 1
	TRISD = 0x0200;
#elif BOARD_ID == 2
	TRISD = 0x0300;
#endif
	TRISE = 0x001c;
	TRISF = 0x0104;
	TRISG = 0x0000;
	STEP_PH_A = 1;
	STEP_PH_B = 0;
	STEP_PH_C = 0;
	STEP_PH_D = 0;
	US_SELECT_A = 0;
	US_SELECT_B = 0;
	US_SELECT_C = 0;
	US_SELECT_D = 0;


	US_SELECT_A = 1;
	US_SELECT_B = 1;
	US_SELECT_C = 1;
	US_SELECT_D = 1;
//***************************************
// timer freq = 32,000,000 / pr value	*
// pr value = 32,000,000 / desired freq	*
// 16,000,000 / 1000 = 32000			*
// 16,000,000 / 38,000 = 421			*
// 16,000,000 / 24,000 = 666			*

// 16,000,000 / 56,000 = 286			*
// 16,000,000 / 40,000 = 400			*
// 16,000,000 / 38,000 = 421			*
// 16,000,000 / 36,700 = 436			*
// 16,000,000 / 36,000 = 444			*
// 16,000,000 / 33,000 = 485			*
// 16,000,000 / 30,000 = 533			*

//***************************************
	OpenTimer1(0x8000,16000);			// 1us time base	
	OpenTimer2(0x8000,DRIVE_RELOAD);	// 20khz, motor pwm drive
	OpenTimer3(0x8000,US_RELOAD);		// 40khz / Sonar source - easured 40.006khz
	OpenTimer4(0x8010,0xffff);			// Used to time ping replies (/256)
	OpenTimer5(0x8000,IR_RELOAD);		// 38 khz / IR - measured 37.994khz


#if BOARD_ID == 1
	IC2CON =0;							// input capture 2 disabled
	IC2CON =0x2002;						// input capture for ping replies (2002 falling edge) (2003 = riding edge)
#elif BOARD_ID == 2
	IC1CON =0;							// input capture 2 disabled
	IC1CON =0x2002;						// input capture for ping replies (2002 falling edge) (2003 = riding edge)
#endif
	
	OpenOC1((OC_IDLE_CON & OC_TIMER2_SRC & OC_PWM_FAULT_PIN_DISABLE), DRIVE_DC, 1);	// drive
	OpenOC2((OC_IDLE_CON & OC_TIMER2_SRC & OC_PWM_FAULT_PIN_DISABLE), DRIVE_DC, 1);	// drive
	OpenOC3((OC_IDLE_CON & OC_TIMER3_SRC & OC_PWM_FAULT_PIN_DISABLE), 333, 1);	// US comparator threshold
	OpenOC4((OC_IDLE_CON & OC_TIMER3_SRC & OC_PWM_FAULT_PIN_DISABLE), 333, 1);	// US freq source
	OC4RS = 0;
	EnableIntT1;
	EnableIntT2;
	EnableIntT5;
IPC7bits.T5IP = 7;

	US_PWM = us_threshold;
	IR_LED_I_1 = 1;
	IR_LED_I_2 = 1;

	right_drive(0);
	left_drive(0);
// initialize distance array
	for(locb=0; locb<(US_QUADRENTS * US_SAMPLES_QUADRENT); locb++)
		{
			distance[locb]=locb;
			distance[locb]*=100;
			distance[locb]=4000;
		}
			
	while(1)
		{
			LED1 = !LED1;
			for(x=0; x<75000; x++)
				{
					ClrWdt();
					check_enabled();
					if(U1STAbits.URXDA)process_USB(U1RXREG);
					scan_edges();
				}
		}
	return(0);	
}

void check_enabled(void)
{
	if(REMOTE_ENABLE)remote_enabled=0;
	else remote_enabled=1;
	LED2 = REMOTE_ENABLE;
	if(last_remote_enabled != remote_enabled)
		{
			if(remote_enabled == 0)
				{
					right_drive(0);
					left_drive(0);
				}
			else
				{
					right_drive(right_speed);
					left_drive(left_speed);
					step_ctr = 0;
					step_dir = 1;
				}
			last_remote_enabled = remote_enabled;
		}
}


//=========================================================================

void OpenTimer1(word config,word period)
{
    TMR1  = 0;          /* Reset Timer1 to 0x0000 */
    PR1   = period;     /* assigning Period to Timer period register */
    T1CON = config;     /* Configure timer control reg */
}

void OpenTimer2(word config,word period)
{
    TMR2  = 0;          /* Reset Timer2 to 0x0000 */
    PR2   = period;     /* assigning Period to Timer period register */
    T2CON = config;     /* configure control reg */
    T2CONbits.T32 = 0;
}

void OpenTimer3(word config,word period)
{
    TMR3  = 0;      /* Reset Timer3 to 0x0000 */
    PR3   = period; /* assigning Period to Timer period register.*/
    T3CON = config; /* configure timer control reg */
}

void OpenTimer4(word config,word period)
{
    TMR4  = 0;      /* Reset Timer4 to 0x0000 */
    PR4   = period; /* assigning Period to Timer period register.*/
    T4CON = config; /* configure timer control reg */
    T4CONbits.T32 = 0;
}

void OpenTimer5(word config,word period)
{
    TMR5  = 0;      /* Reset Timer5 to 0x0000 */
    PR5   = period; /* assigning Period to Timer period register.*/
    T5CON = config; /* configure timer control reg */
}

void OpenOC1(unsigned int config, unsigned int value1, unsigned int value2)
{
    OC1CONbits.OCM = 0; /* turn off OC before switching to new mode */
    OC1RS = value1;     /* assign value1 to OCxRS Secondary Register */
    OC1R = value2;      /* assign value2 to OCxR Main Register*/  
    OC1CON = config;    /* assign config to OCxCON Register*/
}

void OpenOC2(unsigned int config, unsigned int value1, unsigned int value2)
{
    OC2CONbits.OCM = 0; /* turn off OC before switching to new mode */
    OC2RS = value1;     /* assign value1 to OCxRS Secondary Register */
    OC2R = value2;      /* assign value2 to OCxR Main Register*/  
    OC2CON = config;    /* assign config to OCxCON Register*/
}

void OpenOC3(unsigned int config, unsigned int value1, unsigned int value2)
{
    OC3CONbits.OCM = 0; /* turn off OC before switching to new mode */
    OC3RS = value1;     /* assign value1 to OCxRS Secondary Register */
    OC3R = value2;      /* assign value2 to OCxR Main Register*/  
    OC3CON = config;    /* assign config to OCxCON Register*/
}

void OpenOC4(unsigned int config, unsigned int value1, unsigned int value2)
{
    OC4CONbits.OCM = 0; /* turn off OC before switching to new mode */
    OC4RS = value1;     /* assign value1 to OCxRS Secondary Register */
    OC4R = value2;      /* assign value2 to OCxR Main Register*/  
    OC4CON = config;    /* assign config to OCxCON Register*/
}

void OpenOC5(unsigned int config, unsigned int value1, unsigned int value2)
{
    OC5CONbits.OCM = 0; /* turn off OC before switching to new mode */
    OC5RS = value1;     /* assign value1 to OCxRS Secondary Register */
    OC5R = value2;      /* assign value2 to OCxR Main Register*/  
    OC5CON = config;    /* assign config to OCxCON Register*/
}

void OpenTimer45(word config,dword period)
{
    TMR4 = 0;               /* Reset Timer4 to 0x0000 */
    TMR5 = 0;               /* Reset Timer5 to 0x0000 */
    PR4 = period;           /* assigning Period to Timer period register */
    PR5 = period>>16;       /* Period to PR4 and PR5 Register */
    T4CON = config;         /* configure timer control reg */
    T4CONbits.T32 = 1;
}

void left_drive(int speed)
{
	L_PWM = 0;
	while(_RD0 == 1)ClrWdt();
	if(speed > 0)
		{
			L_DRIVE_R = 0;
			if(L_DRIVE_F == 0)L_DRIVE_F = 1;
		}
	else if(speed < 0)
		{
			L_DRIVE_F = 0;
			if(L_DRIVE_R == 0)L_DRIVE_R = 1;
		}
	else
		{
			L_DRIVE_F = 0;
			L_DRIVE_R = 0;
		}
	if(speed != 0)L_PWM = (((abs(speed)*26) / 10)+400);
	else L_PWM = 0;
}	

void right_drive(int speed)
{
	R_PWM = 0;
	while(_RD1 == 1)ClrWdt();
	if(speed > 0)
		{
			R_DRIVE_R = 0;
			if(R_DRIVE_F == 0)R_DRIVE_F = 1;
		}
	else if(speed < 0)
		{
			R_DRIVE_F = 0;
			if(R_DRIVE_R == 0)R_DRIVE_R = 1;
		}
	else
		{
			R_DRIVE_F = 0;
			R_DRIVE_R = 0;
		}
	if(speed != 0)R_PWM = (((abs(speed)*26) / 10)+400);
	else R_PWM = 0;
}	
//*********************************************************

void dump_dist_array(void)
{
	int	x;
	while(U1STAbits.TRMT == 0)ClrWdt();
	U1TXREG = 0x1b;
	for(x=0; x<(US_SAMPLES_QUADRENT * US_QUADRENTS); x++)
		{
			while(U1STAbits.TRMT == 0)ClrWdt();
			U1TXREG = (distance[x] >> 8);
			while(U1STAbits.TRMT == 0)ClrWdt();
			U1TXREG = (distance[x] & 0xff);
		}
}
//*********************************************************

void process_USB(byte rxchar)
{
byte	locb;
int		loci;
	locb = rxchar;
	if(locb == 'D')dump_dist_array();

	else if(locb == 'E')send_edges();

	else if(locb == 'L')
		{
			loci=get_USB_timeout(10000);
			loci <<= 8;
			loci|=get_USB_timeout(10000);
			left_speed = loci; 
			if(remote_enabled != 0)left_drive(loci);
		}

	else if(locb == 'R')
		{
			loci=get_USB_timeout(10000);
			loci <<= 8;
			loci|=get_USB_timeout(10000);
			right_speed = loci; 
			if(remote_enabled != 0)right_drive(loci);
		}

	else if(locb == 'T')
		{
			loci=get_USB_timeout(10000);
			loci <<= 8;
			loci|=get_USB_timeout(10000);

//	OpenTimer3(0x8000,loci*4);	// 24 Khz / US


			us_threshold = loci;
		}

	else U1TXREG = locb;
}
//*********************************************************
byte get_USB_timeout(int timeout)
{
int	locx;
	for(locx=0; locx<timeout; locx++) 
		{
			if(U1STAbits.URXDA)	return(U1RXREG);
			ClrWdt();
		}
	return(0);
}
//*********************************************************
void send_edges(void)
{
	int	x;
	while(U1STAbits.TRMT == 0)ClrWdt();
	U1TXREG = 0x1b;
	for(x=0; x<8; x++)
		{
			while(U1STAbits.TRMT == 0)ClrWdt();
			U1TXREG = (edge_detect[x]);
		}
}
//*********************************************************

void scan_edges(void)
{
	if(IR_SENSE_1)edge_detect[0]=1; else edge_detect[0]=0;
	if(IR_SENSE_2)edge_detect[1]=1; else edge_detect[1]=0;
	if(IR_SENSE_3)edge_detect[2]=1; else edge_detect[2]=0;
	if(IR_SENSE_4)edge_detect[3]=1; else edge_detect[3]=0;
	if(IR_SENSE_5)edge_detect[4]=1; else edge_detect[4]=0;
	if(IR_SENSE_6)edge_detect[5]=1; else edge_detect[5]=0;
	if(IR_SENSE_7)edge_detect[6]=1; else edge_detect[6]=0;
	if(IR_SENSE_8)edge_detect[7]=1; else edge_detect[7]=0;

/*	edge_detect[0]=IR_SENSE_1;
	edge_detect[1]=IR_SENSE_2;
	edge_detect[2]=IR_SENSE_3;
	edge_detect[3]=IR_SENSE_4;
	edge_detect[4]=IR_SENSE_5;
	edge_detect[5]=IR_SENSE_6;
	edge_detect[6]=IR_SENSE_7;
	edge_detect[7]=IR_SENSE_8;
*/	
}