Robot Code 2006

From Team1370

Current revision as of 01:19, 6 March 2006

We start with the full-featured camera-supporting version of the code from 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) [1]

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

[edit] user_routines.c

/*******************************************************************************
* 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. 
*
*******************************************************************************/

#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 int secondCount;
extern unsigned long msecondcount; //millisecond counter, from user_routines_fast.c
int calibratemem = 127; //the last value the calibrate routine set the power to
unsigned long cbuttondelay = 0; //calibrate speed change button delay
int calibrateTurn = 127;  //the last value the test_spin_arm routine set the power to
unsigned long cbuttondelay_2 = 0;  //button delay for test_spin_arm function


/*******************************************************************************
* 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
  /*----------------------------------------------------*/


  // initialize the serial ports
  Init_Serial_Port_One();
  Init_Serial_Port_Two();

  // make sure printf() output goes to the proper port
  #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;

	// don't move this unless you know what you're doing
	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();
	}

	// Turn on the "Switch 3" LED on the operator interface if
	// the camera is pointed at the green light target. The
	// Switch3_LED variable is also used by the Default_Routine()
	// function below, so disable it inside Default_Routine()
	// if you want to keep this functionality. 
	if(Get_Tracking_State() == CAMERA_ON_TARGET)
	{
		Switch3_LED = 1;
	}
	else
	{
		Switch3_LED = 0;
	}

	// 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;
		}
	}

	// 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();


	// IFI's default routine is commented out for safety reasons
	// and because it also tries to use PWM outputs one and two,
	// which conflicts with the default assignment for the pan
	// and tilt servos.
//  Default_Routine();

	// Team 1370 Custom User-Controlled Routine
	Custom_Routine();

	Generate_Pwms(pwm13,pwm14,pwm15,pwm16);

	// don't move this unless you know what you're doing
	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.

/******************* Current PWM & Dig_I/O Setup **********************/
//	pwm01 = Camera Pan			pwm04 = pwm05 = left wheels
//	pwm02 = Camera Tilt			pwm06 = pwm07 = right wheels
//	pwm03 = Camera Power
//								pwm13 = back door
//	pwm08 = launcher wheel		pwm14 = rear conveyor
//	pwm09 = launcher turret		pwm15 = front conveyor
//								pwm16 = rear conveyor feeder
//
//
//	rc_dig_in06 = turret limit switch (side?)
//	rc_dig_in07 = turret limit switch (side?)
/************************************************************/
/******************* Current Joystick Buttons *************************************************************/
//		Joystick 1						Joystick 2						Joystick 3
//	aux1 = front conveyor			aux 1 = rotate left				aux1 = rev up launcher
//	aux2 = rear conveyor			aux 2 = rotate right			aux2 = rev down launcher
//	thumb = switch fwd/rev			thumb = 1/2 step value			thumb = 1/2 step value for each rev click
//	trig = rear feeder motor		trig = move turret at
//										current power
/***********************************************************************************************************/

void Custom_Routine(void)
{
	/****** Send Joystick data to PWMs ******/
	Joystick_Drive();
	/********** End Joystick Setup **********/
	

	/********* Turret Movement **************/
	calibrate_routine();
	/****************************************/


	/********* Conveyor Movement ************/
	spin_conveyors();	
	/****************************************/

} //end of Custom_Routine



/**********************************************
	Sends Joystick & Throttle data to motors
**********************************************/
void Joystick_Drive(void)
{
	/** Limit_Mix requires an int between 2000 and 2254 **/
	/** Throttle_It takes in the Joystick reading and applies a throttle curve to it, then returns an int **/

	pwm04 = pwm05 = Limit_Mix( Throttle_It(254 - p1_y) + 2000);	//Joystick 1 = left two motors
	pwm06 = pwm07 = Limit_Mix( Throttle_It(p2_y) + 2000);		//Joystick 2 = right two motors
}



/*******************************************************************************
	Sets the output sent to the drive motors depending on what kind
		of throttling is preset inside the function
*******************************************************************************/
int Throttle_It(int Joystick)
{
	unsigned char SpeedMode = 0;  //Set SpeedMode to the throttle type you want to use
	int Throttle = p1_wheel;  //Sets Throttle to the joystick 1 throttle wheel
	int DriveSpeed;

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

	switch(SpeedMode)
	{
		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 = ((long)(Joystick - 127) * (long)(Joystick - 127) * (long)(Joystick - 127)) / 16129 + 127;
			break;

		case 3:  //Exponential Equation 2 (Similar to Exponential Equation 1)
			DriveSpeed = Limit_Mix(2000 + (int)(((long) Joystick - 127) * ((long)Joystick - 127) / 127 + 127));
			if(Joystick < 127) DriveSpeed = Limit_Mix(2000 + (int)(254 - DriveSpeed));
			break;
	}
	
	return(DriveSpeed);
}



/***********************************************
  This function switches between user-controlled
	and camera-controlled operation
************************************************/
void calibrate_routine(void)
{
	char AutoFire;
	static char Locking_State;
	static char temp_Locking_State = -1;


	if (1) AutoFire = 0;	//Change to a check to a specified switch

	if (AutoFire == 0)
	{						//Manual Control
		temp_Locking_State = 0;
		test_spin_wheel();
		test_spin_arm();
	}
	else
	{						//Automated Control
		temp_Locking_State = 1;

		if (Get_Tracking_State() == CAMERA_ON_TARGET)
		{
			aim_arm();			//Turns turret to lock onto goal
			rev_launcher();		//Revs up the launcher to prepare to fire
			if (arm_aimed() == TRUE)
			{
				temp_Locking_State = 5;
			}
			else
			{
				temp_Locking_State = 4;
			}
		}
		else if (Get_Tracking_State() == TARGET_IN_VIEW)
		{
			temp_Locking_State = 3;
		}
		else if (Get_Tracking_State() == SEARCHING)
		{
			temp_Locking_State = 2;
			pwm08 == 127;		//Turn off launcher
		}
	}

	/****** Ouputs to the terminal window current status ******/
	if (temp_Locking_State != Locking_State)
	{
		Locking_State = temp_Locking_State;
		switch (Locking_State)
		{
			case 0:
				printf("Manual mode selected.\\r\
");
				break;
	
			case 1:
				printf("Autmated mode selected.\\r\
");
				break;

			case 2:
				printf("Camera searching...\\r\
");
				break;

			case 3:
				printf("Camera locking on...\\r\
");
				break;
	
			case 4:
				printf("Camera locked. Turret locking on...\\r\
");
				break;

			case 5:
				printf("Turret & Camera Locked!!\\r\
");
				break;
		}
	}
	/** ---------------------------------------------------- **/
}



/****************************************
*		aim_arm :: used for automatic	*
*	turret aiming based on camera		*
*	PAN position						*
*****************************************/
void aim_arm(void)
{
	unsigned char target_speed;
												// Previous values: (reversed due to motor working backwards)
	if(pwm01 < 50) target_speed = 164; 				//90
	else if(pwm01 < 114) target_speed = 149;		//105
	else if(pwm01 < 125) target_speed = 134;		//120
	else if(pwm01 < 130) target_speed = 127;		//127
	else if(pwm01 < 141) target_speed = 120;		//134
	else if(pwm01 < 200) target_speed = 105;		//149
	else if(pwm01 < 255) target_speed = 90;			//164

	if (pwm01 < 129 && pwm01 > 125) pwm09 = 127;
	else if (pwm09 < target_speed-5) pwm09 += 4;
	else if (pwm09 > target_speed+5) pwm09 -= 4;
	else pwm09 = target_speed;
}

/****************************************
*		arm_aimed :: used to check		*
*	whether arm and camera are aimed	*
*	together.  Returns TRUE or FALSE by	*
*	way of a char variable				*
*****************************************/
unsigned char arm_aimed(void)
{
	return(((pwm01 >= 125) && (pwm01 <= 129) && (pwm09 = 127)) ? TRUE : FALSE);
}


/****************************************
*	  rev_launcher:: used to rev		*
*	up the launcher to prepare to fire	*
*****************************************/
void rev_launcher(void)
{
	pwm08 = 190;
}



/****************************************
*		get_power :: finds power		*
*	required to achieve distance		*
*	& height needed for launch.			*
*	Returns a char variable				*
****************************************/
unsigned char get_power(void)
{
	//Find power required to launch ball the necessary distance

	/******* MATH ***********
		const unsigned int WallHeight = 120; //in inches
		const unsigned int RobotHeight = 60; //in inches
		unsigned int LaunchHeight = WallHeight - RobotHeight;

		int tilt_degrees = (((int)TILT_SERVO - 144) * 25)/50; //tilt in degrees
		int tilt_radians = (tilt_degrees * 3.14) / 180;
		unsigned int DistanceToGoal;

		DistanceToGoal = (LaunchHeight * cos(short long tilt)) / sin(short long tilt);
		
		
	************************/
	return(127);
}


/*********************************************
	Calibration Routine For
	Powering Launch Wheel
*********************************************/
void test_spin_wheel(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;
			if(calibratemem > 254) calibratemem = 254;
			printf("Shot power now at %d\\r\
",calibratemem);
			cbuttondelay = msecondcount;
		}
		else if(p3_sw_aux2 == 1){
			if(p3_sw_top == 1){ calibratemem -= 1; }
			else{ calibratemem -= 5; }
			if(calibratemem < 127) calibratemem = 127;
			if(calibratemem > 254) calibratemem = 254;
			printf("Shot power now at %d\\r\
",calibratemem);
			cbuttondelay = msecondcount;
		}
	}
	pwm08 = Limit_Mix(calibratemem + 2000);
}



/*********************************************
	Calibration Routine For Aiming Launcher

*********************************************/
void test_spin_arm(void)
{
	if( p2_sw_aux1 == 1 && p2_sw_aux2 == 1 ){} //if both buttons pressed do nothing
	else if( (msecondcount - cbuttondelay_2) > 100 ){ //check that .4 seconds have passed
		if(p2_sw_aux1 == 1){
			if(p2_sw_top == 5){ calibrateTurn += 5; }
			else{ calibrateTurn += 10; }
			if(calibrateTurn < 0) calibrateTurn = 0;
			if(calibrateTurn > 254) calibrateTurn = 254;
			printf("\\r\
");
			printf("Turn power now at %d\\r\
",calibrateTurn);
			cbuttondelay_2 = msecondcount;
		}
		else if(p2_sw_aux2 == 1){
			if(p2_sw_top == 5){ calibrateTurn -= 5; }
			else{ calibrateTurn -= 10; }
			if(calibrateTurn < 0) calibrateTurn = 0;
			if(calibrateTurn > 254) calibrateTurn = 254;
			printf("\\r\
");
			printf("Shot power now at %d\\r\
",calibrateTurn);
			cbuttondelay_2 = msecondcount;
		}
	}
	
	if(p2_sw_trig == 1) pwm09 = Limit_Mix(255 - calibrateTurn + 2000);
	else pwm09 = 127;
}


/*********************************************
*		spin_conveyors :: rotates motors
*	concerning ball movement to the launcher
**********************************************/
void spin_conveyors(void)
{
	/** p1_sw_top (thumb switch) changes between forward and backward **/

	if(p1_sw_aux1 == 1)
	{
		if(p1_sw_top == 1) pwm15 = 254;
		else pwm15 = 0;
	}
	else pwm15 = 127;

	if(p1_sw_aux2 == 1) pwm14 = 0;
	else pwm14 = 127;

	if(p1_sw_trig == 1)
	{
		if(p1_sw_top == 1) pwm13 = 0;
		else pwm13 = 254;
	}
	else pwm13 = 127;
}
















/*******************************************************************************
* 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(); */


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

[edit] user_routines.h

/*******************************************************************************
* 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.
*******************************************************************************/

#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*/

/* ---- Driving ---- */
void Joystick_Drive(void);
int Throttle_It(int);

/* ---- Firing ---- */
void calibrate_routine (void);
void aim_arm(void);
void rev_launcher(void);
unsigned char arm_aimed(void);
unsigned char get_power(void);
void test_spin_wheel(void);
void test_spin_arm(void);
void spin_conveyors(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
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/

[edit] user_routines_fast.c

/*******************************************************************************
* 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.
*
*******************************************************************************/
#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 = 0;
unsigned int	secondCount = 0;
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;

		/**** NEW Timer Code (25ms timer) ****/
			UpdateDisplay = 1;
		/*************************************/

		/****** OLD Timer Code (Second-hand timer) ******
		  Moved to Process_Data_From_Local_IO() routine
		*************************************************/

	}


//  ***  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 */
    {
        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_out03 ^= 1;			// Pin4 toggles every 25ms
		msecondcount += 25;

		if (t25msDelay > 3)
		{							// 100ms has passed
		    rc_dig_out04 ^= 1;		// Pin3 toggles every 100ms
		    t25msDelay = 0;
		    if (t100msDelay > 9)
		    {						// 1 second has passed
			  secondCount++;
			  t100msDelay = 0;
			  //printf("\\r\
");
			  //printf("Elapsed Time (m) = %d\\r\
",secondCount);
		    }	
		    t100msDelay++;
		 }
		 t25msDelay++;
	  }

}

/*******************************************************************************
* 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 incoming data (remember, interrupts are still active) */
}


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

[edit] Recent Changes

[edit] 3/5/2006

• Further reorganization. The joystick to pwm setup now has its own function. The majority of the timer code was moved into the Process_Data_From_Local_IO() function.
• Integrated the new camera code with the files
• Added rev_launcher(); function to prepare the launcher for firing
• Created status read-outs for the camera and turret locking status

[edit] 2/21/2006

• Reorganized functions and moved the text from calibrate_routine into a separate function
• Made calibrate_routine a switcher between user-controlled aiming and camera-controlled aiming
• Added controls for the conveyors (spin_conveyors(); routine)
• Added Pierce's form of the exponential throttling equation
• Added comments on the current PWMs, Digital I/Os, and Joystick configurations
• Made other minor changes

[edit] 2/15/2006

• Added "test_arm_spin" function to test the rotation of the launcher
• Added & commented out code due to testing the drive wheels (the two sides aren't spinning at the same rate)

[edit] 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

[edit] 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.

[edit] 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

[edit] 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