Robot Code 2006
From Team1370
Revision as of 13:44, 10 February 2006 by 167.21.254.12 (Talk)
We start with the full-featured camera-supporting version of the code from http://kevin.org/frc/. We add our changes and additions to the following three files:
Contents |
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)
*/
/*******************************************************************************
* 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) (255 - p1_y) ); //Joystick 1 = left two motors
pwm06 = pwm07 = Limit_Mix( (int) p2_y ); //Joystick 2 = right two motors
pwm08 = (p3_sw_trig == 1 ? 250 : 127); //Joystick 3 trigger = launcher motor on/off
/********** End Joystick Setup **********/
} //end of Custom_Routine
void aim_arm(void)
{
unsigned char target_speed;
if(pwm01 < 50) target_speed = 90;
else if(pwm01 < 125) target_speed = 105;
else if(pwm01 < 130) target_speed = 127;
else if(pwm01 < 200) target_speed = 149;
else if(pwm01 < 255) target_speed = 164;
if (pwm09 < target_speed-5) pwm09 += 3;
else if (pwm09 > target_speed+5) pwm09 -= 3;
else pwm09 = target_speed;
}
unsigned char arm_aimed(void)
{
return(((pwm01 >= 125) && (pwm01 <= 129) && (pwm09 = 127)) ? 1 : 0);
}
unsigned char get_power(unsigned char tilt)
{
//Find power required to launch ball the necessary distance
}
/*******************************************************************************
* *
* 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(); */
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
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);//unused
void Custom_Routine(void);//replacement for Default_Routine
/* 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
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
user_routines_fast.c
we have not yet edited this file
Recent Changes
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
