user_routines seems to be the largest so anticipating a request for it here it is:
Code:
/*******************************************************************************
* 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(); */
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/