/*
The Wooden Menace - Robotic Arm
Written by: Chris
http://www.pyroelectro.com

This program polls input from the PS1 controller & adjusts servos accordingly.
*/


#include <p18f452.h>
#include <timers.h>
#include <delays.h>

//The servos are all set to go to an initial 
			//state when the system is powered on

int servo0 = 0xF63B; // Right Gripper Servo
int servo1 = 0xF077; // Left Gripper Servo
int servo2 = 0xEB00; //Second Stage Link Servo
int servo3 = 0xF277; // First Stage Link Servo
int servo4 = 0xF077; //Rotation Servo

		
int count = 0;		 //Counts every Timer1 Interrupt

					//Declare all functions
void init(void);	 
void grip(void);
void move(int,int,int,int,int);
void sample(void);
void InterruptHandlerHigh (void);

void main(void)
{

int c_val=0, i=0, prev_c_val=0;
				//Set I/O ports
TRISA = 0x00;
TRISC = 0x00;
TRISD = 0x04;
				//Clear Output Ports
PORTA = 0x00;
PORTC = 0x00;
PORTD = 0x00;

				//Setup Interrupts
RCON = 0b000000000; 
INTCON = 0b10100000;

				//Setup Timers 1 & 2
OpenTimer0( TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_2 );
OpenTimer1( TIMER_INT_ON & T1_16BIT_RW & T1_SOURCE_INT & T1_PS_1_2 & T1_OSC1EN_OFF & T1_SYNC_EXT_OFF );
				//Initialize Timers 1 & 2
WriteTimer0( 0x3CAF );
WriteTimer1( 0xF63B );

init(); 		//Set Servo Values

				//Start infinite poll loop
while(1){

	/****** Begin Controller Data Polling *************/
	
	PORTDbits.RD1 = 1;		//Setup Cycle						
	PORTDbits.RD3 = 1;		//Data is input into the two HEF4021BT
	
	PORTDbits.RD1 = 0;
	PORTDbits.RD3 = 0;
	
	/***********************************************/

		for(i=0;i<16;i++)	//Algorithm for capturing the 16-bit
		{					//integer telling us which buttons are
							//pressed and which are not
		PORTDbits.RD1 = 1;
		
		PORTDbits.RD1 = 0;
		
		c_val += PORTDbits.RD2;
		
		  if(i!=15)
			c_val = c_val << 1;
		
		}
		
	/********* End Controller Data Polling *************/	

			//This is the algorithm that evaluates if a button has been pressed
							//and exactly which button if one has been pressed
	if(prev_c_val != c_val)
	{
	
	if((c_val << 2) >> 15 == 0)
		move(0xF03B,0xF477,0,0,0);	 				//X -- GRIP!

	if((c_val << 6) >> 15 == 0)
	move(0,0,(servo2-0x0250),(servo3+0x0100),0);	//UP -- MOVE AWAY

	if((c_val << 7) >> 15 == 0)
	move(0,0,(servo2+0x0250),(servo3-0x0100),0);	//DOWN -- MOVE TOWARDS

	if((c_val << 10) >> 15 == 0)
		move(0,0,0,0,(servo4+0x0250));		//LEFT -- ROTATE LEFT

	if((c_val << 11) >> 15 == 0)
		move(0,0,0,0,(servo4-0x0250)); 		//RIGHT -- ROTATE RIGHT
	
	
	if((c_val << 12) >> 15 == 0)
		move(0xFA77,0xEE07,0,0,0);			//CIRCLE -- UNGRIP!
	

	
	}
	
	prev_c_val = c_val;
	
	c_val = 0;

}

}

void init(void)
{

servo0 = 0xF63B; // Right Gripper 
servo1 = 0xF077; // Left Gripper
servo2 = 0xEF77; // Second Stage Link
servo3 = 0xF277; // First Stage Link

}

	//The move function sets all servo values
void move(int one,int two,int three,int four, int five)
{

if(one)
	servo0 = one;// Right Gripper 
if(two)
	servo1 = two;// Left Gripper
if(three)
	servo2 = three; // Second Stage Link
if(four)
	servo3 = four; // First Stage Link
if(five)
	servo4 = five;
}

	//The sample function, while not used, is a way of demonstrating autonomous movement.
				//The controller does not need to be plugged in while this function runs.
void sample(void)
{
init();

move(0,0,0xF400,0xF000,0);

Delay10KTCYx(250);
Delay10KTCYx(250);	//Wait 1 Second

move(0,0,0xEF77,0xF277,0); // GRIP!

Delay10KTCYx(250);
Delay10KTCYx(250);	//Wait 1 Second

move(0,0,0xEE00,0xF500,0);

Delay10KTCYx(250);
Delay10KTCYx(250);	//Wait 1 Second

move(0,0,0xEF77,0xF277,0);	//UNGRIP!

Delay10KTCYx(250);
Delay10KTCYx(250);	//Wait 1 Second

}

//INTERRUPT CONTROL
#pragma code InterruptVectorHigh = 0x08		//interrupt pointer address (0x18 low priority)
void InterruptVectorHigh (void)
{
        _asm						//assembly code starts
        goto InterruptHandlerHigh	//interrupt control
        _endasm						//assembly code ends
}
#pragma code
#pragma interrupt InterruptHandlerHigh		//end interrupt control

void InterruptHandlerHigh()		// Declaration of InterruptHandler
{									//This gets run when either timer overflows from FFFF->0000
        if(INTCONbits.TMR0IF)		//check if TMR0 interrupt flag is set
        {
				WriteTimer0( 0x3CAF );  //Reset Timer0 for 20mS Delay
				WriteTimer1( 0xFC77 );  //Reset Timer1 for 1mS Delay
				count = 0;				//Reset Timer1 Interrupt Counter
			    INTCONbits.TMR0IF = 0;	//Clear TMR0 Flag               
        }
        if(PIR1bits.TMR1IF == 1 && PIE1bits.TMR1IE == 1)        
        {								//If Timer1 interrupts, do your thing...
		count++;						//Increment Timer1 Interrupt Counter
				switch(count){			//Choose which servo to modify
			case 1:     PORTA = 0x08; // First Stage
					    WriteTimer1( servo3 );
						break;
			case 2:		PORTA = 0x04; // Left Gripper
						WriteTimer1( servo1 );
						break;
			case 3:		PORTA = 0x00;
						PORTC = 0x02; // Swivel/Rotate
						WriteTimer1( servo4 );
						break;
			case 4:		PORTC = 0x00;
						PORTA = 0x02; //Right Gripper
						WriteTimer1( servo0 ); 
						break;
			case 5:		PORTA = 0x00;
						PORTC = 0x01; // Second Stage
						WriteTimer1( servo2 ); 
						break;
			case 6:		PORTC = 0x00; //Future Servo?
						WriteTimer1( 0 );
						break;
				}

                PIR1bits.TMR1IF = 0;		//Clear Timer1 flag
                PIE1bits.TMR1IE = 1;		//Clear Timer1 enable flag set to zero
        }

    INTCONbits.GIE = 1;						//Re-enable all interrupts
}