/*
	Serial Motor Controller

    (C) 2005 Jason Hunt

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

    Contact: huntjas2@msu.edu

    FILE: servo.c

    This file gets position data from the main controller
    and keeps the servos updated and motors

	nulluser@gmail.com

	(5 MHZ instruction clock)
*/

#define START_STRING "Linux robot motorcontroller version 0.4"

#include <int16cxx.h>

#pragma  chip PIC16f628
#pragma config |= 0x3FE2 // for ceramic resinator
//#pragma config |= 0x3FF1 // for internal clock

#pragma bit phase0 @ PORTA.0		// outputs for stepper phase selection
#pragma bit phase1 @ PORTA.1
#pragma bit phase2 @ PORTA.2
#pragma bit phase3 @ PORTA.3

#pragma bit turret_tilt_select @ PORTB.0		// outputs for stepper motor selection
#pragma bit turret_pan_select @ PORTB.3		   

#pragma bit led @ PORTB.5 		// output for led
#pragma bit rx	@ PORTB.1 		// data from host          (input)

unsigned char servo_base;		// Base time offsets
unsigned char servo_on_mask;		// or with this to turn servo[x] on
unsigned char servo_off_mask;	// and with this to turn servo[x] off
unsigned char servo_pos;		   		// positions of servos

unsigned char int_mode;				// the state of the intrrupt handler

unsigned char fire;


int turret_state;
int motor_state;


#define turret_stop       0
#define turret_tilt_up    1
#define turret_tilt_down  2
#define turret_pan_left   3
#define turret_pan_right  4



/* the ISR to update the servos */
#pragma interruptSaveCheck  n
#pragma origin 4

interrupt ISR(void)
{
    int_save_registers

	// check for timer zero interrupt
	if (T0IF)
	{
		if (int_mode == 0) // setupup for base pos wait
		{
			int_mode = 1; 					   // next trigger we will setup for base delay
			OPTION = 4;   					   // reset back into 1:16 prescaled mode
			TMR0 = servo_base;      // set base time delay		
	   		PORTA |= servo_on_mask; // turn this one on
		} else
		if (int_mode == 1) 
		{
			int_mode = 2;					// next mode will turn off and check for long delay
			TMR0 = servo_pos;	// reset the timer to the pos
	
		} else
		if (int_mode == 2)
		{
			int_mode = 0;						// reset back to base mode

	  	    PORTA &= servo_off_mask; // turn the current servo off
	
    		OPTION = 7;		// ser the prescale for a longer delay time
			TMR0 = 30;		// set the value for the longer delay
		}

	    T0IF = 0; // clear timer zero interrupt
	} else
	if (TMR1IF)	// timer1 interrupt
	{
		TMR1L = 0x80;				// reset to roll over in 128 cycles
		TMR1H = 0xea;

		if (fire)
			fire--;
		else
			servo_pos = 200;
		
		led=!led;

		if (turret_state != turret_stop)
		{
			if (turret_state == turret_tilt_up)   motor_state--; else
			if (turret_state == turret_tilt_down) motor_state++; else
			if (turret_state == turret_pan_left)  motor_state++; else
    		if (turret_state == turret_pan_right) motor_state--; 

			if (motor_state > 3) motor_state = 0; else
			if (motor_state < 0) motor_state = 3;

			if (motor_state == 0) PORTA = (PORTA & 0xf0) | 0x01; else
			if (motor_state == 1) PORTA = (PORTA & 0xf0) | 0x04; else
			if (motor_state == 2) PORTA = (PORTA & 0xf0) | 0x02; else
			if (motor_state == 3) PORTA = (PORTA & 0xf0) | 0x08; 
		} else
		{
			PORTA &= 0xf0;	// clear phase select lines
		}

		TMR1IF = 0; // clear flag
	}

    int_restore_registers
}
/* end of interrupt service routine */



/* Set the serial port to  9600 8N1 */
void setup_serial(void)
{
	SPBRG = 129;   // divisor for 9600 baud
//    TXSTA = 0x24; // transmitt enable, high speed
    TXSTA = 0x04; // transmitt enable, high speed

	RCSTA = 0x90; // serial enabled



	char ch = RCREG;  // flush buffer
	ch = RCREG;
	ch = RCREG;
}
/* end of setup serial */


/* sends a byte to the serial port */
/*void send_serial(unsigned char byte)
{
	while ((TXSTA & 0x02) == 0);	// wait until buffer is empty
	TXREG  = byte;					// set the transmit byte
}*/
/* end of send byte */


/* sends a byte to the serial port */
unsigned char get_serial(void)
{
	while ((PIR1 & 0x20) == 0);
	return(RCREG);
}
/* end of send byte */


/* called only once during startup */
void start_setup( void )
{
	// setup io ports
	PORTA = 0;				// clear the ports
	PORTB = 0;

	CMCON = 0x07;			// switch comparators off

	TRISA = 0x00;			// all output
	TRISB = 0x02;			// bit 0 (bump) and 1 (rx) are inputs

    GIE = 1;				// interrupts allowed 
	PEIE = 1;				// enable periphial interrupt

	// setup timer 0
    OPTION = 4;			// prescaler divide by 16
    TMR0 = 0;				// clear the timer
    T0IE = 1;				// enable interrupt 


	// setup timer 1
	TMR1L = 0x80;			// reset timer
	TMR1H = 0xe0;

	TMR1ON = 0; 			// shut off timer to modify values
	T1CKPS1 = 1;			// setup prescaler
	T1CKPS0 = 1;
	T1OSCEN = 0;			// shut off oscillator
	TMR1CS = 0;				// internal clock
	TMR1IE = 1;				// enable interrupt
	TMR1ON = 1; 			// turn timer on
	TMR1IF = 0;				// clear interrupt flag*/

}
/* end of start_setup */


/* small delay to let everything settle */
void start_delay( void )
{
	unsigned int i, j;

	for (i = 0; i < 200; i++)
		for (j = 0; j < 50; j++);
}
/* end of start delay */


/* end of start delay */


/* reset all servoes to middle positions */
void home_controls( void )
{
	int_mode = 0;
	fire = 0;

	motor_state = 0;

	turret_state = turret_stop;

	// home the servos
    servo_pos = 200;
	servo_on_mask = 0x10;
	servo_off_mask = 0xff - 0x10;  
	servo_base = 134;
}
/* end of home_servos */



/* main function */
void main(void)
{
	start_setup();

	home_controls();

	setup_serial();

	start_delay();

	while(1)
	{
		char com = get_serial();	// get command

		if (com == 0x60 && fire == 0) 
		{ 
			servo_pos = 100; 
			fire = 60; 
		} else
		if (com == 0x10) 
		{ 
			turret_state = turret_tilt_up;
			turret_tilt_select = 1; 
			turret_pan_select = 0;
		} else
		if (com == 0x20) 
		{ 
			turret_state = turret_tilt_down;
			turret_tilt_select = 1; 
			turret_pan_select = 0;
		} else
		if (com == 0x30) 
		{ 
			turret_state = turret_pan_left;
			turret_pan_select = 1; 
			turret_tilt_select = 0;
		}  else
		if (com == 0x40) 
		{ 
			turret_state = turret_pan_right;
			turret_pan_select = 1; 
			turret_tilt_select = 0;
		}  else
		if (com == 0x50) 
		{ 
			turret_state = turret_stop;
			turret_tilt_select = 0;
			turret_pan_select = 0;
		} 
	}
}
/* end of main */
