/*******************************************************************************
* 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)
{
static unsigned int Hybrid_Starting_Position=0;
static char Straight_Long_1=1;
static char Straight_Long_2=0;
static char Straight_Long_3=0;
static char Straight_Short_1=0;
static char Straight_Short_2=0;
static char Turn_90_1=0;
static char Turn_90_2=0;
static char Turn_90_3=0;
static char Turn_90_4=0;
static char Stop_0=0;
static char Stop_1=0;
static char Stop_2=0;
static char Stop_3=0;
static char Stop_4=0;
static int Loop=0;
/* 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;
Hybrid_Starting_Position=RC_Hybrid_Starting_Position_2;
Hybrid_Starting_Position=(Hybrid_Starting_Position << 1) | RC_Hybrid_Starting_Position_1;
RC_Pneum_Trans_High = 0;
RC_Pneum_Trans_Low = 1;
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. */
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
// if (RC_Hybrid_Move){
// if (Stop_0){
// if (Loop<=80){
// Auto_Out (127,127);
// Loop++;
// } else {
// Reset_Encoder_1_Count();
// Reset_Encoder_2_Count();
// Loop=0;
// Stop_0 = 0;
// Straight_Long_1 = 1;
// }
// }
if (Straight_Long_1){
if (Encoder_Right_Count<Bot_Straight_Long){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Straight_Long_1 = 0;
Stop_1 = 1;
}
}
if (Stop_1){
if (Loop<=25){
Auto_Out (117,135);
Loop++;
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Loop=0;
Stop_1 = 0;
Turn_90_1 = 1;
}
}
if (Turn_90_1){
if (Encoder_Right_Count<Bot_Turn_90){
Auto_Out (127,255);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Turn_90_1 = 0;
Straight_Short_1 = 1;
}
}
if (Straight_Short_1){
if (Encoder_Right_Count<Bot_Straight_Short){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Stop_2 = 1;
Straight_Short_1 = 0;
}
}
if (Stop_2){
if (Loop<=25){
Auto_Out (117,135);
Loop++;
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Loop=0;
Stop_2 = 0;
Turn_90_2 = 1;
}
}
if (Turn_90_2){
if (Encoder_Right_Count<Bot_Turn_90){
Auto_Out (70,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Turn_90_2 = 0;
Straight_Long_2 = 1;
}
}
if (Straight_Long_2){
if (Encoder_Right_Count<Bot_Straight_Long){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Straight_Long_2 = 0;
Stop_3 = 1;
}
}
/* if (Stop_3){
if (Loop<=20){
Auto_Out (127,127);
Loop++;
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Loop=0;
Stop_3 = 0;
Turn_90_3 = 1;
}
}
if (Turn_90_3){
if (Encoder_Right_Count<Bot_Turn_90){
Auto_Out (127,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Turn_90_3 = 0;
Straight_Short_2 = 1;
}
}
if (Straight_Short_2){
if (Encoder_Right_Count<Bot_Straight_Short){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Stop_4 = 1;
Straight_Short_2 = 0;
}
}
if (Stop_4){
if (Loop<=20){
Auto_Out (127,127);
Loop++;
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Loop=0;
Stop_4 = 0;
Turn_90_4 = 1;
}
}
if (Turn_90_4){
if (Encoder_Right_Count<Bot_Turn_90){
Auto_Out (127,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Turn_90_4 = 0;
Straight_Long_3 = 1;
}
}
if (Straight_Long_3){
if (Encoder_Right_Count<Bot_Straight_Long){
Auto_Out (60,187);
} else {
Reset_Encoder_1_Count();
Reset_Encoder_2_Count();
Straight_Long_3 = 0;
}
}
*/
// }
// if (RC_Hybrid_Move){// if (RC_Hybrid_Poke_1){// }// if (RC_Hybrid_Poke_2){// }
// RC_Hybrid_Dflt_Poke_Position 0=Inside; 1=Middle// Hybrid_Starting_Position 1=Inside ; 2=Middle ; 3=Outside; 0=Assume No Autonomous...if Hybrod_Move...will only drive straight }
//Process commands from IR Board//Button 1 Overpass 1 - 1st open position from inside//Button 2 Overpass 1 - 1st open position from outside//Button 3 Overpass 2 - 1st open position from inside//Button 4 Overpass 2 - 1st open position from outside// if (RC_IRBoard_CMD1){// }// if (RC_IRBoard_CMD2){// }// if (RC_IRBoard_CMD3){// }// if (RC_IRBoard_CMD4){// }
LabView_Out(); // Write Output to LabView or Data loggr
Putdata(&txdata); /* DO NOT DELETE, or you will get no PWM outputs! */
}
}
}
/*******************************************************************************
* 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 = INPUT; /* 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. */
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
Initialize_Encoders();
//I EXPECT INITIALIZATION VALUES
Reset_Encoder_1_Count(-153);
Reset_Encoder_2_Count(-415);
//start comment
Initialize_Gyro();
Initialize_ADC();
//end comment
init_pid(&arm, 100, 0, 0, 120, 35); // 275 0 0
init_pid(&wrist, 33, 0, 0, 20, 80); //45 30 0
init_pid(&Mr_Roboto, 55, 0 , 0, 100, 25);
init_pid(&robot_dist, 95, 0, 0, 100, 8);
init_pid(&gyro_c, 20, 0, 0, 100, 8);
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 */
}