
; CC5X Version 3.1J, Copyright (c) B Knudsen Data
; C compiler for the PICmicro family
; ************  26. Sep 2005  19:49  *************

	processor  16F628
	radix  DEC

TMR0        EQU   0x01
STATUS      EQU   0x03
PORTA       EQU   0x05
TRISA       EQU   0x85
PORTB       EQU   0x06
TRISB       EQU   0x86
Carry       EQU   0
Zero_       EQU   2
RP0         EQU   5
RP1         EQU   6
T0IF        EQU   2
T0IE        EQU   5
GIE         EQU   7
OPTION_REG  EQU   0x81
PIR1        EQU   0x0C
TMR1L       EQU   0x0E
TMR1H       EQU   0x0F
RCSTA       EQU   0x18
RCREG       EQU   0x1A
CMCON       EQU   0x1F
TXSTA       EQU   0x98
SPBRG       EQU   0x99
PEIE        EQU   6
TMR1IF      EQU   0
TMR1ON      EQU   0
TMR1CS      EQU   1
T1OSCEN     EQU   3
T1CKPS0     EQU   4
T1CKPS1     EQU   5
TMR1IE      EQU   0
turret_tilt_select EQU   0
turret_pan_select EQU   3
servo_base  EQU   0x23
servo_on_mask EQU   0x24
servo_off_mask EQU   0x25
servo_pos   EQU   0x26
int_mode    EQU   0x27
fire        EQU   0x28
turret_state EQU   0x29
motor_state EQU   0x2A
svrWREG     EQU   0x70
svrSTATUS   EQU   0x20
ch          EQU   0x21
i           EQU   0x21
j           EQU   0x22
com         EQU   0x21

	GOTO main

  ; FILE turret.c
			;/*
			;	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
	ORG 0x0004
			;
			;interrupt ISR(void)
			;{
ISR
			;    int_save_registers
	MOVWF svrWREG
	SWAPF STATUS,W
	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVWF svrSTATUS
			;
			;	// check for timer zero interrupt
			;	if (T0IF)
	BTFSS 0x0B,T0IF
	GOTO  m004
			;	{
			;		if (int_mode == 0) // setupup for base pos wait
	MOVF  int_mode,1
	BTFSS 0x03,Zero_
	GOTO  m001
			;		{
			;			int_mode = 1; 					   // next trigger we will setup for base delay
	MOVLW .1
	MOVWF int_mode
			;			OPTION = 4;   					   // reset back into 1:16 prescaled mode
	MOVLW .4
	BSF   0x03,RP0
	MOVWF OPTION_REG
			;			TMR0 = servo_base;      // set base time delay		
	BCF   0x03,RP0
	MOVF  servo_base,W
	MOVWF TMR0
			;	   		PORTA |= servo_on_mask; // turn this one on
	MOVF  servo_on_mask,W
	IORWF PORTA,1
			;		} else
	GOTO  m003
			;		if (int_mode == 1) 
m001	BCF   0x03,RP0
	BCF   0x03,RP1
	DECFSZ int_mode,W
	GOTO  m002
			;		{
			;			int_mode = 2;					// next mode will turn off and check for long delay
	MOVLW .2
	MOVWF int_mode
			;			TMR0 = servo_pos;	// reset the timer to the pos
	MOVF  servo_pos,W
	MOVWF TMR0
			;	
			;		} else
	GOTO  m003
			;		if (int_mode == 2)
m002	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  int_mode,W
	XORLW .2
	BTFSS 0x03,Zero_
	GOTO  m003
			;		{
			;			int_mode = 0;						// reset back to base mode
	CLRF  int_mode
			;
			;	  	    PORTA &= servo_off_mask; // turn the current servo off
	MOVF  servo_off_mask,W
	ANDWF PORTA,1
			;	
			;    		OPTION = 7;		// ser the prescale for a longer delay time
	MOVLW .7
	BSF   0x03,RP0
	MOVWF OPTION_REG
			;			TMR0 = 30;		// set the value for the longer delay
	MOVLW .30
	BCF   0x03,RP0
	MOVWF TMR0
			;		}
			;
			;	    T0IF = 0; // clear timer zero interrupt
m003	BCF   0x0B,T0IF
			;	} else
	GOTO  m018
			;	if (TMR1IF)	// timer1 interrupt
m004	BCF   0x03,RP0
	BCF   0x03,RP1
	BTFSS 0x0C,TMR1IF
	GOTO  m018
			;	{
			;		TMR1L = 0x80;				// reset to roll over in 128 cycles
	MOVLW .128
	MOVWF TMR1L
			;		TMR1H = 0xea;
	MOVLW .234
	MOVWF TMR1H
			;
			;		if (fire)
	MOVF  fire,1
	BTFSC 0x03,Zero_
	GOTO  m005
			;			fire--;
	DECF  fire,1
			;		else
	GOTO  m006
			;			servo_pos = 200;
m005	MOVLW .200
	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVWF servo_pos
			;		
			;		led=!led;
m006	MOVLW .32
	BCF   0x03,RP0
	BCF   0x03,RP1
	XORWF PORTB,1
			;
			;		if (turret_state != turret_stop)
	MOVF  turret_state,1
	BTFSC 0x03,Zero_
	GOTO  m016
			;		{
			;			if (turret_state == turret_tilt_up)   motor_state--; else
	DECFSZ turret_state,W
	GOTO  m007
	DECF  motor_state,1
	GOTO  m010
			;			if (turret_state == turret_tilt_down) motor_state++; else
m007	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  turret_state,W
	XORLW .2
	BTFSS 0x03,Zero_
	GOTO  m008
	INCF  motor_state,1
	GOTO  m010
			;			if (turret_state == turret_pan_left)  motor_state++; else
m008	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  turret_state,W
	XORLW .3
	BTFSS 0x03,Zero_
	GOTO  m009
	INCF  motor_state,1
	GOTO  m010
			;    		if (turret_state == turret_pan_right) motor_state--; 
m009	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  turret_state,W
	XORLW .4
	BTFSC 0x03,Zero_
	DECF  motor_state,1
			;
			;			if (motor_state > 3) motor_state = 0; else
m010	BCF   0x03,RP0
	BCF   0x03,RP1
	BTFSC motor_state,7
	GOTO  m011
	MOVLW .4
	SUBWF motor_state,W
	BTFSS 0x03,Carry
	GOTO  m011
	CLRF  motor_state
	GOTO  m012
			;			if (motor_state < 0) motor_state = 3;
m011	BCF   0x03,RP0
	BCF   0x03,RP1
	BTFSS motor_state,7
	GOTO  m012
	MOVLW .3
	MOVWF motor_state
			;
			;			if (motor_state == 0) PORTA = (PORTA & 0xf0) | 0x01; else
m012	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  motor_state,1
	BTFSS 0x03,Zero_
	GOTO  m013
	MOVLW .240
	ANDWF PORTA,W
	IORLW .1
	MOVWF PORTA
	GOTO  m017
			;			if (motor_state == 1) PORTA = (PORTA & 0xf0) | 0x04; else
m013	BCF   0x03,RP0
	BCF   0x03,RP1
	DECFSZ motor_state,W
	GOTO  m014
	MOVLW .240
	ANDWF PORTA,W
	IORLW .4
	MOVWF PORTA
	GOTO  m017
			;			if (motor_state == 2) PORTA = (PORTA & 0xf0) | 0x02; else
m014	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  motor_state,W
	XORLW .2
	BTFSS 0x03,Zero_
	GOTO  m015
	MOVLW .240
	ANDWF PORTA,W
	IORLW .2
	MOVWF PORTA
	GOTO  m017
			;			if (motor_state == 3) PORTA = (PORTA & 0xf0) | 0x08; 
m015	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  motor_state,W
	XORLW .3
	BTFSS 0x03,Zero_
	GOTO  m017
	MOVLW .240
	ANDWF PORTA,W
	IORLW .8
	MOVWF PORTA
			;		} else
	GOTO  m017
			;		{
			;			PORTA &= 0xf0;	// clear phase select lines
m016	MOVLW .240
	BCF   0x03,RP0
	BCF   0x03,RP1
	ANDWF PORTA,1
			;		}
			;
			;		TMR1IF = 0; // clear flag
m017	BCF   0x03,RP0
	BCF   0x03,RP1
	BCF   0x0C,TMR1IF
			;	}
			;
			;    int_restore_registers
m018	BCF   0x03,RP0
	BCF   0x03,RP1
	SWAPF svrSTATUS,W
	MOVWF STATUS
	SWAPF svrWREG,1
	SWAPF svrWREG,W
			;}
	RETFIE
			;/* end of interrupt service routine */
			;
			;
			;
			;/* Set the serial port to  9600 8N1 */
			;void setup_serial(void)
			;{
setup_serial
			;	SPBRG = 129;   // divisor for 9600 baud
	MOVLW .129
	BSF   0x03,RP0
	BCF   0x03,RP1
	MOVWF SPBRG
			;//    TXSTA = 0x24; // transmitt enable, high speed
			;    TXSTA = 0x04; // transmitt enable, high speed
	MOVLW .4
	MOVWF TXSTA
			;
			;	RCSTA = 0x90; // serial enabled
	MOVLW .144
	BCF   0x03,RP0
	MOVWF RCSTA
			;
			;
			;
			;	char ch = RCREG;  // flush buffer
	MOVF  RCREG,W
	MOVWF ch
			;	ch = RCREG;
	MOVF  RCREG,W
	MOVWF ch
			;	ch = RCREG;
	MOVF  RCREG,W
	MOVWF ch
			;}
	RETURN
			;/* 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)
			;{
get_serial
			;	while ((PIR1 & 0x20) == 0);
m019	BCF   0x03,RP0
	BCF   0x03,RP1
	BTFSS PIR1,5
	GOTO  m019
			;	return(RCREG);
	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  RCREG,W
	RETURN
			;}
			;/* end of send byte */
			;
			;
			;/* called only once during startup */
			;void start_setup( void )
			;{
start_setup
			;	// setup io ports
			;	PORTA = 0;				// clear the ports
	BCF   0x03,RP0
	BCF   0x03,RP1
	CLRF  PORTA
			;	PORTB = 0;
	CLRF  PORTB
			;
			;	CMCON = 0x07;			// switch comparators off
	MOVLW .7
	MOVWF CMCON
			;
			;	TRISA = 0x00;			// all output
	BSF   0x03,RP0
	CLRF  TRISA
			;	TRISB = 0x02;			// bit 0 (bump) and 1 (rx) are inputs
	MOVLW .2
	MOVWF TRISB
			;
			;    GIE = 1;				// interrupts allowed 
	BSF   0x0B,GIE
			;	PEIE = 1;				// enable periphial interrupt
	BSF   0x0B,PEIE
			;
			;	// setup timer 0
			;    OPTION = 4;			// prescaler divide by 16
	MOVLW .4
	MOVWF OPTION_REG
			;    TMR0 = 0;				// clear the timer
	BCF   0x03,RP0
	CLRF  TMR0
			;    T0IE = 1;				// enable interrupt 
	BSF   0x0B,T0IE
			;
			;
			;	// setup timer 1
			;	TMR1L = 0x80;			// reset timer
	MOVLW .128
	MOVWF TMR1L
			;	TMR1H = 0xe0;
	MOVLW .224
	MOVWF TMR1H
			;
			;	TMR1ON = 0; 			// shut off timer to modify values
	BCF   0x10,TMR1ON
			;	T1CKPS1 = 1;			// setup prescaler
	BSF   0x10,T1CKPS1
			;	T1CKPS0 = 1;
	BSF   0x10,T1CKPS0
			;	T1OSCEN = 0;			// shut off oscillator
	BCF   0x10,T1OSCEN
			;	TMR1CS = 0;				// internal clock
	BCF   0x10,TMR1CS
			;	TMR1IE = 1;				// enable interrupt
	BSF   0x03,RP0
	BSF   0x8C,TMR1IE
			;	TMR1ON = 1; 			// turn timer on
	BCF   0x03,RP0
	BSF   0x10,TMR1ON
			;	TMR1IF = 0;				// clear interrupt flag*/
	BCF   0x0C,TMR1IF
			;
			;}
	RETURN
			;/* end of start_setup */
			;
			;
			;/* small delay to let everything settle */
			;void start_delay( void )
			;{
start_delay
			;	unsigned int i, j;
			;
			;	for (i = 0; i < 200; i++)
	BCF   0x03,RP0
	BCF   0x03,RP1
	CLRF  i
m020	MOVLW .200
	BCF   0x03,RP0
	BCF   0x03,RP1
	SUBWF i,W
	BTFSC 0x03,Carry
	GOTO  m023
			;		for (j = 0; j < 50; j++);
	CLRF  j
m021	MOVLW .50
	BCF   0x03,RP0
	BCF   0x03,RP1
	SUBWF j,W
	BTFSC 0x03,Carry
	GOTO  m022
	INCF  j,1
	GOTO  m021
m022	BCF   0x03,RP0
	BCF   0x03,RP1
	INCF  i,1
	GOTO  m020
			;}
m023	RETURN
			;/* end of start delay */
			;
			;
			;/* end of start delay */
			;
			;
			;/* reset all servoes to middle positions */
			;void home_controls( void )
			;{
home_controls
			;	int_mode = 0;
	BCF   0x03,RP0
	BCF   0x03,RP1
	CLRF  int_mode
			;	fire = 0;
	CLRF  fire
			;
			;	motor_state = 0;
	CLRF  motor_state
			;
			;	turret_state = turret_stop;
	CLRF  turret_state
			;
			;	// home the servos
			;    servo_pos = 200;
	MOVLW .200
	MOVWF servo_pos
			;	servo_on_mask = 0x10;
	MOVLW .16
	MOVWF servo_on_mask
			;	servo_off_mask = 0xff - 0x10;  
	MOVLW .239
	MOVWF servo_off_mask
			;	servo_base = 134;
	MOVLW .134
	MOVWF servo_base
			;}
	RETURN
			;/* end of home_servos */
			;
			;
			;
			;/* main function */
			;void main(void)
			;{
main
			;	start_setup();
	CALL  start_setup
			;
			;	home_controls();
	CALL  home_controls
			;
			;	setup_serial();
	CALL  setup_serial
			;
			;	start_delay();
	CALL  start_delay
			;
			;	while(1)
			;	{
			;		char com = get_serial();	// get command
m024	CALL  get_serial
	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVWF com
			;
			;		if (com == 0x60 && fire == 0) 
	MOVF  com,W
	XORLW .96
	BTFSS 0x03,Zero_
	GOTO  m025
	MOVF  fire,1
	BTFSS 0x03,Zero_
	GOTO  m025
			;		{ 
			;			servo_pos = 100; 
	MOVLW .100
	MOVWF servo_pos
			;			fire = 60; 
	MOVLW .60
	MOVWF fire
			;		} else
	GOTO  m024
			;		if (com == 0x10) 
m025	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  com,W
	XORLW .16
	BTFSS 0x03,Zero_
	GOTO  m026
			;		{ 
			;			turret_state = turret_tilt_up;
	MOVLW .1
	MOVWF turret_state
			;			turret_tilt_select = 1; 
	BSF   0x06,turret_tilt_select
			;			turret_pan_select = 0;
	BCF   0x06,turret_pan_select
			;		} else
	GOTO  m024
			;		if (com == 0x20) 
m026	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  com,W
	XORLW .32
	BTFSS 0x03,Zero_
	GOTO  m027
			;		{ 
			;			turret_state = turret_tilt_down;
	MOVLW .2
	MOVWF turret_state
			;			turret_tilt_select = 1; 
	BSF   0x06,turret_tilt_select
			;			turret_pan_select = 0;
	BCF   0x06,turret_pan_select
			;		} else
	GOTO  m024
			;		if (com == 0x30) 
m027	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  com,W
	XORLW .48
	BTFSS 0x03,Zero_
	GOTO  m028
			;		{ 
			;			turret_state = turret_pan_left;
	MOVLW .3
	MOVWF turret_state
			;			turret_pan_select = 1; 
	BSF   0x06,turret_pan_select
			;			turret_tilt_select = 0;
	BCF   0x06,turret_tilt_select
			;		}  else
	GOTO  m024
			;		if (com == 0x40) 
m028	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  com,W
	XORLW .64
	BTFSS 0x03,Zero_
	GOTO  m029
			;		{ 
			;			turret_state = turret_pan_right;
	MOVLW .4
	MOVWF turret_state
			;			turret_pan_select = 1; 
	BSF   0x06,turret_pan_select
			;			turret_tilt_select = 0;
	BCF   0x06,turret_tilt_select
			;		}  else
	GOTO  m024
			;		if (com == 0x50) 
m029	BCF   0x03,RP0
	BCF   0x03,RP1
	MOVF  com,W
	XORLW .80
	BTFSS 0x03,Zero_
	GOTO  m024
			;		{ 
			;			turret_state = turret_stop;
	CLRF  turret_state
			;			turret_tilt_select = 0;
	BCF   0x06,turret_tilt_select
			;			turret_pan_select = 0;
	BCF   0x06,turret_pan_select
			;		} 
			;	}
	GOTO  m024

	ORG 0x2007
	DATA 3FE2H
	END


; *** KEY INFO ***

; 0x0004  168 word(s)  8 % : ISR
; 0x00AC   16 word(s)  0 % : setup_serial
; 0x00BC    8 word(s)  0 % : get_serial
; 0x00C4   32 word(s)  1 % : start_setup
; 0x00E4   23 word(s)  1 % : start_delay
; 0x00FB   15 word(s)  0 % : home_controls
; 0x010A   74 word(s)  3 % : main

; RAM usage: 12 bytes (4 local), 212 bytes free
; Maximum call level: 1 (+1 for interrupt)
; Total of 337 code words (16 %)
