/********************************************************************* * Description of program: * * C file for letting the robot cruise around, either by pre- * * path or by a path generated by the compiler for generating a list * * of commands. * * * * * * * * * * * * So much for the purpose of this file * ********************************************************************* * Author: * * Max. Heise, 15.03.2002, 16H10 - created * * Max. Heise, 18.03.2002, 12H00 - Changed so that the two pairs of * * LEDs function the opposite way, one is on, the other off by * * default. This is better because the LED on will signal that the * * program was loaded correctly and the PIC is running. * * Max. Heise, 19.03.2002, 14H40 - Copied asm functions from project * * pwm_f877 and converted them to pseudo C(Inline assembler). * * Max. Heise, 19.03.2002, 19H00 - Conversion to pure C complete * * Max. Heise, 20.03.2002, 10H30 - error in generating mask for LSB * * PWM register, added enable and IN2s for PWM, rewrote * * startSwitchIsOn to faster handle the default case that the * * switch is on. Added init for ENs, IN2s and switch to port init * * functions. More comments. * * Max. Heise, 22.03.2002, 13H10 - Changed usage of pins to have SDA * * and SCL free. * * Max. Heise, 22.03.2002, 13H30 - Copied source file to project * * cruise, added blinking LED0 with help of timer1 module * * Max. Heise, 25.03.2002, 10H00 - Added cruising in main * * Max. Heise, 26.03.2002, 16H00 - Added timer type and variable * * globalTimer * * Max. Heise, 27.03.2002, 16H00 - Changed reload value and start of * * programmed cruising course. * * Max. Heise, 28.03.2002, 13H00 - Found typo/bug in initPortD, it * * was setting PORTC=0 instead of PORTD=0. * * Max. Heise, 28.03.2002, 14H20 - Small bug in counter code that * * * not permit waits==0 * * * ********************************************************************* * TODO/Bugs: * * * *********************************************************************/ /******************* * Defines *******************/ #define TRUE 1 #define FALSE 0 #define PWM0ENABLE RD3 #define PWM1ENABLE RC5 #define PWMIN12 RD1 #define PWMIN02 RD2 #define SWITCH0 RD4 #define BUTTON0 RD6 #define BUTTON1 RD5 #define LED0 RB1 #define LED1 RB0 #define LED2 RD7 #define ERR_CHANNELNO 1 /******************* * Includes *******************/ #include #include /******************* * Macros *******************/ #define bsf(file,index) ((file)|=1<<(index)) #define bcf(file,index) ((file)&=~(1<<(index))) /******************* * global variables *******************/ char counter; /* for function waitRAT */ unsigned char errorno; /*************************************************************** * Struct for global timer * timer1Counter is incremented every overflow of timer1 in ISR, if * timer1Counter>N, it is set to 0 * counter is decremented every N ticks of timer1Counter, * Timer1 is incrementing from 0x0000 to 0xFFFF every instruction * cycle by setting the prescaler to 1:1. At f=4MHz ext clock, * -> T=0,25µs*4=1µs seconds. * 50000=0x3CAF * N=0,1s/deltaT, deltaT=50000*T=50000*1µs=50000µs=0,05s * N=2 * isTerminated==1 -> timer terminated * !isTerminated && !isRunning -> timer is not active * !isTerminated && isRunning -> timer is running ***************************************************************/ struct stru_timer { unsigned int timer1Counter; unsigned int counter; char isTerminated; char isRunning; unsigned int N; } globalTimer; #define TMR1_MSB_RELOAD 0x3C #define TMR1_LSB_RELOAD 0xAF #define GLOBAL_TIMER_N 2 /******************************************************************************************** * Init of PWM channels * Init for PWM mode of Capcom module, p. 61 * PR2's value is calculated like that: * We need a frequency of at least 15kHz, to * avoid hearing the motors. That gives us a * period Tpwm of 6,67,e-5, the formula is on * p. 61 of the PIC16F877 documentation. * With TMR2 prescaler set to 1 it is necessary * to set PR2 to decimal 65=0x41. * Duty cycle, 10 bits: * 00 0000 0000b = 0d * 00 1111 1111b = 255d = 1/4 = default * 01 1111 1111b = 511d = 1/2 * 10 1111 1111b = 767d = 3/4 * 11 1111 1111b = 1023d * 8 MSB Bits in CCPR1L or CCPR2L: * 0011 1111 = 0x3F * 1011 1111 = 0xBF * PWM Mode for Capcom module is initialised * by setting CCP1CON and CCP2CON file bits * 3:0 to binary 11XX. ********************************************************************************************/ void initPWM(void) { /************** * Setup PR2 **************/ PR2=0x41; /**************************************** * Set duty cycle 8 MSB Bits to 0, do not * forget the 2 LSB later ! ****************************************/ CCPR1L=0x00; CCPR2L=0x00; /************************************************** * make RC2/CCP1 and RC1/CCP2 pins an output, p. 29 * by clearing the pins **************************************************/ PORTC=0x00; TRISC&=0xF9; /* 1111 1001 */ /****************************************************************** * set Timer 2 TMR2 prescale value to 1 by clearing bits 1:0, p. 55 * and start the timer by setting bit 2 * 0000 0100=0x04 ******************************************************************/ T2CON=0x04; /*********************************************************** * setup CCP1,CCP2 for PWM operation * clear 2 LSB bits 5:4, set bits 3:0 * to 1100 for PWM mode * 0000 1100=0x0C * 0x00 to clear the two LSB bits of the PWM duty cycle * 0x30 to set the two LSB bits of the PWM duty cycle * 0x0C to set PWM operation of CCP1, CCP2 module ***********************************************************/ CCP1CON=0x0C; CCP2CON=0x0C; /************************************************* * Both CCP1 and CCP2 should now be configured for * PWM operation, but the duty cycle is still set * to 0, so no signal should appear. *************************************************/ } /******************************************************************************************** * Init of Timer1 module * p. 51 ff ********************************************************************************************/ void initTimer1(void) { /****************** * Timer1 clock source * is internal clock (Fosc/4) ******************/ TMR1CS=0; /***************** * Set start values * see calculation * above at stru_timer * TMR1H:TMR1L=10000=0x2710 *****************/ TMR1H=0x27; TMR1L=0x10; /***************** * Prescaler is set * to 1:1 by setting * bits T1CON<5:4> * to 00 ***************** T1CON&=0xCF; /* 1100 1111 */ T1CON|=0x00; /* 0000 0000 */ } /******************************************************************************************** * Start Timer1 ********************************************************************************************/ void startTimer1(void) { /************************ * Reload timer register * with value 0x3CAF ************************/ TMR1H=TMR1_MSB_RELOAD; TMR1L=TMR1_LSB_RELOAD; TMR1ON=1; } /******************************************************************************************** * Stop Timer1 ********************************************************************************************/ void stopTimer1(void) { TMR1ON=0; } /******************************************************************************************** * Init the interrupt control * First in INTCON * then in PIE1 ********************************************************************************************/ void initInterrupt(void) { /********************* * Global interrupt * control flag in * INTCON *********************/ GIE=1; /********************* * Peripheral interrupt * control flag in * INTCON *********************/ PEIE=1; /********************* * Enable interrupt * for Timer1 module * in PIE1 *********************/ TMR1IE=1; /********************* * And clear the TMR1IF * flag for the first * time in PIR1 *********************/ TMR1IF=0; } /******************************************************************************************** * Interrupt service routine ********************************************************************************************/ void interrupt isr(void) { if(TMR1IF) { /************************ * Reload timer register * with value 0xD8EF ************************/ TMR1H=TMR1_MSB_RELOAD; TMR1L=TMR1_LSB_RELOAD; /************************ * Toggle LED0 ************************/ LED0 ? LED0=0: LED0=1; /********************* * If timer is running *********************/ if(globalTimer.isRunning) { /********************* * Increment the * timer1 counter and check if >= N *********************/ globalTimer.timer1Counter++; if(globalTimer.timer1Counter>=globalTimer.N) { if(globalTimer.counter > 0) { globalTimer.counter--; } globalTimer.timer1Counter=0; if(globalTimer.counter==0) { /******************* * Stop the timer *******************/ globalTimer.isRunning=0; globalTimer.isTerminated=1; } } } /********************* * And clear the TMR1IF * flag in PIR1 *********************/ TMR1IF=0; } } /******************************************************************************************** * startGlobalTimer ********************************************************************************************/ void startGlobalTimer(unsigned int counterValue) { /********************* * Disable interrupt * for Timer1 module * in PIE1 *********************/ TMR1IE=0; /************************ * Reload timer register * with value 0xD8EF ************************/ TMR1H=TMR1_MSB_RELOAD; TMR1L=TMR1_LSB_RELOAD; /********************* * Set timer values *********************/ globalTimer.counter=counterValue; globalTimer.isRunning=1; globalTimer.isTerminated=0; globalTimer.timer1Counter=0; /********************* * Enable interrupt * for Timer1 module * in PIE1 *********************/ TMR1IE=1; } /******************************************************************************************** * isFinishedGlobalTimer ********************************************************************************************/ char isFinishedGlobalTimer(void) { char retval; globalTimer.isTerminated ? retval=1 : retval=0; return retval; } /******************************************************************************************** * Init of A/D converter module; p.111 * We are using RA0/AN0 and RA1/AN1 as * analog inputs beware that AN7, AN6 and * AN5 are *not* available on 28pin devices * such as 873, 874 * Unfortunately there is no mode for ADCON1, * p. 112 with CHAN/REFS 2/0, so we have to * sacrify one pin, RA3/AN3, and leave it as an * unused analog input. Mode 3/0 is choosen * by setting bits 3:0 of ADCON1 to 0x04, * binary 0100. ********************************************************************************************/ void initAD(void) { /********************************** * set TRISA 0,1,3 bits for analog * inputs **********************************/ TRISA|=0x0B; /* 0000 1011 */ /******************************************** * ADCON1, format left justified, clear bit 7 * mode 3/0 bits 3:0 binary 0100 ********************************************/ ADCON1=0x04; /****************************************************************** * at the beginning select input channel AN0, bits 5:3 * set to 000, later toggle between channel 0 and channel 1. * Setting for channel 1 is 001 * We are using a 4MHz clock, so regarding table 11-1, p. 116 * we should use a operation time of 8Tosc for a conversion * that is bits 7:6 of ADCON0 set to binary 01 * thus we get a binary value of 01 000 0 0 1 or 0100 0001 or 0x41 * for channel 0. For channel 1 the ADCON0 value is * 01 001 0 0 1 equal 0x49 ******************************************************************/ ADCON0=0x41; /********************************************* * init is now complete, to use the A/D module * follow with step 3., p. 113 *********************************************/ } /******************************************************************************************** * Init of PortA is not needed yet, but analog channels on port A are set up in initAD. ********************************************************************************************/ /******************************************************************************************** * Init of PortB ********************************************************************************************/ void initPortB(void) { /******************* * Outputs: clear TRISB bits * RB1 LED * RB0 LED *******************/ TRISB&=0xFC; // 0xFC -> 1111 1100 /******************* * clear port * register PORTB *******************/ PORTB=0; } /******************************************************************************************** * Init of PortC ********************************************************************************************/ void initPortC(void) { /********************************** * Note that RC1/CCP2 and RC2/CCP1 * are outputs, too. But these are * set in function initPWMChannel() ********************************** /******************* * Outputs: clear TRISC bits * RC5 EN1 * (RC1 CCP2,PWM1) * (RC2 CCP1,PWM0) *******************/ TRISC&=0xDF; /* 1101 1111 */ /******************* * clear port * register PORTC *******************/ PORTC=0; } /******************************************************************************************** * Init of PortD ********************************************************************************************/ void initPortD(void) { /******************* * Inputs: set TRISD bits * RD6 button * RD5 button * RD4 switch *******************/ TRISD|=0x70; /* 0111 0000 */ /******************* * Outputs: clear TRISD bits * RD7 LED for switch * RD3 EN0 * RD2 IN02 * RD1 IN12 *******************/ TRISD&=0x71; /* 0111 0001 */ /********************** * clear port register * PORTC **********************/ PORTD=0; } /******************************************************************************************** * Toggle the A/D channel used for sampling * an analog value from the the potentiometers * see comment in function above * For channel 0 the ADCON0 value is 0x41 * For channel 1 the ADCON0 value is 0x49 * ADCON1 does not change * Location of files: * ADCON0 bank 0, 0x1F ********************************************************************************************/ void toggleADChannel(void) { if(ADCON0==0x41) { /***************************** * Channel 0 currently selected * now switch to channel 1 *****************************/ ADCON0=0x49; } else { /***************************** * Channel 1 currently selected * now switch to channel 0 *****************************/ ADCON0=0x41; } } /******************************************************************************************** * Required acquisition time for A/D module * p. 115 equation 11-1 Tacq=19,72,e-6 * seconds, approx. 2,e-5 seconds. * One instruction is executed in one * cycle, one cycle at 4MHz takes 2,5,e-7 * seconds. Thus we must idle for 80 * instructions * Formula for this loop * Inst(I)=1+1+( (I-1)*(1+2) )+2+2=6+3I-3=3I+3 * +2 Inst.Cycles for the call of this function * => Inst(I)=3I+5 * Inst(I) must be 80 * 80=3I+5 * => I=25 * Timing critical, so this is still in assembler. ********************************************************************************************/ void waitRAT(void) { #asm I EQU 25 movlw I ; 1 Inst.Cycle movwf _counter ; 1 Inst.Cycle WaitRAT_Loop decfsz _counter,f ; 1 Inst.Cycle if I!=0, 2 Inst.Cycles if I==0 goto WaitRAT_Loop ; 2 Inst.Cycles #endasm } /******************************************************************************************** * Wait for A/D conversion to complete * Wait for result, A/D conversion is complete * when bit ADCON0,2 is cleared by hardware. * We only have to wait .... ********************************************************************************************/ void waitADComplete(void) { while((ADCON0&0x04)!=0x00) { /******************* * A/D conversion still * running, do nothing *******************/ ; /* NOP */ } } /******************************************************************************************** * Start A/D conversion by setting bit 2 in ADCON0 * One should wait at least 2 Tad, * p. 115 Note 4 before starting the next A/D * conversion. 2 Tad is, table 11-1, * p. 116 16Tosc is equal 4,e-6 seconds which * is equal 16 Inst.Cycles. * So lets hope that we have enough other things to do * to spend these 16 inst. cycles elsewhere before making the * next A/D conversion ;) ********************************************************************************************/ startADConversion(void) { ADCON0|=0x04; /* 0000 0100 */ } /******************************************************************************************** * PWMChanEN * Sets or Resets the enable for PWM channel 0 or 1 ********************************************************************************************/ void PWMChanEN(char channel, char state) { /************************* * check parameters, this is * why I like objects ..... *************************/ if(channel > 1 || channel < 0 || state > 1 || state < 0 ) { /*************** * scream error ***************/ errorno=ERR_CHANNELNO; } if(channel==0) { /********** * chan0 **********/ PWM0ENABLE=state; } else { /********** * chan1 **********/ PWM1ENABLE=state; } } /******************************************************************************************** * Sets value of Duty Cycle for PWM channel 0 or 1 ********************************************************************************************/ void setDutyCycle( const unsigned char chan, const unsigned char MSB, const unsigned char LSB ) { volatile char *pDutyCycleMSB; volatile char *pDutyCycleLSB; /********************** * Check parameters **********************/ if(chan<0 || chan > 1) { errorno=ERR_CHANNELNO; } /********************* * Change pointers to * the correct register *********************/ if(chan==1) { pDutyCycleMSB=&CCPR2L; pDutyCycleLSB=&CCP2CON; } else { /************* * Channel 0 *************/ pDutyCycleMSB=&CCPR1L; pDutyCycleLSB=&CCP1CON; } /************** * MSB is easy, * just assign it **************/ *pDutyCycleMSB=MSB; /*********************** * PWM 2 LSB bits are stored * in CCPxCON<5:4> * if 2 LSB bits in LSB are * set: * LSB: 1100 0000 * CCPxCON: nn11 nnnn n->don't change * First clear the bits 5:4 * in CCPxCON. Then set to * result in LSB ***************************/ (*pDutyCycleLSB)&=0xCF; /* 1100 1111 */ (*pDutyCycleLSB)|=(LSB>>2); } /******************************************************************************************** * startSwitchIsOn * It is not sufficent that RD4 is HIGH, we need to observe a transition from LOW to HIGH * (positive edge), therefor a bit more logic is needed than for the other two buttons. * There are 3 possibilities: * 1. RD4 is LOW, return 0 * 2. RD4 is HIGH, no pos. edge was ever observed, return 0 * 3. RD4 is HIGH and a pos. edge has once been observed, return 1 ********************************************************************************************/ char startSwitchIsOn(void) { /************************** * Variables **************************/ static bit lastState; static bit currentState; static bit posEdgeObserved; static bit wasHereBefore; char retval=0; /* bits can either be static or global, but never local variables * with this C compiler */ const unsigned int waitTime=20; /* Two seconds */ /************************* * store current state * and get new state *************************/ lastState=currentState; currentState=SWITCH0; /************************* * This to avoid having the * whole thing start on power * up when SWITCH0 is in HIGH * position. *************************/ if( !wasHereBefore ) { wasHereBefore=1; lastState=currentState; } /************************** * Check for pos. edge **************************/ if( (!lastState) && currentState ) { /********************* * This is a pos edge! *********************/ posEdgeObserved=1; /********************* * OK, now that we have * the pos.edge wait two * seconds *********************/ startGlobalTimer(waitTime); while(!isFinishedGlobalTimer()) { // NOP } } /********************************* * Now check the three possibil. * listed above *********************************/ if( (!currentState) || (currentState && (!posEdgeObserved))) { posEdgeObserved=0; retval=0; } else { if( currentState && posEdgeObserved ) { retval=1; } } /************************* * if we return TRUE then * switch on the led RD7 and * enable the two PWM channels * else switch off * and finally return from function .... * Do nothing if an error * occured *************************/ if(retval && !errorno) { LED2=1; PWMChanEN(0, 1); PWMChanEN(1, 1); } else { LED2=0; /* LED for switch */ LED1=0; /* LED which is normaly on */ setDutyCycle(0, 0, 0); setDutyCycle(1, 0, 0); } return retval; } /******************************************************************************************** * led0 ********************************************************************************************/ void buttonled0(void) { /****************** * First pair of * button+LED ******************/ if(!BUTTON0) { /****************** * RD5 low ******************/ LED0=1; } else { /****************** * RD5 high ******************/ LED0=0; } } /******************************************************************************************** * led1 * Note that buttonled0 and buttonled1 are functioning with inverted meaning. ********************************************************************************************/ void buttonled1(void) { /****************** * Second pair of * button+LED ******************/ if(BUTTON1) { /****************** * RD6 high ******************/ LED1=1; } else { /****************** * RD6 low ******************/ LED1=0; } } /******************************************************************************************** * Main ********************************************************************************************/ void main(void) { /******************* * local variabes *******************/ unsigned int ticks; unsigned int timer1Now=0; /* const unsigned char leftMotorMSB[]= { 50, 25, 20, 30, 40, 50, 60, 70, 80, 90, 100, 0, 10, 255 }; const unsigned char rightMotorMSB[]={ 50, 0, 20, 30, 40, 50, 60, 70, 80, 90, 100, 25, 10, 255 }; const unsigned int waitTime[]= { 100, 50, 15, 15, 15, 15, 15, 15, 15, 15, 15, 50, 5000, 1000 }; */ #define STRAIGHT 30 #define TURN_LM 15 #define TURN_RM 30 #define START 10 #define PAUSE 0 #define START_TIME 0 #define TURN_TIME 11 #define STRAIGHT_TIME 30 #define PAUSE_TIME 0 const unsigned char leftMotorMSB[]= { START, STRAIGHT, PAUSE, TURN_LM, PAUSE, START, \ STRAIGHT, PAUSE, TURN_LM, PAUSE, START, STRAIGHT, \ PAUSE, TURN_LM, PAUSE, START, STRAIGHT, PAUSE, \ TURN_LM }; const unsigned char rightMotorMSB[]={ START, STRAIGHT, PAUSE, TURN_RM, PAUSE, START, \ STRAIGHT, PAUSE, TURN_RM, PAUSE, START, STRAIGHT, \ PAUSE, TURN_RM, PAUSE, START, STRAIGHT, PAUSE, \ TURN_RM }; const unsigned int waitTime[]= { START_TIME, STRAIGHT_TIME, PAUSE_TIME, TURN_TIME, PAUSE_TIME, START_TIME, \ STRAIGHT_TIME, PAUSE_TIME, TURN_TIME, PAUSE_TIME, START_TIME, STRAIGHT_TIME, \ PAUSE_TIME, TURN_TIME, PAUSE_TIME, START_TIME, STRAIGHT_TIME, PAUSE_TIME, \ TURN_TIME }; // 0 1 2 3 4 5 // 6 7 8 9 10 11 // 12 13 14 15 16 17 // 18 const unsigned char MAXINDEX=18; char i; static bit start; /******************* * Init hardware *******************/ initPortB(); initPortC(); initPortD(); initPWM(); initAD(); initTimer1(); initInterrupt(); startTimer1(); globalTimer.N=GLOBAL_TIMER_N; /******************* * Endless loop *******************/ while(TRUE) { /**************** * Still available * for debugging ****************/ buttonled1(); start=1; i=0; while(startSwitchIsOn()) { if(i<=MAXINDEX || start) { if(start) { start=0; startGlobalTimer(waitTime[i]); setDutyCycle(0, leftMotorMSB[i], 0); setDutyCycle(1, rightMotorMSB[i], 0); } /**************** * Do the cruising ****************/ if( !isFinishedGlobalTimer()) { /******************* * Change nothing, * just cruise on *******************/ setDutyCycle(0, leftMotorMSB[i], 0); setDutyCycle(1, rightMotorMSB[i], 0); } else { /******************** * set duty cycle to new * value ********************/ i++; startGlobalTimer(waitTime[i]); setDutyCycle(0, leftMotorMSB[i], 0); setDutyCycle(1, rightMotorMSB[i], 0); } } else { /****************** * End of programmed * movement, shut down ******************/ setDutyCycle(0, 0, 0); setDutyCycle(1, 0, 0); } } /* End of while(startSwitchIsOn()) */ } /* End of while(true) */ } /* End of main */ /******************************************************************************************** * EOF ********************************************************************************************/