#include <system.h>
#include "types.h"

#pragma CLOCK_FREQ 48000000


#define FRAC(H) ((int)((H)*16))

// using a 10 Mhz external crystal                                                                                    
#pragma DATA   _CONFIG1H, 00100110b  // HS Osc. with 4xPLL on                                                                                      
#pragma DATA   _CONFIG2L, 00000001b  // BO reset off, POT Off                                                                                    
#pragma DATA   _CONFIG2H, 00000000b  // Watchdog off                                                                                     
#pragma DATA   _CONFIG3H, 00000001b  // CCP2 Mux with 0-RB3 or 1-RC1                                                                                        
#pragma DATA   _CONFIG4L, 10000000b  // Debugger off, LVP Off, No Stack Ovf reset 
#pragma DATA   _CONFIG5L, 00001111b  // No Code Protection                                                                                  
#pragma DATA   _CONFIG5H, 11000000b  // No EEPROM or boot block protection                                                                                   
#pragma DATA   _CONFIG6L, 00001111b  // No Write Protection                                                                                  
#pragma DATA   _CONFIG6H, 11100000b  // No EEPROM, Boot or config write protection                                                                                           
#pragma DATA   _CONFIG7L, 00001111b  // No Table Read Protection                                                                               
#pragma DATA   _CONFIG7H, 01000000b  // No Boot Block Table Read Protection


//Define some values for sampling different A2D channels
#define chan0	0b10000001		//select channel AN0 on PA0
#define chan1	0b10001001		//select channel AN1 on PA1
#define chan2	0b10010001		//select channel AN2 on PA2
#define chan3	0b10011001		//select channel AN3 on PA3
#define chan4	0b10100001		//select channel AN4 on PA5
#define chan5	0b10101001		//select channel AN5 on PE0
#define chan6	0b10110001		//select channel AN6 on PE1
#define chan7	0b10111001		//select channel AN8 on PE2

#define	GYRO		chan0
#define	Y_SLOW		chan1
#define	X_FAST		chan2
#define	X_SLOW		chan3
#define	Y_FAST		chan4
#define MOT1TORQUE	chan5
#define MOT2TORQUE	chan6
#define BAT_VOLT	chan7

#define FORWARD		0x00
#define BACKWARD	0x01

#define motor1speed ccpr1l
#define motor2speed ccpr2l

//Declare the Pin assignments
//Define PortA: Analog inputs from the sensors
//#define	GRYO		_porta,0		//Input
//#define	GYRO_TMP	_porta,1		//Input
//#define	Y_FAST		_porta,2		//Input
//#define	X_SLOW		_porta,3		//Input
//#define	X_SLOW	_porta,4		//Input
//#define	Y_FAST	_porta,5		//Input

//Define PortB: SPI and programming pins
#define	SPI_CS		_portb,0		//Input
#define	SPI_CLOCK	_portb,1		//Input
#define	SPI_MOSI	_portb,2		//Input
#define	SPI_MISO	_portb,3		//Output
//#define OPEN<---		_portb,4	//
//#define OPEN<---		_portb,5	//
//#define programming-clock	_portb,6//Input
//#define programming-Data 	_portb,7//Input

//Define PortC: Motor Control and UART
#define MOT1DIR		_portc,0		//Output
#define MOT1PWM		_portc,1		//Output
#define MOT2PWM		_portc,2		//Output
#define MOT1BRK		_portc,3		//Output
#define MOT2DIR		_portc,4		//Output
#define MOT2BRK		_portc,5		//Output
#define RS232_TX	_portc,6		//Output
#define RS232_RX	_portc,7		//Output

//Define PortD: Status Port
#define GYRO_ST		_portd,0		//Output
#define ACCL_ST2  	_portd,1		//Output
#define ACCL_ST1	_portd,2		//Output
//#define OPEN		_portd,3		//Output
#define L_CS		_portd,4		//Output
#define L_DAT		_portc,5		//Output
#define L_CLK		_portc,6		//Output
//#define OPEN<---	_portc,7		//Output

//Define PortE: Motor Status
//#define MOT1TORQUE	_porte,0	//Input
//#define MOT2TORQUE	_porte,1	//Input
//#define OPEN<---		_porte,2		//


//#define DEBUG_MODE_VISUAL	1

#define GYROOFFSET 	7350			//FRAC(460)	//in counts/deg/sec*16

#define ACCEL_ONE_G	FRAC(63)

// Global variables
BYTE sample = 0;
WORD gyroSamples[8];
WORD accelXSamples[8];

//New Variables
#define GYRO_OFFSET_SAMPLES ((unsigned long)1)
#define GYRO_ANGLE_WASHOUT 	10

#define ACCEL_OFFSET	512*8

volatile unsigned long ulGyroOffset;
volatile signed long slGyroSpeed = 0;
volatile signed long slGyroAngle = 0;
volatile signed long slPosition = 0;

volatile unsigned int uiTI = GYRO_OFFSET_SAMPLES;

char run = 0;

signed long GYROOFFSET_var = FRAC(460);


signed long i = 80;	
signed long output = 0;
signed long gyroRate = 0;
signed long accelXRate = FRAC(512);
signed long accelXRateSlow = FRAC(512);
signed long accelYRate = FRAC(512);
signed long accelYRateSlow = FRAC(0);
signed long lAccelAngle = 0;
signed long lAngleError = 460;

signed long accelAverage = 0;

#define	TIMER_RELOAD_HIGH	0xB6	//0x48
#define	TIMER_RELOAD_LOW	0xC2	//0xE4


//Prototypes
WORD sampleA2D ( char whichChannel );

char isr_variable = 0;
unsigned int uiAdcAvg = 0;
unsigned int uiAdcAvgAccel = 0;

void interrupt ( void )
{
	clear_bit( intcon, GIE );  	//Disable interrupts
	
	if( test_bit( intcon, TMR0IF ) )
	{
		//Reload the counter
		tmr0h = TIMER_RELOAD_HIGH;
		tmr0l = TIMER_RELOAD_LOW;
		
		//Sample the sensors
		//gyroRate = sampleA2D ( GYRO );
		accelXRate = sampleA2D ( X_FAST );
		accelXRateSlow = sampleA2D ( X_SLOW );
		accelYRate = sampleA2D ( Y_FAST );
		accelYRateSlow = 0;//sampleA2D ( Y_SLOW );
		
		uiAdcAvg = 0;
		uiAdcAvgAccel = 0;
		for(isr_variable=0;isr_variable<8;isr_variable++)
		{
			uiAdcAvg += sampleA2D ( GYRO );
			uiAdcAvgAccel += sampleA2D ( Y_FAST );			
		}
		
		if(uiTI)
		{
			uiTI--;
			ulGyroOffset += (unsigned long) uiAdcAvg;
			slGyroSpeed = 0;
			slGyroAngle = 0;
		}
		else
		{	
//			slGyroSpeed = (signed long)(ulGyroOffset/GYRO_OFFSET_SAMPLES);
			slGyroSpeed = (signed long)(ulGyroOffset);
			ulGyroOffset -= slGyroSpeed;
			ulGyroOffset += (unsigned long) uiAdcAvg;
			slGyroSpeed -= (signed long)uiAdcAvg;
			
			slGyroAngle += slGyroSpeed;	
			
/*			if(slGyroAngle > 0)
				slGyroAngle -= GYRO_ANGLE_WASHOUT;
			else if(slGyroAngle < 0)
				slGyroAngle += GYRO_ANGLE_WASHOUT;*/
				
			accelYRateSlow = (signed long)uiAdcAvgAccel;
			
		}		
		run = 1;
	
		clear_bit( intcon, TMR0IF );		//Clear the interup flag
 	}

    set_bit( intcon, GIE );     	//Enable interrupts
}


void init()
{
	porta = 0b00000000;		//PORTA 0x00
	portb = 0b00000000;		//PORTB 0x00
	portc = 0b00000000;		//PORTC 0x00
	portd = 0b00000000;		//PORTD 0x00
	porte = 0b00000000;		//PORTE 0x00
	
	trisa = 0b00011111;		//PORTA: IN: All 				OUT: None
	trisb = 0b11110111;		//PORTB: IN: CS,CLK,MOSI  		OUT: MISO
	trisc = 0b00000000;		//PORTC: IN: None 				OUT: All
	trisd = 0b00000000;		//PORTD: IN: None				OUT: GYRO_ST,ACCL_ST2, ACCL_ST1, L_CS, L_DAT, L_CLK
	trise = 0b00000111;		//PORTE: IN: All 				OUT: None


	//Setup the A2D for analog on all PortA and PortE
	adcon0 = 0b10000010;	//Setup A2D on all portA pins and set osc. bits
	adcon1 = 0b11000000;	//Setup A2D right justified
	trisa = 0b11111111;		//PORTA all inputs
	trise = 0b00000111;		//PORTE all inputs


	//Setup PWM1 and PWM2
	trisc &= ~(0b00000110);	//Set CPP1 and CPP2 to outputs
	ccp1con = 0b0001111;	//Set ccp1 to PWM1 and enable
	ccpr1l = 0x00;			//Set default duty cycle to 0xXX/0xFF
	ccp2con = 0b0001111;	//Set ccp2 to PWM2 and enable
	ccpr2l = 0x00;			//Set default duty cycle to 0xXX/0xFF	 
	pr2 = 0xFF;				//Set period of PWM to max
	t2con = 0b00000111;		//Set Prescaler to 16 and enable TMR2	
	
	//Setup timer0 to have 1ms interrupt
	tmr0h = TIMER_RELOAD_HIGH;
	tmr0l = TIMER_RELOAD_LOW;
	t0con = 0b10000100;		//Set Prescaler to 256 and enable TMR0
	
	//Setup the UART for XX baud
	trisc |= (0b10000000);	//Set RX to input
	trisc &= ~(0b01000000);	//Set TX to output	
	rcsta = 0b10010000;		//Setup the RXTSA register for RS232
	txsta = 0b00100000;		//Setup the TXSTA register for RS232
	spbrg = 12;				//Setup the baud rate generator for 57.6kHz at 48MHz (12MHz XT)
//	spbrg = 10;				//Setup the baud rate generator for 57.6kHz at 40MHz (10MHz XT)	

	set_bit(intcon, TMR0IE);		//Enable the timer interupt
	clear_bit(intcon, TMR0IF);	//Clear the interrupt flag
	set_bit(intcon, GIE);	//Enable interrupts	
	set_bit(intcon, PEIE);	//Enable interrupts	
}


//----------------------------------------------------------
// Transmit a byte out the UART
// Returns on sucsessful completion
void putCharUart ( char outGoing )
{
	txreg = outGoing;
	while( !test_bit( txsta,TRMT ) ){};
}

//----------------------------------------------------------
// Set motor 1 speed and direction
void setMotor1 ( char direction, char speed )
{
	motor1speed = speed;
	if( direction == FORWARD )
	{
		_asm{
			bsf MOT1DIR;
			bcf MOT1BRK;
		}	
	}
	else
	{
		_asm{
			bcf MOT1DIR
			bcf MOT1BRK
		}	
	}
}

//----------------------------------------------------------
// Set motor 2 speed and direction
void setMotor2 ( char direction, char speed )
{
	motor2speed = speed;
	if( direction == FORWARD )
	{
		_asm{
			bsf MOT2DIR;
			bcf MOT2BRK;
		}	
	}
	else
	{
		_asm{
			bcf MOT2DIR
			bcf MOT2BRK
		}	
	}
}


//----------------------------------------------------------
// Sample the A2D on channel whichChannel
WORD sampleA2D ( char whichChannel )
{
	adcon0 = whichChannel;
	delay_us (50);
	set_bit( adcon0, GO );
    while ( test_bit( adcon0,GO ));      //Wait for conversion to complete
    return ( (adresh << 8) + adresl );
}

//----------------------------------------------------------
// 
void displayWORD ( WORD value )
{
	WORD tempValue = value;
	BYTE NumH = ((tempValue>>8)&0x00FF);
	BYTE NumL = (tempValue&0x00FF);
	char TenK = 0;
	char Thou = 0;
	char Hund = 0;
	char Tens = 0;
	char Ones = 0;	
	_asm
	{
			swapf   _NumH, W
			iorlw	0xF0     //b'11110000'
			movwf   _Thou
			addwf   _Thou,F
			addlw   0xE2
			movwf   _Hund
			addlw   0x32
			movwf   _Ones
	
			movf    _NumH,W
			andlw   0x0F
			addwf   _Hund,F
			addwf   _Hund,F
			addwf   _Ones,F
			addlw   0xE9
			movwf   _Tens
			addwf   _Tens,F
			addwf   _Tens,F
	
			swapf   _NumL,W
			andlw   0x0F
			addwf   _Tens,F
			addwf   _Ones,F
	
			rlcf    _Tens,F
			rlcf    _Ones,F
			comf    _Ones,F
			rlcf    _Ones,F
	
			movf    _NumL,W
			andlw   0x0F
			addwf   _Ones,F
			rlcf    _Thou,F
	
			movlw   0x07
			movwf   _TenK

			movlw   0x0A                             ; Ten
		Lb1:
			decf    _Tens,F
			addwf  _Ones,F
			btfss   _status, 0
			goto    Lb1
		Lb2:
			decf    _Hund,F
			addwf  _Tens,F
			btfss   _status, 0
			goto    Lb2
		Lb3:
			decf    _Thou,F
			addwf  _Hund,F
			btfss   _status, 0
			goto    Lb3
		Lb4:
			decf    _TenK,F
			addwf  _Thou,F
			btfss   _status, 0
			goto    Lb4
        }
        if( TenK == 0 )
        {
			putCharUart( ' ' );
		}
		else
		{
			putCharUart(0x30+TenK);
		}
        putCharUart(0x30+Thou);
        putCharUart(0x30+Hund);
        putCharUart(0x30+Tens);
        putCharUart(0x30+Ones);	       
}

void showData ()
{
	WORD tempSample = 0;
	putCharUart ( 0x0C ); 
	
	putCharUart ( 'G' ); 
	putCharUart ( 'o' ); 
	displayWORD ( ulGyroOffset );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );
	
	putCharUart ( 'G' ); 
	putCharUart ( 's' ); 
	displayWORD ( slGyroSpeed );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );
	
	putCharUart ( 'G' ); 
	putCharUart ( 'a' ); 
	displayWORD ( slGyroAngle );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );
				
/*	putCharUart ( 'O' ); 
	putCharUart ( ' ' ); 
	displayWORD ( output );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );
	 
	putCharUart ( 'Y' ); 
	putCharUart ( 's' ); 
	putCharUart ( ':' ); 
	putCharUart ( ' ' ); 
	displayWORD ( accelYRateSlow );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' ); */
	
/*	putCharUart ( 'Y' ); 
	putCharUart ( 'f' ); 
	putCharUart ( ':' ); 
	putCharUart ( ' ' ); 
	displayWORD ( accelYRate );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );
	 
	putCharUart ( 'X' ); 
	putCharUart ( 's' ); 
	putCharUart ( ':' ); 
	putCharUart ( ' ' ); 
	displayWORD ( accelXRateSlow );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' ); 
	
	putCharUart ( 'X' ); 
	putCharUart ( 'f' ); 
	putCharUart ( ':' ); 
	putCharUart ( ' ' ); 
	displayWORD ( accelXRate );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );*/
	 
/*	putCharUart ( 'M' ); 
	putCharUart ( '1' ); 
	putCharUart ( ':' ); 
	putCharUart ( ' ' ); 
	putCharUart ( ' ' ); 
	putCharUart ( ' ' ); 
	tempSample = sampleA2D ( MOT1TORQUE );
	displayWORD ( tempSample );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );
	 
	putCharUart ( 'M' ); 
	putCharUart ( '2' ); 
	putCharUart ( ':' ); 
	putCharUart ( ' ' ); 
	putCharUart ( ' ' ); 
	putCharUart ( ' ' ); 
	tempSample = sampleA2D ( MOT2TORQUE );
	displayWORD ( tempSample );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );*/
	 
/*	putCharUart ( 'B' ); 
	putCharUart ( 'a' ); 
	putCharUart ( 't' ); 
	putCharUart ( ':' ); 
	putCharUart ( ' ' ); 
	putCharUart ( ' ' ); 
	tempSample = 100;	//sampleA2D ( BAT_VOLT );
	displayWORD ( tempSample );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );
	putCharUart ( '\r' ); 
	putCharUart ( '\n' );	 */
}
void sendInt( signed long value )
{
	putCharUart ( (value >> 24) & 0xFF );	//1000 MSB
	putCharUart ( (value >> 16) & 0xFF );	//0100
	putCharUart ( (value >>  8) & 0xFF);	//0010
	putCharUart ( (value >>  0) & 0xFF );	//0001 LSB
}

void sendDebugMessage( void )
{
	putCharUart ( 180 );
	putCharUart ( 181 );
					
	sendInt ( gyroRate );
	sendInt ( accelAverage /*accelXRateSlow*/ );
	sendInt ( accelYRateSlow );
	sendInt ( ulGyroOffset );
	sendInt ( slGyroSpeed );
	sendInt ( slGyroAngle );
	//sendInt(lAccelAngle);
	sendInt ( output );
}

void main()
{
	slPosition = 0;
	accelAverage = ACCEL_OFFSET;
	init();

	while(1)
	{
		while(!run){};
#define BOT_SHUTOFF_ANGLE 100000
		if((slGyroAngle > BOT_SHUTOFF_ANGLE) || (slGyroAngle < -BOT_SHUTOFF_ANGLE))
			slGyroAngle = 0;
		
#define Kpa	64
#define Kpg	1
#define Kd	6
#define Ki	1

		accelAverage = ((signed long)(accelAverage*7)/8)+(signed long)(accelYRateSlow/8);

		//Compute angle to equilibrium
//		output  = ((signed long)((slGyroAngle)/((signed long)4)));
		output  = ((signed long)((accelAverage-ACCEL_OFFSET)*((signed long)Kpa)));	
		output += ((signed long)((slGyroAngle)*((signed long)Kpg)));	
		output += ((signed long)(slGyroSpeed/((signed long)Kd)));		
//		output += (slPosition*((signed long)Ki));		
		output /= 8;
		

						
		if (output >= 0xFF)
            output = 0xFF;
        else if (output <= -0xFF)
            output = -0xFF;
//      else if ( output <= 0x20 && output >= 0x00 )
//			output = 0x20;
//      else if ( output >= -0x20 && output <= 0x00 )
//			output = -0x20;
#if	1		      
        if( output > 0 )
        {
			setMotor1( FORWARD, output );
			setMotor2( FORWARD, output );
		}
		else
		{
			setMotor1( BACKWARD, -output );
			setMotor2( BACKWARD, -output );
		}
#endif

		
		slPosition += output;
		
#define BOT_POS_WASHOUT 50
		if(slPosition > BOT_POS_WASHOUT)
			slPosition -= BOT_POS_WASHOUT;
		else if(slPosition < -BOT_POS_WASHOUT)
			slPosition += BOT_POS_WASHOUT;
		else
			slPosition = 0;		
		
		run = 0;
#ifdef	DEBUG_MODE_VISUAL		
		showData();
#else
		sendDebugMessage();
#endif
	}
}

/*
	Cases that need to be considered
	
	Case 1:
	Perfectly balanced in the vertical position.... not likely to happen
	
	Case 2:
	Accelerating towards vertical
	
	Case 3:
	


*/