#include <pic18fregs.h>

#define PROP_CONTROL
#define INTEGRAL_CONTROL
#define DIFF_CONTROL
#define ULTRASOUND_CONTROL

#define X_AXIS
#define Y_AXIS

//#define XPROP 0.0012
//#define YPROP 0.0012

/*

#define XPROP 0.003
#define YPROP 0.003

#define MAX_PROP 13333

#define XINTEGRAL 0.0001
#define YINTEGRAL 0.0001

#define XDIFF 0.0011
#define YDIFF 0.0011
*/

#define XPROP 0.0002
#define YPROP 0.0002

#define MAX_PROP 13333

#define XINTEGRAL 0.00001
#define YINTEGRAL 0.00001

#define XDIFF 0.0007
#define YDIFF 0.0007



#define ULTRASOUND_TARGET 13
#define ULTRASOUND_P 1
#define ULTRASOUND_ERROR_MAX 13


// minimum for speed controller is about 4% 
#define INIT_SPEED 200
#define ABSOLUTE_MIN 215
#define ABSOLUTE_MAX 625

//#define XTARGET -150
#define XTARGET -150
#define YTARGET -1750


//363
#define IDLE_THROTTLE 355

#define MOTOR1_OFFSET 0
#define MOTOR2_OFFSET 0
#define MOTOR3_OFFSET 0
#define MOTOR4_OFFSET 0

#define MOTOR1_IDLE (IDLE_THROTTLE + MOTOR1_OFFSET)
#define MOTOR2_IDLE (IDLE_THROTTLE + MOTOR2_OFFSET)
#define MOTOR3_IDLE (IDLE_THROTTLE + MOTOR3_OFFSET)
#define MOTOR4_IDLE (IDLE_THROTTLE + MOTOR4_OFFSET)

//#define SPEAKER_ENABLE
#ifdef SPEAKER_ENABLE
#define SPK_ON	T2CON|=0x04;
#define SPK_OFF	T2CON&=(-0x04);
#define SPK_TOG	T2CON^=0x04;
#endif

code char __at __CONFIG1L conf1l = 0x30;
code char __at __CONFIG1H conf1h = 0x02;
code char __at __CONFIG2L conf2l = 0x3F;
code char __at __CONFIG2H conf2h = 0x1E;
code char __at __CONFIG3H conf3h = 0x81;
code char __at __CONFIG4L conf4l = 0x81;
code char __at __CONFIG5L conf5l = 0x0F;
code char __at __CONFIG5H conf5h = 0xC8;
code char __at __CONFIG6L conf6l = 0x0F;
code char __at __CONFIG6H conf6h = 0xE0;
code char __at __CONFIG7L conf7h = 0x0F;

int MOTOR_DELTA;
unsigned int TEMP_UNSIGNED;
int XACCEL;
int YACCEL;
int LAST_XERROR;
int LAST_YERROR;
int INTEGRAL_YACCEL;
int INTEGRAL_XACCEL;

int XERROR;
int YERROR;

unsigned char ULTRASOUND;
int ULTRASOUND_ERROR;

int MOTOR1;
int MOTOR2;
int MOTOR3;
int MOTOR4;

unsigned char XPROP_P_FLAG;
unsigned char XPROP_N_FLAG;

unsigned char INIT_NUM;

// these are changed inside and interupt and therefore must be declared volatile
volatile unsigned char ESTOP;
volatile unsigned char FLAG_NUM;
volatile unsigned char FLAG_NUM2;

int i;

void Delay_ms(long ms);
void Delay_4us(long us);

/*
* PORTA
*	RA0: X accel input
*	RA1: Y accel input
*	RA2: Ultrasound input
*	RA3: ESTOP LED output
*
* PORTB
*	RB0: Motor1 output
*	RB1: Motor2 output
*	RB2: Motor3 output
*	RB3: Motor4 output
*	RB4: R/C kill switch input
*
*
*/




//void high_isr(void) shadowregs interrupt;

void main(void) {
	TRISA = 0x07;				// set up RA0/AN0, RA1/AN1, and RA2/AN2 as inputs
	
	TRISB = 0x10;				// set all of PORTB to be outputs, except for RB4 which reads the PWM kill switch input
	
#ifdef SPEAKER_ENABLE
	PORTC = 0x00;
	TRISC = 0x00;
	PR2 = 0x80;
	CCPR1L = 0x40;
	CCP1CON = 0x0c;
	T2CON = 0x03;
#endif
	
	
	ADCON0 = 0x01;				// select AN0, enable A/D module
	ADCON1 = 0x0C;				// set up RA0/AN0, RA1/AN1, and RA2/AN2 as an analog input and set up RA3..RA5 as digital I/Os
	ADCON2 = 0x2E;				// configure A/D module result to left justified, acquisition time to 32 us, and clock period to 2.67 us
	
	INTCON = 0xC8;				// Enable interupts and enable PORTB interupts
	INTCON2 = 0xD1;				// INTEDG0 interupts on a PORTB rising edge, INTEDG1 interupts on a falling egde, interupt
								// is high priority
	T0CON = 0x88;       		// initialize Timer0 with no prescaler
	
	// Disable interrupts for init
	INTCONbits.RBIE = 0;
	
	ESTOP = 0;	
	
	XERROR = 0;
	YERROR = 0;
	
	LAST_XERROR = 0;
	INTEGRAL_XACCEL = 0;
	
	LAST_YERROR = 0;
	INTEGRAL_YACCEL = 0;
	
	XPROP_P_FLAG = 0;
	XPROP_N_FLAG = 0;
	
	FLAG_NUM = 0;
	FLAG_NUM2 = 3;
	
	INIT_NUM = 0;
	
	ULTRASOUND = 0;
	ULTRASOUND_ERROR = 0;
	
	/*
	while (1)
	{
		// altitude: do A to D conversion on the ultrasound sensor
		ADCON0 = 0x0B;
		while (ADCON0bits.GO);
		
		// ULTRASOUND is an unsigned character
		ULTRASOUND = ADRESH;
		
		if (ULTRASOUND > 25)
		{
			PORTAbits.RA3 = 0;
		} else
		{
			PORTAbits.RA3 = 1;
		}
	}
	*/
	
	// controller init
	for (i=0;i<500;i++)
	{
		
		//send minimum throttle

#ifdef X_AXIS
		// pulse for motor1
		PORTBbits.RB0 = 1;
		Delay_4us(INIT_SPEED);
		PORTBbits.RB0 = 0;
		

		// pulse for motor2
		PORTBbits.RB1 = 1;
		Delay_4us(INIT_SPEED);
		PORTBbits.RB1 = 0;
#endif
		
#ifdef Y_AXIS
		// pulse for motor3
		PORTBbits.RB2 = 1;
		Delay_4us(INIT_SPEED);
		PORTBbits.RB2 = 0;
		
		// pulse for motor4
		PORTBbits.RB3 = 1;
		Delay_4us(INIT_SPEED);
		PORTBbits.RB3 = 0;
#endif
		//wait for the next pulse
		
		Delay_ms(11);
		
		//i = 501;
	}
	
	//ok, PIC and controller init is done
	
	
	while (1)
	{
		//now start up PID
		
		//first, do A to D conversion of the X axis (AN0)
		
		ADCON0 = 0x03;				// select AN0 and set GO_DONE bit to start A/D conversion
		while (ADCON0bits.GO);		// do nothing until the A/D conversion is done
		
		TEMP_UNSIGNED = ADRESH;
		TEMP_UNSIGNED = TEMP_UNSIGNED * 256;
		TEMP_UNSIGNED += ADRESL;
		
		XACCEL = TEMP_UNSIGNED - 32768;
		
		
		// A to D conversion on the Y axis
		ADCON0 = 0x07;
		while (ADCON0bits.GO);
		
		TEMP_UNSIGNED = ADRESH;
		TEMP_UNSIGNED = TEMP_UNSIGNED * 256;
		TEMP_UNSIGNED + ADRESL;
		
		YACCEL = TEMP_UNSIGNED - 32768;
		
		// altitude: do A to D conversion on the ultrasound sensor
		ADCON0 = 0x0B;
		while (ADCON0bits.GO);
		
		// ULTRASOUND is an unsigned character
		ULTRASOUND = ADRESH;

		MOTOR1 = MOTOR1_IDLE;
		MOTOR2 = MOTOR2_IDLE;
		
		MOTOR3 = MOTOR3_IDLE;
		MOTOR4 = MOTOR4_IDLE;
		
		//proprotional control
#ifdef PROP_CONTROL
		XERROR = XACCEL - XTARGET;
		
		if (XERROR > MAX_PROP)
		{
			XERROR = MAX_PROP;
		} else if (XERROR < -MAX_PROP)
		{
			XERROR = -MAX_PROP;
		}
		
		// If XERROR > 0, we are rolling right
		
		MOTOR1 += XPROP * XERROR;
		MOTOR2 -= XPROP * XERROR;
		
		YERROR = YACCEL - YTARGET;
		
		// If YERROR > 0, we are pitching down
		
		MOTOR3 += YPROP * YERROR;
		MOTOR4 -= YPROP * YERROR;
#endif

		
		// integral control
#ifdef INTEGRAL_CONTROL
		INTEGRAL_XACCEL += XTARGET - XACCEL;
		INTEGRAL_YACCEL += YTARGET - YACCEL;
		
		MOTOR1 += XINTEGRAL * INTEGRAL_XACCEL;
		MOTOR2 -= XINTEGRAL * INTEGRAL_XACCEL;
		
		MOTOR3 += YINTEGRAL * INTEGRAL_YACCEL;
		MOTOR4 -= YINTEGRAL * INTEGRAL_YACCEL;
#endif
		
		// differential control
#ifdef DIFF_CONTROL
		
		// MOTOR_DELTA is the change in the error of the accelerometer
		
		MOTOR_DELTA = (XACCEL - XTARGET) - LAST_XERROR;
		
		MOTOR1 += XDIFF * MOTOR_DELTA;
		MOTOR2 -= XDIFF * MOTOR_DELTA;
		
		MOTOR_DELTA = (YACCEL - YTARGET) - LAST_YERROR;
		
		MOTOR3 += YDIFF * MOTOR_DELTA;
		MOTOR4 -= YDIFF * MOTOR_DELTA;
		
		
		LAST_XERROR = XACCEL - XTARGET;
		LAST_YERROR = YACCEL - YTARGET;
#endif
		
#ifdef ULTRASOUND_CONTROL
			
		ULTRASOUND_ERROR = ULTRASOUND - ULTRASOUND_TARGET;
		// Altitude PID control
		
		if (ULTRASOUND_ERROR > ULTRASOUND_ERROR_MAX)
		{
			ULTRASOUND_ERROR = ULTRASOUND_ERROR_MAX;
		}
		
		// Check to make sure that the craft is not tilted too much making the
		// altitude value unreliable
		if (XACCEL > 8000 || XACCEL < -8000 || YACCEL > 8000 || YACCEL < -8000)
		{
			// unreliable
			PORTAbits.RA3 = 1;
		} else
		{		
			MOTOR1 -= ULTRASOUND_P * ULTRASOUND_ERROR;
			MOTOR2 -= ULTRASOUND_P * ULTRASOUND_ERROR;
			MOTOR3 -= ULTRASOUND_P * ULTRASOUND_ERROR;
			MOTOR4 -= ULTRASOUND_P * ULTRASOUND_ERROR;
			
			PORTAbits.RA3 = 0;
		}
#endif
		// -------------------------- DO NOT SET MOTOR[1-4] BELOW THIS LINE --------------------------
		
		
		// PID is done.  Now convert those values into a signal that the speed controller can recieves
		
		// RB0 -- Motor 1
		// RB1 -- Motor 2
		// RB2 -- Motor 3
		// RB3 -- Motor 4
		
		// bounds checking
		if (MOTOR1 > ABSOLUTE_MAX)
		{
			MOTOR1 = ABSOLUTE_MAX;
		} else if (MOTOR1 < ABSOLUTE_MIN)
		{
			MOTOR1 = ABSOLUTE_MIN;
		}
		
		if (MOTOR2 > ABSOLUTE_MAX)
		{
			MOTOR2 = ABSOLUTE_MAX;
		} else if (MOTOR2 < ABSOLUTE_MIN)
		{
			MOTOR2 = ABSOLUTE_MIN;
		}
		
		if (MOTOR3 > ABSOLUTE_MAX)
		{
			MOTOR3 = ABSOLUTE_MAX;
		} else if (MOTOR3 < ABSOLUTE_MIN)
		{
			MOTOR3 = ABSOLUTE_MIN;
		}
		
		if (MOTOR4 > ABSOLUTE_MAX)
		{
			MOTOR4 = ABSOLUTE_MAX;
		} else if (MOTOR4 < ABSOLUTE_MIN)
		{
			MOTOR4 = ABSOLUTE_MIN;
		}
		
		// Check for emergency stop
		if (ESTOP != 0)
		{
			MOTOR1 = INIT_SPEED;
			MOTOR2 = INIT_SPEED;
			MOTOR3 = INIT_SPEED;
			MOTOR4 = INIT_SPEED;
			
		}
		
		// Protect the critical delays from interrupts
		INTCONbits.RBIE = 0;
		
#ifdef X_AXIS
		
		// pulse for motor1
		PORTBbits.RB0 = 1;
		Delay_4us(MOTOR1);
		PORTBbits.RB0 = 0;
		
		// pulse for motor2
		PORTBbits.RB1 = 1;
		Delay_4us(MOTOR2);
		PORTBbits.RB1 = 0;
#endif
		
#ifdef Y_AXIS
		
		// pulse for motor3
		PORTBbits.RB2 = 1;
		Delay_4us(MOTOR3);
		PORTBbits.RB2 = 0;
		
		// pulse for motor4
		PORTBbits.RB3 = 1;
		Delay_4us(MOTOR4);
		PORTBbits.RB3 = 0;
			
		if (INIT_NUM > 15)
		{
			// reenable PORTB interrupts
			INTCONbits.RBIE = 1;
		} else
		{
			INIT_NUM ++;
		}
		
		//wait for the next pulse
		Delay_ms(13);
#endif
	
	}
	
}

void isr_high (void) __interrupt(1)
{
	unsigned char LOWER;
	unsigned char HIGHER;
	
	//check to see if it went high or low

	if (PORTBbits.RB4 == 0)
	{
		//we just went low
		//check the timer
		LOWER = TMR0L;
		HIGHER = TMR0H;
		
		if (HIGHER > 40 && HIGHER < 48)
		{
			if (FLAG_NUM >= 10)
			{
				// SHUTDOWN CONDITION
				ESTOP = 1;
			} else
			{
				FLAG_NUM ++;
			}
		} else
		{
			if (FLAG_NUM2 <= 0)
			{
				FLAG_NUM --;
				FLAG_NUM2 = 1;
			} else
			{
				FLAG_NUM2 --;
			}
		}
		
		TMR0L = 0x00;
		TMR0H = 0x00;
		
	} else
	{
		//we just went high
		
		//set the timer
		TMR0L = 0x00;
		TMR0H = 0x00;
	}
	
	//clear the PORTB interrupt bit
	INTCON = 0xC8;
	
}

void Delay_ms(long ms)
{
	long i;
	
	while (ms--)
		for (i=0; i < 398; i++)
			;
}

void Delay_4us(long us)
{
	while (us--)
		;
}