Editing Robot Code 2006

We start with the full-featured camera-supporting version of the code from [http://kevin.org/frc/ http://kevin.org/frc/].

NOTE: Math functions can be used by downloading and including the math.c and math.h files from the following file (courtesy of Team 296 and the ChiefDelphi forums) [http://www.chiefdelphi.com/forums/papers.php?s=&action=downloadpaper&paperid=378]

We add our changes and additions to the following three files:



== user_routines.c ==
<pre>
/*******************************************************************************
* FILE NAME: user_routines.c <FRC VERSION>
*
* DESCRIPTION:
*  This file contains the default mappings of inputs  
*  (like switches, joysticks, and buttons) to outputs on the RC.  
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your 
*  project and replace it with a modified copy. 
*
*
*
* LAST MODIFIED 2/13/2006 BY PIERCE LOPEZ
*******************************************************************************/

#include <stdio.h>

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "serial_ports.h"
#include "camera.h"
#include "camera_menu.h"
#include "tracking.h"
#include "tracking_menu.h"
#include "eeprom.h"
#include "terminal.h"


extern unsigned char aBreakerWasTripped;

/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/
/* EXAMPLES: (see MPLAB C18 User's Guide, p.9 for all types)
unsigned char wheel_revolutions = 0; (can vary from 0 to 255)
unsigned int  delay_count = 7;       (can vary from 0 to 65,535)
int           angle_deviation = 142; (can vary from -32,768 to 32,767)
unsigned long very_big_counter = 0;  (can vary from 0 to 4,294,967,295)
*/

extern unsigned long msecondcount; //millisecond counter, from user_routines_fast.c
unsigned int calibratemem = 127; //the last value the calibrate routine set the power to
unsigned long cbuttondelay = 0; //calibrate speed change button delay


/*******************************************************************************
* FUNCTION NAME: Limit_Switch_Max
* PURPOSE:       Sets a PWM value to neutral (127) if it exceeds 127 and the
*                limit switch is on.
* CALLED FROM:   this file
* ARGUMENTS:     
*     Argument       Type             IO   Description
*     --------       -------------    --   -----------
*     switch_state   unsigned char    I    limit switch state
*     *input_value   pointer           O   points to PWM byte value to be limited
* RETURNS:       void
*******************************************************************************/
void Limit_Switch_Max(unsigned char switch_state, unsigned char *input_value)
{
  if (switch_state == CLOSED)
  { 
    if(*input_value > 127)
      *input_value = 127;
  }
}


/*******************************************************************************
* FUNCTION NAME: Limit_Switch_Min
* PURPOSE:       Sets a PWM value to neutral (127) if it's less than 127 and the
*                limit switch is on.
* CALLED FROM:   this file
* ARGUMENTS:     
*     Argument       Type             IO   Description
*     --------       -------------    --   -----------
*     switch_state   unsigned char    I    limit switch state
*     *input_value   pointer           O   points to PWM byte value to be limited
* RETURNS:       void
*******************************************************************************/
void Limit_Switch_Min(unsigned char switch_state, unsigned char *input_value)
{
  if (switch_state == CLOSED)
  { 
    if(*input_value < 127)
      *input_value = 127;
  }
}


/*******************************************************************************
* FUNCTION NAME: Limit_Mix
* PURPOSE:       Limits the mixed value for one joystick drive.
* CALLED FROM:   Default_Routine, this file
* ARGUMENTS:     
*     Argument             Type    IO   Description
*     --------             ----    --   -----------
*     intermediate_value    int    I    
* RETURNS:       unsigned char
*******************************************************************************/
unsigned char Limit_Mix (int intermediate_value)
{
  static int limited_value;
  
  if (intermediate_value < 2000)
  {
    limited_value = 2000;
  }
  else if (intermediate_value > 2254)
  {
    limited_value = 2254;
  }
  else
  {
    limited_value = intermediate_value;
  }
  return (unsigned char) (limited_value - 2000);
}


/*******************************************************************************
* FUNCTION NAME: User_Initialization
* PURPOSE:       This routine is called first (and only once) in the Main function.  
*                You may modify and add to this function.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void User_Initialization (void)
{
  Set_Number_of_Analog_Channels(SIXTEEN_ANALOG);    /* DO NOT CHANGE! */

/* FIRST: Set up the I/O pins you want to use as digital INPUTS. */
  digital_io_01 = digital_io_02 = digital_io_03 = digital_io_04 = INPUT;
  digital_io_05 = digital_io_06 = digital_io_07 = digital_io_08 = INPUT;
  digital_io_09 = digital_io_10 = digital_io_11 = digital_io_12 = INPUT;
  digital_io_13 = digital_io_14 = digital_io_15 = digital_io_16 = INPUT;
  digital_io_18 = INPUT;  /* Used for pneumatic pressure switch. */
    /* 
     Note: digital_io_01 = digital_io_02 = ... digital_io_04 = INPUT; 
           is the same as the following:

           digital_io_01 = INPUT;
           digital_io_02 = INPUT;
           ...
           digital_io_04 = INPUT;
    */

/* SECOND: Set up the I/O pins you want to use as digital OUTPUTS. */
  digital_io_17 = OUTPUT;    /* Example - Not used in Default Code. */

/* THIRD: Initialize the values on the digital outputs. */
  rc_dig_out17 = 0;

/* FOURTH: Set your initial PWM values.  Neutral is 127. */
  pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
  pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;

/* FIFTH: Set your PWM output types for PWM OUTPUTS 13-16.
  /*   Choose from these parameters for PWM 13-16 respectively:               */
  /*     IFI_PWM  - Standard IFI PWM output generated with Generate_Pwms(...) */
  /*     USER_CCP - User can use PWM pin as digital I/O or CCP pin.           */
  Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);

  /* 
     Example: The following would generate a 40KHz PWM with a 50% duty cycle on the CCP2 pin:

         CCP2CON = 0x3C;
         PR2 = 0xF9;
         CCPR2L = 0x7F;
         T2CON = 0;
         T2CONbits.TMR2ON = 1;

         Setup_PWM_Output_Type(USER_CCP,IFI_PWM,IFI_PWM,IFI_PWM);
  */

  /* Add any other initialization code here. */

  /******************************************************
			Timer Setup 
  *******************************************************/
	T1CON = 0x30;		// 1:8 Prescale
	TMR1H = 0x85;		// Load high byte of Timer1
	TMR1L = 0xED;		// Load low byte of Timer1
	T1CONbits.TMR1ON = 1;	// Turn Timer1 on

	IPR1bits.TMR1IP = 0;	// Set Timer1 Interrupt to low priority
	PIE1bits.TMR1IE = 1;	// Interrupt when Timer1 overflows
	INTCONbits.GIEL = 1;	// Enable Global Low Priority Interrupts
  /*----------------------------------------------------*/

  Init_Serial_Port_One();
  Init_Serial_Port_Two();

#ifdef TERMINAL_SERIAL_PORT_1    
  stdout_serial_port = SERIAL_PORT_ONE;
#endif

#ifdef TERMINAL_SERIAL_PORT_2    
  stdout_serial_port = SERIAL_PORT_TWO;
#endif

  Putdata(&txdata);            /* DO NOT CHANGE! */

//  ***  IFI Code Starts Here***
//
//  Serial_Driver_Initialize();
//
//  printf("IFI 2006 User Processor Initialized ...\\r");  /* Optional - Print initialization message. */

  User_Proc_Is_Ready();         /* DO NOT CHANGE! - last line of User_Initialization */
}

/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Master_uP
* PURPOSE:       Executes every 26.2ms when it gets new data from the master 
*                microprocessor.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void Process_Data_From_Master_uP(void)
{
	static unsigned char count = 0;
	static unsigned char camera_menu_active = 0;
	static unsigned char tracking_menu_active = 0;
	unsigned char terminal_char;
	unsigned char returned_value;

	Getdata(&rxdata);

	// send diagnostic information to the terminal, but don't 
	// overwrite the camera or tracking menu if it's active
	if(camera_menu_active == 0 && tracking_menu_active == 0)
	{
		Tracking_Info_Terminal();
	}

	// This function is responsable for camera initialization 
	// and camera serial data interpretation. Once the camera
	// is initialized and starts sending tracking data, this 
	// function will continuously update the global T_Packet_Data 
	// structure with the received tracking information.
	Camera_Handler();

	// This function reads data placed in the T_Packet_Data
	// structure by the Camera_Handler() function and if new
	// tracking data is available, attempts to keep the center
	// of the tracked object in the center of the camera's
	// image using two servos that drive a pan/tilt platform.
	// If the camera doesn't have the object within it's field 
	// of view, this function will execute a search algorithm 
	// in an attempt to find the object.
	if(tracking_menu_active == 0)
	{
		Servo_Track();
	}

	// this logic guarantees that only one of the menus can be
	// active at any giiven time
	if(camera_menu_active == 1)
	{
		// This function manages the camera menu functionality,
		// which is used to enter camera initialization and
		// color tracking parameters.
		camera_menu_active = Camera_Menu();
	}
	else if(tracking_menu_active == 1)
	{
		// This function manages the tracking menu functionality,
		// which is used to enter parameters that describe how
		// the pan and tilt servos will behave while in searching
		// and tracking modes.
		tracking_menu_active = Tracking_Menu();
	}
	else
	{
		// has the user sent any data via the terminal?
		terminal_char = Read_Terminal_Serial_Port();
		// check to see if any "hotkeys" have been pressed
		if(terminal_char == CM_SETUP_KEY)
		{
			camera_menu_active = 1;
		}
		else if(terminal_char == TM_SETUP_KEY)
		{
			tracking_menu_active = 1;
		}
	}

	Custom_Routine();

	Generate_Pwms(pwm13,pwm14,pwm15,pwm16);

	// This funtion is used by the functions Camera_Menu() and
	// Tracking_Menu() to manage the writing of initialization
	// parameters to your robot controller's non-volatile
	// Electrically Erasable Programmable Read-Only Memory
	// (EEPROM)
	EEPROM_Write_Handler();

	Putdata(&txdata);
}


//  ***  IFI Code Starts Here***
//
//  static unsigned char i;
//
//  Getdata(&rxdata);   /* Get fresh data from the master microprocessor. */
//
//  Default_Routine();  /* Optional.  See below. */
//
//  /* Add your own code here. (a printf will not be displayed when connected to the breaker panel unless a Y cable is used) */
//
//  printf("Port1 Y %3d, X %3d, Fire %d, Top %d\\r",p1_y,p1_x,p1_sw_trig,p1_sw_top);  /* printf EXAMPLE */
//
//  Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
//
//  /* Eample code to check if a breaker was ever tripped. */
//
//  if (aBreakerWasTripped)
//  {
//    for (i=1;i<29;i++)
//    {
//      if (Breaker_Tripped(i))
//        User_Byte1 = i;  /* Update the last breaker tripped on User_Byte1 (to demonstrate the use of a user byte) 
//                            Normally, you do something else if a breaker got tripped (ex: limit a PWM output)     */
//    }
//  }
//
//  Putdata(&txdata);             /* DO NOT CHANGE! */
//}









/*****************************************************************************
*                                                                            *
*                       THIS IS WHERE WE WRITE OUR CODE                      *
*                                                           - p-Lo           *
*****************************************************************************/

//p-Lo: I've left Default_Routine unmodified and unused, as the camera code does by default.
//p-Lo: Thus, here's the *real* default code, which we're using.

void Custom_Routine(void)
{
	/****** Send Joystick data to PWMs ******/
	pwm04 = pwm05 = Limit_Mix( (int) Thottle_It(255 - p1_y) + 2000);  //Joystick 1 = left two motors
	pwm06 = pwm07 = Limit_Mix( (int) Thottle_It(p2_y) + 2000);  //Joystick 2 = right two motors
	
	pwm08 = (p3_sw_trig == 1 ? 250 : 127);  //Joystick 3 trigger = launcher motor on/off
	/********** End Joystick Setup **********/

	calibrate_routine();  //uncomment to control the launch power with the calibration routine

} //end of Custom_Routine



unsigned char Throttle_It(unsigned char Joystick)
{
	unsigned char Caser = 0;  //Set case to the throttle type you want to use
	unsigned char Throttle = p1_wheel;  //Sets Throttle to the joystick 1 throttle wheel
	unsigned char DriveSpeed = 127;

	if (Joystick >= 255) Joystick = 254;
	if (Throttle >= 255) Throttle = 254;

	switch(Caser)
	{
		case 0:  //No Throttling
			DriveSpeed = Joystick;
			break;

		case 1:  //Linear Equation (actually uses a throttle)
			DriveSpeed = ((Joystick - 127) * (Throttle) / 254) + 127;
			break;

		case 2:  //Exponential Equation 1
			DriveSpeed = (char)(((int)(Joystick - 127) * (int)(Joystick - 127) * (int)(Joystick - 127)) / 16129 + 127);
			break;

		case 3:  //Exponential Equation 2
			
			break;
	}

	return(DriveSpeed);
}


void aim_arm(void)
{
	unsigned char target_speed;

	if(pwm01 < 50) target_speed = 90;
	else if(pwm01 < 114) target_speed = 105;
	else if(pwm01 < 125) target_speed = 120;
	else if(pwm01 < 130) target_speed = 127;
	else if(pwm01 < 141) target_speed = 134;
	else if(pwm01 < 200) target_speed = 149;
	else if(pwm01 < 255) target_speed = 164;

	if (pwm09 < target_speed-5) pwm09 += 4;
	else if (pwm09 > target_speed+5) pwm09 -= 4;
	else pwm09 = target_speed;

	if (pwm01 < 129 && pwm01 > 125) pwm09 = 127;
}

unsigned char arm_aimed(void)
{
	return(((pwm01 >= 125) && (pwm01 <= 129) && (pwm09 = 127)) ? TRUE : FALSE);
}

unsigned char get_power(unsigned char tilt)
{
	//Find power required to launch ball the necessary distance
}


/**********************************************
  This function is used to calibrate
  the firing power of the robot.
  Use 3rd joystick aux1 button to increase
  the power, aux2 to decrease. Hold the top
  button to increase/decrease by less. The
  current power is then printed to a connected
  computer.                - p-Lo
************************************************/
void calibrate_routine(void)
{
	if( p3_sw_aux1 == 1 && p3_sw_aux2 == 1 ){} //if both buttons pressed do nothing
	else if( (msecondcount - cbuttondelay) > 400 ){ //check that .4 seconds have passed
		if(p3_sw_aux1 == 1){
			if(p3_sw_top == 1){ calibratemem += 1; }
			else{ calibratemem += 5; }
			if(calibratemem < 127) calibratemem = 127;
			printf("Shot power now at %d\
",calibratemem);
			cbuttondelay = msecondcount;
		}
		else if(p3_sw_aux2 == 1){
			if(p3_sw_top == 1){ calibratemem -= 1; }
			else{ calibratemem -= 5; }
			if(calibratemem < 127) calibratemem = 127;
			printf("Shot power now at %d\
",calibratemem);
			cbuttondelay = msecondcount;
		}
	}
	pwm08 = Limit_Mix(calibratemem + 2000);
}











/*******************************************************************************
*                                                                              *
*            THE FOLLOWING IS _NOT_ USED!!                                     *
*                                                    - p-Lo                    *
********************************************************************************/


/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE:       Performs the default mappings of inputs to outputs for the
*                Robot Controller.
* CALLED FROM:   this file, Process_Data_From_Master_uP routine
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void Default_Routine(void)
{
  
 /*---------- Analog Inputs (Joysticks) to PWM Outputs-----------------------
  *--------------------------------------------------------------------------
  *   This maps the joystick axes to specific PWM outputs.
  */
  pwm01 = p1_y;   
  pwm02 = p2_y;   
  pwm03 = p3_y;   
  pwm04 = p4_y;   
  pwm05 = p1_x;   
  pwm06 = p2_x;   
  pwm07 = p3_x;   
  pwm08 = p4_x;   
  pwm09 = p1_wheel;
  pwm10 = p2_wheel;   
  pwm11 = p3_wheel;   
  pwm12 = p4_wheel;   
  
 /*---------- 1 Joystick Drive ----------------------------------------------
  *--------------------------------------------------------------------------
  *  This code mixes the Y and X axis on Port 1 to allow one joystick drive. 
  *  Joystick forward  = Robot forward
  *  Joystick backward = Robot backward
  *  Joystick right    = Robot rotates right
  *  Joystick left     = Robot rotates left
  *  Connect the right drive motors to PWM13 and/or PWM14 on the RC.
  *  Connect the left  drive motors to PWM15 and/or PWM16 on the RC.
  */  
  pwm13 = pwm14 = Limit_Mix(2000 + p1_y + p1_x - 127);
  pwm15 = pwm16 = Limit_Mix(2000 + p1_y - p1_x + 127);
  
 /*---------- Buttons to Relays----------------------------------------------
  *--------------------------------------------------------------------------
  *  This default code maps the joystick buttons to specific relay outputs.  
  *  Relays 1 and 2 use limit switches to stop the movement in one direction.
  *  The & used below is the C symbol for AND                                
  */
  relay1_fwd = p1_sw_trig & rc_dig_in01;  /* FWD only if switch1 is not closed. */
  relay1_rev = p1_sw_top  & rc_dig_in02;  /* REV only if switch2 is not closed. */
  relay2_fwd = p2_sw_trig & rc_dig_in03;  /* FWD only if switch3 is not closed. */
  relay2_rev = p2_sw_top  & rc_dig_in04;  /* REV only if switch4 is not closed. */
  relay3_fwd = p3_sw_trig;
  relay3_rev = p3_sw_top;
  relay4_fwd = p4_sw_trig;
  relay4_rev = p4_sw_top;
  relay5_fwd = p1_sw_aux1;
  relay5_rev = p1_sw_aux2;
  relay6_fwd = p3_sw_aux1;
  relay6_rev = p3_sw_aux2;
  relay7_fwd = p4_sw_aux1;
  relay7_rev = p4_sw_aux2;
  relay8_fwd = !rc_dig_in18;  /* Power pump only if pressure switch is off. */
  relay8_rev = 0;
  
  /*---------- PWM outputs Limited by Limit Switches  ------------------------*/
  
  Limit_Switch_Max(rc_dig_in05, &pwm03);
  Limit_Switch_Min(rc_dig_in06, &pwm03);
  Limit_Switch_Max(rc_dig_in07, &pwm04);
  Limit_Switch_Min(rc_dig_in08, &pwm04);
  Limit_Switch_Max(rc_dig_in09, &pwm09);
  Limit_Switch_Min(rc_dig_in10, &pwm09);
  Limit_Switch_Max(rc_dig_in11, &pwm10);
  Limit_Switch_Min(rc_dig_in12, &pwm10);
  Limit_Switch_Max(rc_dig_in13, &pwm11);
  Limit_Switch_Min(rc_dig_in14, &pwm11);
  Limit_Switch_Max(rc_dig_in15, &pwm12);
  Limit_Switch_Min(rc_dig_in16, &pwm12);
  
 /*---------- ROBOT FEEDBACK LEDs------------------------------------------------
  *------------------------------------------------------------------------------
  *   This section drives the "ROBOT FEEDBACK" lights on the Operator Interface.
  *   The lights are green for joystick forward and red for joystick reverse.
  *   Both red and green are on when the joystick is centered.  Use the
  *   trim tabs on the joystick to adjust the center.     
  *   These may be changed for any use that the user desires.                       
  */	
  
  if (user_display_mode == 0) /* User Mode is Off */
    
  { /* Check position of Port 1 Joystick */
    if (p1_y >= 0 && p1_y <= 56)
    {                     /* Joystick is in full reverse position */
      Pwm1_green  = 0;    /* Turn PWM1 green LED - OFF */
      Pwm1_red  = 1;      /* Turn PWM1 red LED   - ON  */
    }
    else if (p1_y >= 125 && p1_y <= 129)
    {                     /* Joystick is in neutral position */
      Pwm1_green  = 1;    /* Turn PWM1 green LED - ON */
      Pwm1_red  = 1;      /* Turn PWM1 red LED   - ON */
    }
    else if (p1_y >= 216 && p1_y <= 255)
    {                     /* Joystick is in full forward position*/
      Pwm1_green  = 1;    /* Turn PWM1 green LED - ON  */
      Pwm1_red  = 0;      /* Turn PWM1 red LED   - OFF */
    }
    else
    {                     /* In either forward or reverse position */
      Pwm1_green  = 0;    /* Turn PWM1 green LED - OFF */
      Pwm1_red  = 0;      /* Turn PWM1 red LED   - OFF */
    }  /*END Check position of Port 1 Joystick
    
    /* Check position of Port 2 Y Joystick 
           (or Port 1 X in Single Joystick Drive Mode) */
    if (p2_y >= 0 && p2_y <= 56)
    {                     /* Joystick is in full reverse position */
      Pwm2_green  = 0;    /* Turn pwm2 green LED - OFF */
      Pwm2_red  = 1;      /* Turn pwm2 red LED   - ON  */
    }
    else if (p2_y >= 125 && p2_y <= 129)
    {                     /* Joystick is in neutral position */
      Pwm2_green  = 1;    /* Turn PWM2 green LED - ON */
      Pwm2_red  = 1;      /* Turn PWM2 red LED   - ON */
    }
    else if (p2_y >= 216 && p2_y <= 255)
    {                     /* Joystick is in full forward position */
      Pwm2_green  = 1;    /* Turn PWM2 green LED - ON  */
      Pwm2_red  = 0;      /* Turn PWM2 red LED   - OFF */
    }
    else
    {                     /* In either forward or reverse position */
      Pwm2_green  = 0;    /* Turn PWM2 green LED - OFF */
      Pwm2_red  = 0;      /* Turn PWM2 red LED   - OFF */
    }  /* END Check position of Port 2 Joystick */
    
    /* This drives the Relay 1 and Relay 2 "Robot Feedback" lights on the OI. */
    Relay1_green = relay1_fwd;    /* LED is ON when Relay 1 is FWD */
    Relay1_red = relay1_rev;      /* LED is ON when Relay 1 is REV */
    Relay2_green = relay2_fwd;    /* LED is ON when Relay 2 is FWD */
    Relay2_red = relay2_rev;      /* LED is ON when Relay 2 is REV */

    Switch1_LED = !(int)rc_dig_in01;
    Switch2_LED = !(int)rc_dig_in02;
    Switch3_LED = !(int)rc_dig_in03;
    
  } /* (user_display_mode = 0) (User Mode is Off) */
  
  else  /* User Mode is On - displays data in OI 4-digit display*/
  {
    User_Mode_byte = backup_voltage*10; /* so that decimal doesn't get truncated. */
  }   
  
} /* END Default_Routine(); */


/******************************************************************************/
/******************************************************************************/
/******************************************************************************/


</pre>

== user_routines.h ==
<pre>
/*******************************************************************************
* FILE NAME: user_routines.h
*
* DESCRIPTION: 
*  This is the include file which corresponds to user_routines.c and
*  user_routines_fast.c
*  It contains some aliases and function prototypes used in those files.
*
* USAGE:
*  If you add your own routines to those files, this is a good place to add
*  your custom macros (aliases), type definitions, and function prototypes.
*
* LAST MODIFIED 2/12/2006 BY PIERCE LOPEZ
*******************************************************************************/

#ifndef __user_program_h_
#define __user_program_h_


/*******************************************************************************
                            MACRO DECLARATIONS
*******************************************************************************/
/* Add your macros (aliases and constants) here.                              */
/* Do not edit the ones in ifi_aliases.h                                      */
/* Macros are substituted in at compile time and make your code more readable */
/* as well as making it easy to change a constant value in one place, rather  */
/* than at every place it is used in your code.                               */
/*
 EXAMPLE CONSTANTS:
#define MAXIMUM_LOOPS   5
#define THE_ANSWER      42
#define TRUE            1
#define FALSE           0
#define PI_VAL          3.1415

 EXAMPLE ALIASES:
#define LIMIT_SWITCH_1  rc_dig_int1  (Points to another macro in ifi_aliases.h)
#define MAIN_SOLENOID   solenoid1    (Points to another macro in ifi_aliases.h)
*/

/* Used in limit switch routines in user_routines.c */
#define OPEN        1     /* Limit switch is open (input is floating high). */
#define CLOSED      0     /* Limit switch is closed (input connected to ground). */



/*******************************************************************************
                            TYPEDEF DECLARATIONS
*******************************************************************************/
/* EXAMPLE DATA STRUCTURE */
/*
typedef struct
{
  unsigned int  NEW_CAPTURE_DATA:1;
  unsigned int  LAST_IN1:1;
  unsigned int  LAST_IN2:1;
  unsigned int  WHEEL_COUNTER_UP:1;
  unsigned int  :4;
  unsigned int wheel_left_counter;
  unsigned int wheel_right_counter;
} user_struct;
*/


/*******************************************************************************
                           FUNCTION PROTOTYPES
*******************************************************************************/

/* These routines reside in user_routines.c */
void User_Initialization(void);
void Process_Data_From_Master_uP(void);
void Default_Routine(void);

/* OUR FUNCTIONS*/
void Custom_Routine(void);		/*Team 1370 Custom 2006 Routine*/
void aim_arm(void);
unsigned char Throttle_It(unsigned char);
unsigned char arm_aimed(void);
unsigned char get_power(unsigned char);
void calibrate_routine (void);

/* These routines reside in user_routines_fast.c */
void InterruptHandlerLow (void);  /* DO NOT CHANGE! */
void User_Autonomous_Code(void);  /* Only in full-size FRC system. */
void Process_Data_From_Local_IO(void);


#endif
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/

</pre>

== user_routines_fast.c ==
<pre>
/*******************************************************************************
* FILE NAME: user_routines_fast.c <FRC VERSION>
*
* DESCRIPTION:
*  This file is where the user can add their custom code within the framework
*  of the routines below. 
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your 
*  project and replace it with a modified copy. 
*
* OPTIONS:  Interrupts are disabled and not used by default.
*
*
*
* LAST MODIFIED 2/13/2006
*******************************************************************************/
#include <stdio.h>

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "serial_ports.h"
#include "camera.h"
#include "camera_menu.h"
#include "tracking.h"
#include "tracking_menu.h"
#include "eeprom.h"
#include "terminal.h"
// #include "user_Serialdrv.h"


/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/

/*	Timer Variables   */
unsigned char	t25msDelay;
unsigned char	t100msDelay;
unsigned char	UpdateDisplay;
unsigned int	secondCount;
unsigned long	msecondcount = 0; //measures milliseconds

/*******************************************************************************
* FUNCTION NAME: InterruptVectorLow
* PURPOSE:       Low priority interrupt vector
* CALLED FROM:   nowhere by default
* ARGUMENTS:     none
* RETURNS:       void
* DO NOT MODIFY OR DELETE THIS FUNCTION 
*******************************************************************************/
#pragma code InterruptVectorLow = LOW_INT_VECTOR
void InterruptVectorLow (void)
{
  _asm
    goto InterruptHandlerLow  /*jump to interrupt routine*/
  _endasm
}


/*******************************************************************************
* FUNCTION NAME: InterruptHandlerLow
* PURPOSE:       Low priority interrupt handler
* If you want to use these external low priority interrupts or any of the
* peripheral interrupts then you must enable them in your initialization
* routine.  Innovation First, Inc. will not provide support for using these
* interrupts, so be careful.  There is great potential for glitchy code if good
* interrupt programming practices are not followed.  Especially read p. 28 of
* the "MPLAB(R) C18 C Compiler User's Guide" for information on context saving.
* CALLED FROM:   this file, InterruptVectorLow routine
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
#pragma code
#pragma interruptlow InterruptHandlerLow save=PROD,section(".tmpdata")

void InterruptHandlerLow ()     
{
	if (PIR1bits.RC1IF && PIE1bits.RC1IE) // rx1 interrupt?
	{
		#ifdef ENABLE_SERIAL_PORT_ONE_RX
		Rx_1_Int_Handler(); // call the rx1 interrupt handler (in serial_ports.c)
		#endif
	}                              
	else if (PIR3bits.RC2IF && PIE3bits.RC2IE) // rx2 interrupt?
	{
		#ifdef ENABLE_SERIAL_PORT_TWO_RX
		Rx_2_Int_Handler(); // call the rx2 interrupt handler (in serial_ports.c)
		#endif
	} 
	else if (PIR1bits.TX1IF && PIE1bits.TX1IE) // tx1 interrupt?
	{
		#ifdef ENABLE_SERIAL_PORT_ONE_TX
		Tx_1_Int_Handler(); // call the tx1 interrupt handler (in serial_ports.c)
		#endif
	}                              
	else if (PIR3bits.TX2IF && PIE3bits.TX2IE) // tx2 interrupt?
	{
		#ifdef ENABLE_SERIAL_PORT_TWO_TX
		Tx_2_Int_Handler(); // call the tx2 interrupt handler (in serial_ports.c)
		#endif
	}
	else if (PIR1bits.TMR1IF)
	{
	  PIR1bits.TMR1IF = 0;		// Clear the interrupt flag
	  TMR1H = 0x85;			// Reset Timer 1
	  TMR1L = 0xED;
	  if (t25msDelay > 3)
	  {				// 100ms has passed
	    rc_dig_out03 ^= 1;		// Pin3 toggles every 100ms
	    t25msDelay = 0;
	    if (t100msDelay > 9)
	    {				// 1 second has passed
		  UpdateDisplay = 1;	// so we set our own flag
		  t100msDelay = 0;
	    }	
	    t100msDelay++;
	  }
	  t25msDelay++;

		//msecond count is an unsigned long, with a maximum value over 4 billion
	  if (msecondcount > 4000000000) msecondcount = 4000000001;
	  msecondcount += 25;
	}


//  ***  IFI Code Starts Here***
//                              
//  unsigned char int_byte;       
//  if (INTCON3bits.INT2IF && INTCON3bits.INT2IE)       /* The INT2 pin is RB2/DIG I/O 1. */
//  { 
//    INTCON3bits.INT2IF = 0;
//  }
//  else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE)  /* The INT3 pin is RB3/DIG I/O 2. */
//  {
//    INTCON3bits.INT3IF = 0;
//  }
//  else if (INTCONbits.RBIF && INTCONbits.RBIE)  /* DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed. */
//  {
//    int_byte = PORTB;        /* You must read or write to PORTB */
//    INTCONbits.RBIF = 0;     /*     and clear the interrupt flag         */
//  }                          /*     to clear the interrupt condition.  */
//  else
//  { 
//    CheckUartInts();    /* For Dynamic Debug Tool or buffered printf features. */
//  }
}


/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE:       Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode.  It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM:   main.c file, main() routine when in Autonomous mode
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void User_Autonomous_Code(void)
{
  /* Initialize all PWMs and Relays when entering Autonomous mode, or else it
     will be stuck with the last values mapped from the joysticks.  Remember, 
     even when Disabled it is reading inputs from the Operator Interface. 
  */
  pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
  pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
  relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;
  relay3_fwd = relay3_rev = relay4_fwd = relay4_rev = 0;
  relay5_fwd = relay5_rev = relay6_fwd = relay6_rev = 0;
  relay7_fwd = relay7_rev = relay8_fwd = relay8_rev = 0;

  while (autonomous_mode)   /* DO NOT CHANGE! */
  {
    if (statusflag.NEW_SPI_DATA)      /* 26.2ms loop area */
    {
	static unsigned char count = 0;
	static unsigned char camera_menu_active = 0;
	static unsigned char tracking_menu_active = 0;
	unsigned char terminal_char;
	unsigned char returned_value;

	Getdata(&rxdata);

	// send diagnostic information to the terminal, but don't 
	// overwrite the camera or tracking menu if it's active
	if(camera_menu_active == 0 && tracking_menu_active == 0)
	{
		Tracking_Info_Terminal();
	}

	// This function is responsable for camera initialization 
	// and camera serial data interpretation. Once the camera
	// is initialized and starts sending tracking data, this 
	// function will continuously update the global T_Packet_Data 
	// structure with the received tracking information.
	Camera_Handler();

	// This function reads data placed in the T_Packet_Data
	// structure by the Camera_Handler() function and if new
	// tracking data is available, attempts to keep the center
	// of the tracked object in the center of the camera's
	// image using two servos that drive a pan/tilt platform.
	// If the camera doesn't have the object within it's field 
	// of view, this function will execute a search algorithm 
	// in an attempt to find the object.
	if(tracking_menu_active == 0)
	{
		Servo_Track();
	}

	// this logic guarantees that only one of the menus can be
	// active at any giiven time
	if(camera_menu_active == 1)
	{
		// This function manages the camera menu functionality,
		// which is used to enter camera initialization and
		// color tracking parameters.
		camera_menu_active = Camera_Menu();
	}
	else if(tracking_menu_active == 1)
	{
		// This function manages the tracking menu functionality,
		// which is used to enter parameters that describe how
		// the pan and tilt servos will behave while in searching
		// and tracking modes.
		tracking_menu_active = Tracking_Menu();
	}
	else
	{
		// has the user sent any data via the terminal?
		terminal_char = Read_Terminal_Serial_Port();
		// check to see if any "hotkeys" have been pressed
		if(terminal_char == CM_SETUP_KEY)
		{
			camera_menu_active = 1;
		}
		else if(terminal_char == TM_SETUP_KEY)
		{
			tracking_menu_active = 1;
		}
	}


        //Getdata(&rxdata);   /* DO NOT DELETE, or you will be stuck here forever! */

        /* Add your own autonomous code here. */
		if ((secondCount > 2) && (secondCount < 6))
		{
			pwm04 = pwm05 = 255 - 200;
			pwm06 = pwm07 = 200;
		}
		else if ((secondCount > 8) && (secondCount < 10))
		{
			pwm04 = pwm05 = 127;
			pwm06 = pwm07 = 200;
		}
		else
		{
			pwm04 = pwm05 = 127;
			pwm06 = pwm07 = 127;
		}

        Generate_Pwms(pwm13,pwm14,pwm15,pwm16);

        Putdata(&txdata);   /* DO NOT DELETE, or you will get no PWM outputs! */
    }
  }
}

/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Local_IO
* PURPOSE:       Execute user's realtime code.
* You should modify this routine by adding code which you wish to run fast.
* It will be executed every program loop, and not wait for fresh data 
* from the Operator Interface.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void Process_Data_From_Local_IO(void)
{
  /* Add code here that you want to be executed every program loop. */

  if (UpdateDisplay)
  {
	INTCONbits.GIEL = 0;		// Diable Low Priority Interrupts
	UpdateDisplay = 0;
	INTCONbits.GIEL = 1;		// Enable Low Priority Interrupts
	rc_dig_out04 ^= 1;			// Pin4 toggles every 1s
	secondCount++;
	//printf("\\r\
");
	//printf("Pin 4 = %d, Elapsed Time (s) = %d\\r\
", (int)rc_dig_out04,secondCount);
  }
}

/*******************************************************************************
* FUNCTION NAME: Serial_Char_Callback
* PURPOSE:       Interrupt handler for the TTL_PORT.
* CALLED FROM:   user_SerialDrv.c
* ARGUMENTS:     
*     Argument             Type    IO   Description
*     --------             ----    --   -----------
*     data        unsigned char    I    Data received from the TTL_PORT
* RETURNS:       void
*******************************************************************************/

void Serial_Char_Callback(unsigned char data)
{
  /* Add code to handle incomming data (remember, interrupts are still active) */
}


/******************************************************************************/
/******************************************************************************/
/******************************************************************************/


</pre>

== Recent Changes ==
=== 2/14/2006 ===
*Added the "Throttle_It" function to user_routines.c for easily choosing between various throttling types. ''This function is mainly for testing purposes''
*Added "Throttle_It" prototype to user_routines.h

=== 2/13/2006 ===
*Removed second count printout to the serial port
*Added two more turn speed steppings to the arm aiming routine
*Added the ability to fine adjust in power calibration mode
*Made the power calibration mode max out at 255 instead of going back to zero
*Added " + 2000" to all newer Limit_Mix calls, since the function asks for an input between 2000 and 2254.

=== 2/12/2006 ===
*Added millisecond counter ''msecondcount'' in user_routines_fast.c and user_routines.c
*Added new function, ''calibrate_routine()'', in user_routines.c, prototyped it in user_routines.h, and added a call to it in from Custom_Routine(). ''Comment out the call to calibrate_routine() in Custom_Routine() for standard robot operation.''
*Added some variables specific to calibrate_routine() to user_routines.c

=== Earlier ===
'''In user_routines.c:'''
*Added Timer1 initialization code to the void User_Initialization(void) function
*Changed Joystick direction coding; added typecasting into integer
*Added functions arm_aim, arm_aimed, and get_power functions

'''In user_routines.h:'''
*Added function prototypes for arm_aim, arm_aimed, and get_power functions

'''In user_routines_fast.c:'''
*Added camera code to the User_Autonomous_Code function
*Added includes at the top for the camera code that was added
*Added a basic autonomous routine in the User_Autonomous_Code function for testing purposes
*Added timer variables at the top of the file as part of the Timer1 setup
*Added the "else if (PIR1bits.TMR1IF)" and its content to the InterruptHandlerLow function as part of the Timer1 setup
*Added the "if (UpdateDisplay)" procedure to the Process_Data_From_Local_IO function as part of the Timer1 setup