Robot Code 2006
From Team1370
(Difference between revisions)
(created page) |
|||
Line 175: | Line 175: | ||
/* Add any other initialization code here. */ | /* 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_One(); | ||
Init_Serial_Port_Two(); | Init_Serial_Port_Two(); | ||
Line 338: | Line 351: | ||
void Custom_Routine(void) | 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 | ||
+ | } | ||
Line 623: | Line 662: | ||
== user_routines_fast.c == | == user_routines_fast.c == | ||
''we have not yet edited this file'' | ''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 |
Revision as of 13:44, 10 February 2006
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