/******************************************************************************* * FILE NAME: user_routines_fast.c * * DESCRIPTION: * This file is where the user can add their custom code within the framework * of the routines below. This is the autonomous code. * * USAGE: * You can either modify this file to fit your needs, or remove it from your * project and replace it with a modified copy. * * OPTIONS: Interrupts are disabled and not used by default. * *******************************************************************************/ #include "ifi_aliases.h" #include "distance.h" #include "ifi_default.h" #include "ifi_utilities.h" #include "servo.h" #include "user_routines.h" #include "serial_ports.h" #if defined(ENCODER) #include "encoder.h" #endif #if defined(QUADRATUREINT) #include "quadratureint.h" #endif #include "char_int.h" #include "timedelay.h" #include // #include "quadrature.h" #define ERROR(a) // extern unsigned char quadstateleft, quadstateright; // extern long quadleft, quadright; // extern int quad_error; #if defined(QUADRATUREINT) extern void *quadleft; extern void *quadright; #endif /******************************************************************************* * FUNCTION NAME: InterruptVectorLow * PURPOSE: Low priority interrupt vector * CALLED FROM: nowhere by default * ARGUMENTS: none * RETURNS: void * DO NOT MODIFY OR DELETE THIS FUNCTION *******************************************************************************/ #pragma code InterruptVectorLow = LOW_INT_VECTOR void InterruptVectorLow (void) { _asm goto InterruptHandlerLow /*jump to interrupt routine*/ _endasm } /******************************************************************************* * FUNCTION NAME: InterruptHandlerLow * PURPOSE: Low priority interrupt handler * If you want to use these external low priority interrupts or any of the * peripheral interrupts then you must enable them in your initialization * routine. Innovation First, Inc. will not provide support for using these * interrupts, so be careful. There is great potential for glitchy code if good * interrupt programming practices are not followed. Especially read p. 28 of * the "MPLAB(R) C18 C Compiler User's Guide" for information on context saving. * CALLED FROM: this file, InterruptVectorLow routine * ARGUMENTS: none * RETURNS: void *******************************************************************************/ #pragma code // #pragma interruptlow InterruptHandlerLow save=PROD,section(".tmpdata") #pragma interruptlow InterruptHandlerLow save=PROD void InterruptHandlerLow () { #if defined(ENCODER) unsigned char Port_B; unsigned char Port_B_Delta; #endif #if defined(INTERRUPTSERIAL) if (PIR1bits.RC1IF && PIE1bits.RC1IE) // rx1 interrupt? { #ifdef ENABLE_SERIAL_PORT_ONE_RX Rx_1_Int_Handler(); // call the rx1 interrupt handler (in serial_ports.c) #endif } else if (PIR3bits.RC2IF && PIE3bits.RC2IE) // rx2 interrupt? { #ifdef ENABLE_SERIAL_PORT_TWO_RX Rx_2_Int_Handler(); // call the rx2 interrupt handler (in serial_ports.c) #endif } else if (PIR1bits.TX1IF && PIE1bits.TX1IE) // tx1 interrupt? { #ifdef ENABLE_SERIAL_PORT_ONE_TX Tx_1_Int_Handler(); // call the tx1 interrupt handler (in serial_ports.c) #endif } else if (PIR3bits.TX2IF && PIE3bits.TX2IE) // tx2 interrupt? { #ifdef ENABLE_SERIAL_PORT_TWO_TX Tx_2_Int_Handler(); // call the tx2 interrupt handler (in serial_ports.c) #endif } else #endif #if defined(ENCODER) if (INTCON3bits.INT2IF && INTCON3bits.INT2IE) // encoder 1 interrupt? { INTCON3bits.INT2IF = 0; // clear the interrupt flag // printf("A: %d, B: %d\n\r", (int)digital_io_01, (int)rc_dig_in11); #ifdef ENABLE_ENCODER_1 Encoder_1_Int_Handler(); // call the left encoder interrupt handler (in encoder.c) #endif } else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE) // encoder 2 interrupt? { INTCON3bits.INT3IF = 0; // clear the interrupt flag #ifdef ENABLE_ENCODER_2 Encoder_2_Int_Handler(); // call right encoder interrupt handler (in encoder.c) #endif } else if (INTCONbits.RBIF && INTCONbits.RBIE) // encoder 3-6 interrupt? { Port_B = PORTB; // remove the "mismatch condition" by reading port b INTCONbits.RBIF = 0; // clear the interrupt flag Port_B_Delta = Port_B ^ Old_Port_B; // determine which bits have changed Old_Port_B = Port_B; // save a copy of port b for next time around if (Port_B_Delta & 0x10) // did external interrupt 3 change state? { #ifdef ENABLE_ENCODER_3 Encoder_3_Int_Handler(Port_B & 0x10 ? 1 : 0); // call the encoder 3 interrupt handler (in encoder.c) #endif } if (Port_B_Delta & 0x20) // did external interrupt 4 change state? { #ifdef ENABLE_ENCODER_4 Encoder_4_Int_Handler(Port_B & 0x20 ? 1 : 0); // call the encoder 4 interrupt handler (in encoder.c) #endif } if (Port_B_Delta & 0x40) // did external interrupt 5 change state? { #ifdef ENABLE_ENCODER_5 Encoder_5_Int_Handler(Port_B & 0x40 ? 1 : 0); // call the encoder 5 interrupt handler (in encoder.c) #endif } if (Port_B_Delta & 0x80) // did external interrupt 6 change state? { #ifdef ENABLE_ENCODER_6 Encoder_6_Int_Handler(Port_B & 0x80 ? 1 : 0); // call the encoder 6 interrupt handler (in encoder.c) #endif } } #endif #if defined(QUADRATUREINT) if (INTCONbits.RBIF && INTCONbits.RBIE) // interrupt 3-6 { INTCONbits.RBIF = 0; // clear the interrupt flags quadratureint_ISR(quadleft, QUADRATUREINTLEFT); quadratureint_ISR(quadright,QUADRATUREINTRIGHT); } #endif // else // { // CheckUartInts(); /* For Dynamic Debug Tool or buffered printf features. */ // } } /******************************************************************************* * 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) { int left_speed, right_speed, delay=0; unsigned char switch_case,autonomous_state,icnt=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; /* Ethernet plugs to pick which autonomous state we enter */ switch_case=0; if(rc_dig_in13 ) switch_case|=1; if(rc_dig_in14 ) switch_case|=2; if(rc_dig_in15 ) switch_case|=4; if(rc_dig_in16 ) switch_case|=8; if(rc_dig_in17 ) switch_case|=16; if(rc_dig_in18 ) switch_case|=32; switch_case = 0x3f ^ switch_case; switch(switch_case) //read the ethernet plugs and go to that case in the switch { case 1: // From left start pos, go forward around rack and block far side. autonomous_state = 1; break; case 33: // From right start pos, go forward around rack and block far side. case 9: autonomous_state = 1; break; case 8: //drive straight autonomous_state = 8; break; case 16: // score autonomously autonomous_state = 9; break; default: // Stop and do nothing. autonomous_state = 0; break; } while (autonomous_mode) /* DO NOT CHANGE! */ { #if defined(ENCODER) || defined(QUADRATUREINT) long left_encoder; long right_encoder; #endif signed char heading_target; if (statusflag.NEW_SPI_DATA) /* Autonomous slow (26.2ms) loop */ { unsigned char newstate = autonomous_state; Getdata(&rxdata); /* DO NOT DELETE, or you will be stuck here forever! */ #if defined(ENCODER) /* Update encoder counts */ left_encoder = Get_Encoder_1_Count(); right_encoder = Get_Encoder_2_Count(); #endif #if defined(QUADRATUREINT) left_encoder = quadratureint_get(quadleft); right_encoder = quadratureint_get(quadright); #endif left_speed = right_speed = 0; #define delay_time 1000 #if defined(AUTODEBUG) pwm08 = switch_case; pwm09 = DISTANCE(left_encoder,0); pwm10 = DISTANCE(0,right_encoder); pwm11 = (unsigned char) (Get_Analog_Value(rc_ana_in01) >> 2); pwm12 = autonomous_state; #endif switch (autonomous_state) //state machine which holds all diffrent autonmous routes, both offensive and defensive { case 1: /* Defensive Route: Initial forward motion */ if( HEADING(left_encoder,right_encoder) > 0) //"drunken sailor walk" if the heading changes from straight it auto compensates { left_speed = 0; right_speed = 127; } else { left_speed = 127; right_speed = 0; } //left_speed = right_speed = 127; if (DISTANCE(left_encoder,right_encoder) >= INITIAL_DISTANCE) //once desginated inital distance is reached go to next state { timedelay_init(&delay,delay_time); //initilize delay of 1 second newstate++; // go to next state #if defined(AUTODEBUG) printf("States:(%d,%d) Left Encoder:%ld, Right Encoder:%ld\r",autonomous_state,newstate, left_encoder, right_encoder); printf("delay=%d\r",delay); #endif } break; /* End state 1 */ case 2: // Defensive Route: Stop and wait for the motors to stop case 4: // Defensive Route: Stop and wait for the motors to stop case 6: // Defensive Route: Stop and wait for the motors to stop left_speed = right_speed = 0; //full stop #if defined(AUTODEBUG) printf("delay=%d\r", delay); #endif if(timedelay_update(&delay)) //wait for delay to compleate { #if defined(AUTODEBUG) printf("Delay Done! %d\r", delay); printf("States:(%d,%d) Left Encoder:%ld, Right Encoder:%ld\r",autonomous_state,newstate, left_encoder, right_encoder); #endif newstate++; //go to next state } break; /* End Time Delay */ case 3: /* Defensive Route: 90 degree turn depending on which auto switch */ if (switch_case & 0x20) //left turn { heading_target = -90; left_speed = -127; right_speed = 127; } else //right turn { heading_target = 90; left_speed = 127; right_speed = -127; } if (HEADING(left_encoder,right_encoder) >= heading_target) //check to see if turn is completed { timedelay_init(&delay,delay_time); //initilize 1 second delay newstate++; //go to the next state #if defined(AUTODEBUG) printf("States:(%d,%d) Left Encoder:%ld, Right Encoder:%ld\r",autonomous_state,newstate, left_encoder, right_encoder); #endif } break; /* End state 3 */ case 5: // Defensive Route: Go forward if( HEADING(left_encoder,right_encoder) > heading_target) //"drunken sailor walk" if the heading changes from straight it auto compensates { left_speed = 0; right_speed = 127; } else { left_speed = 127; right_speed = 0; } if (DISTANCE(left_encoder,right_encoder) >= INITIAL_DISTANCE+114) { timedelay_init(&delay,delay_time); newstate++; // printf("States:(%d,%d) Left Encoder:%ld, Right Encoder:%ld\r",autonomous_state,newstate, left_encoder, right_encoder); } break; case 7: // Go backward if( HEADING(left_encoder,right_encoder) > heading_target) { left_speed = -127; right_speed = 0; } else { left_speed = 0; right_speed = -127; } if (DISTANCE(left_encoder,right_encoder) <= INITIAL_DISTANCE) { timedelay_init(&delay,delay_time); newstate = 4; // printf("States:(%d,%d) Left Encoder:%ld, Right Encoder:%ld\r",autonomous_state,newstate, left_encoder, right_encoder); } break; /* End state 5*/ case 8: left_speed = right_speed = 127; if (DISTANCE(left_encoder,right_encoder) >= 480) { newstate=0; } break; case 9: //pressurize claw, change arm to angle left_speed = right_speed=0; //make sure we don't go anywhere relay2_fwd = 1; //Presurize claw relay2_rev = 0; if(servo(&pwm05, 125, (unsigned char)(Get_Analog_Value(rc_ana_in01) >> 2), 5, 253, 2)==1) //140 = test angle, this needs to be figured out { newstate=10; // when arm is withen target range, go to next state } break; case 10: //move forward towards rack left_speed = right_speed = 127; //full fowards if (DISTANCE(left_encoder,right_encoder) >= 132) //drive forward 132 inches (11 feet) { newstate=11; //when at 132 inches go to next state } break; case 11: //release claw and back away from rack relay2_fwd = 0; //release claw relay2_rev = 1; left_speed = right_speed = -127; //full reverse break; default: /* Permanent stop - end of defense route */ left_speed = right_speed = 0; //printf("Done\r"); break; /* End state 0 */ } /* End autonomous state machine switch */ autonomous_state = newstate; //printf("Heading:%ld Distance:%ld L/R=%ld,%ld Speed L/R=%d,%d\r", HEADING(left_encoder,right_encoder), DISTANCE(left_encoder,right_encoder),DISTANCE(left_encoder,0), DISTANCE(0,right_encoder),left_speed,right_speed); if(icnt++ > 2) { #if defined(QUADRATUREINT) && defined(QUADRATUREINTDEBUG) printf("quadint:" " left=%ld(%d %d) err=%d(%d) z=%d " "right=%ld(%d %d) err=%d(%d) z=%d\r", quadratureint_get(quadleft), (int)QUADRATUREINTLEFT, (int)quadratureint_getlastx(quadleft), quadratureint_geterr(quadleft), (int)quadratureint_getlasterr(quadleft), quadratureint_getzero(quadleft), quadratureint_get(quadright), (int)QUADRATUREINTRIGHT, (int)quadratureint_getlastx(quadright), quadratureint_geterr(quadright), (int)quadratureint_getlasterr(quadright), quadratureint_getzero(quadright)); #endif // printf("%ld Autonomous State:%d, Speed (L,R):%d,%d encoder (L,R):%d,%d delay: %x\r", // time,autonomous_state,left_speed,right_speed,(int)left_encoder,(int)right_encoder,delay); icnt = 0; } pwm01 = pwm02 = char_int(left_speed); //set left drive motors pwm03 = pwm04 = char_int(-right_speed); //inverse and set left drive motors Generate_Pwms(pwm13,pwm14,pwm15,pwm16); #if 1 //no protection from faulty pressure guage relay8_fwd = !rc_dig_in01; /* Power pump only if pressure switch is off. */ relay8_rev = 0; #else //protection from fauly pressure guage (should not be required) if( 2 & toggleswitch(&toggle_state, 2, 14, (unsigned char)rc_dig_in01) ) { relay8_fwd = 0; } else { relay8_fwd = 1; } relay8_rev = 0; #endif Putdata(&txdata); /* DO NOT DELETE, or you will get no PWM outputs! */ } else { // Fast autonomous loop #if defined(QUADRATUREINT) (void) quadratureint_update(quadleft); (void) quadratureint_update(quadright); #endif } // time++; } } // void User_Autonomous_Code() /******************************************************************************* * FUNCTION NAME: Process_Data_From_Local_IO * PURPOSE: Execute user's realtime code. * You should modify this routine by adding code which you wish to run fast. * It will be executed every program loop, and not wait for fresh data * from the Operator Interface. * CALLED FROM: main.c * ARGUMENTS: none * RETURNS: void *******************************************************************************/ void Process_Data_From_Local_IO(void) { // quad_error+=quadrature(&quadstateleft,(char)rc_dig_in01, // (char)rc_dig_in02,&quadleft); // printf("quad=%d,%d io=%d,%d\r",(int)quadstateleft,(int)quadleft,(int)rc_dig_in01,(int)rc_dig_in02); #if defined(QUADRATUREINT) (void) quadratureint_update(quadleft); (void) quadratureint_update(quadright); #endif } /******************************************************************************* * FUNCTION NAME: Serial_Char_Callback * PURPOSE: Interrupt handler for the TTL_PORT. * CALLED FROM: user_SerialDrv.c * ARGUMENTS: * Argument Type IO Description * -------- ---- -- ----------- * data unsigned char I Data received from the TTL_PORT * RETURNS: void *******************************************************************************/ // void Serial_Char_Callback(unsigned char data) // { // /* Add code to handle incomming data (remember, interrupts are still active) */ // } /******************************************************************************/ /******************************************************************************/ /******************************************************************************/