2006 CMUcam2 Code

[aaeamdar]

user_routines seems to be the largest so anticipating a request for it here it is:

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

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#ifndef _CAMERA
#endif
#ifdef _CAMERA
#include "camera.h"
#include "serial_ports.h"
#endif
#define round(x) (     (int)((x < 127) * (x - .5) + (x > 127) * (x + .5))     )


extern unsigned char aBreakerWasTripped;
extern T_Packet_Data_Type camera;
unsigned char lookTable [255];
unsigned char c = 0;

/*** 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) 32-bit storage, PAUL D
*/

/*******************************************************************************
* 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;
    */
#ifdef _CAMERA
#ifdef TERMINAL_SERIAL_PORT_1    
  stdout_serial_port = SERIAL_PORT_ONE;
#endif

#ifdef TERMINAL_SERIAL_PORT_2    
  stdout_serial_port = SERIAL_PORT_TWO;
#endif
#endif

/* 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. */
#ifdef _CAMERA
  Init_Serial_Port_Two();

#define stdout_serial_port SERIAL_PORT_ONE
  Track_Color(GREEN);
#endif
for (c = 0; c < STOP; c++)
{
	lookTable[c] = 254 - round(   (pow(   (c-STOP)/STOP, GENTLE_EXP   ) + STOP)   	);
}
for (c = STOP; c < 255; c++)
{
	lookTable[c] = round(   (pow(   (c-STOP)/STOP, GENTLE_EXP   ) + STOP)   	);
}

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

  #ifndef _CAMERA
  Serial_Driver_Initialize();
  #endif

  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
*******************************************************************************/
static unsigned char COUNTERFORDISPLAY;
void Process_Data_From_Master_uP(void)
{
  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) */
 COUNTERFORDISPLAY++;
 if(COUNTERFORDISPLAY>=38)
  {
  	COUNTERFORDISPLAY=0;
  	printf("Port1 Y %3d, X %3d, Fire %d, Top %d\r",pwm01,pwm05,p1_sw_trig,p1_sw_top);  /* printf EXAMPLE */
  	printf("Port2 Y %3d, X %3d, Fire %d, Top %d\r",pwm02,pwm06,p2_sw_trig,p2_sw_top);  /* printf EXAMPLE */
  }

  Generate_Pwms(pwm13,pwm14,pwm15,pwm16);

  /* Example code to check if a breaker was ever tripped. */
#ifndef _CAMERA
  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)     */
    }
  }
#endif

#ifdef _CAMERA
Camera_Handler();
Servo_Track();
#endif

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

/*******************************************************************************
* 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.
  */
#if 1  // new version, does it work? 1/20/06
	if (p1_y > STOP) // Going fwd
	{	if (DIFF < 0) // Going left
			LEFT_MOTOR = Limit_Mix(2000 + p1_y + DIFF); 
		else if (DIFF > 0) // Going right
			RIGHT_MOTOR = Limit_Mix(2000 + p1_y - DIFF); 
	}
	else if (p1_y < STOP) // Going bwd
	{	if (DIFF < 0) // Going left
			LEFT_MOTOR = Limit_Mix(2000 + p1_y - DIFF); 
		else if (DIFF > 0) // Going right
			RIGHT_MOTOR = Limit_Mix(2000 + p1_y + DIFF); 
	}
	else // if stopped
	{	
		LEFT_MOTOR = Limit_Mix(2000 + DIFF);
		RIGHT_MOTOR = Limit_Mix(2000 - DIFF); 
	}
#endif
#if 0 // two joystick drive, left front-back right steering
//	p1_y = squareSteer(p1_y);
//	p2_x = squareSteer(p2_x);

	if(p1_y <= 127+DEAD_ZONE && p1_y >= 127-DEAD_ZONE) //if p1_y in dead zone
	{	LEFT_MOTOR = STOP;
		RIGHT_MOTOR = STOP;
	}
 	else
	{	LEFT_MOTOR = p1_y;
		RIGHT_MOTOR = p1_y;
	}
	printf("Motor 1: %d Motor 2: %d ", LEFT_MOTOR, RIGHT_MOTOR);
	printf("Left joy: %d R Joy: %d ", p1_y, p2_x);
	if (!(p2_x <= STOP+DEAD_ZONE && p2_x >= STOP-DEAD_ZONE)) // if p2_x not in dead zone
	{
		if (p2_x > STOP+DEAD_ZONE) //if >than dead zone fwd
		{
			if (LEFT_MOTOR + (p2_x - STOP) /STEER_FACTOR_FAST > FULL_FWD) // prevents overloading
				LEFT_MOTOR = FULL_FWD;									// if over 254, 254
			else
				LEFT_MOTOR += (p2_x - STOP) /STEER_FACTOR_FAST;	
			if (RIGHT_MOTOR - (p2_x - STOP) / STEER_FACTOR_SLOW < FULL_BWD) // prevents overloading
				RIGHT_MOTOR = FULL_BWD;									// if below 0, 0
			else
				RIGHT_MOTOR -= (p2_x - STOP) / STEER_FACTOR_SLOW;
		} // end >than dead zone fwd
		if (p2_x < STOP-DEAD_ZONE)
		{
			if  (LEFT_MOTOR - (p2_x - STOP) / STEER_FACTOR_SLOW < 0) // prevents overloading
				LEFT_MOTOR = FULL_BWD;
			else
				LEFT_MOTOR -= (p2_x - STOP) /STEER_FACTOR_SLOW;
			if (RIGHT_MOTOR + (p2_x - STOP) /STEER_FACTOR_FAST > FULL_FWD) // prevents overloading
				RIGHT_MOTOR = FULL_FWD;
			else
				RIGHT_MOTOR += (p2_x - STOP) /STEER_FACTOR_FAST;
		}
	}
	printf("After: Motor 1: %d Motor 2: %d\n", LEFT_MOTOR, RIGHT_MOTOR);		
#endif

#if _TANK_DRIVE // if we want tank drive  
	if(p1_y <= 127+DEAD_ZONE && p1_y >= 127-DEAD_ZONE)
  		LEFT_MOTOR = 127;
 	else
		LEFT_MOTOR = p1_y;
  	if(p2_y <= 127+DEAD_ZONE && p2_y >= 127-DEAD_ZONE)
  		RIGHT_MOTOR = STOP;
  	else
		RIGHT_MOTOR = p2_y;
#endif // end tank drive
#if _BUTTON_FUN // if we want to have one joystick drive with trig

  if (p1_sw_aux1)//Joystick 1 left aux button
	LEFT_MOTOR = STOP; //STOP motor 1
  if (p2_sw_aux1) //Joystick 2 right aux button
	RIGHT_MOTOR = STOP; //STOP motor 2

#endif  //end trigger one joystick drive

  /*if(p3_y <= 127+DEAD_ZONE && p3_y >= 127-DEAD_ZONE)
  	pwm03 = STOP;
  else
	pwm03 = p3_y;   
  if(p4_y <= STOP+DEAD_ZONE && p4_y >= STOP-DEAD_ZONE)
  	pwm04 = STOP;
  else
	pwm04 = p4_y;   
  if(p1_x <= STOP+DEAD_ZONE && p1_x >= STOP-DEAD_ZONE)
	pwm05 = STOP;
  else
 	pwm05 = p1_x;  
  if(p2_x <= STOP+DEAD_ZONE && p2_x >= STOP-DEAD_ZONE)
	pwm06 = STOP;
  else
 	pwm06 = p2_x;   
  if(p3_x <= STOP+DEAD_ZONE && p3_x >= STOP-DEAD_ZONE)
	pwm07 = STOP;
  else
 	pwm07 = p3_x;   
  if(p4_x <= STOP+DEAD_ZONE && p4_x >= STOP-DEAD_ZONE)
	pwm08 = STOP;
  else
 	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(); */


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