/* This simple program reads the input from a variable resistor and the then uses pwm to change the speed of the wheel It used timer1 and interupts */ #include #define FCY 117920000 #include #include #include #include #include #include #include #include #include #define PERIOD 7300 // for 1000 Hz pwm frequency #define MAX_DUTY 2*7300 // MAX_DUTY = 2*PERIOD since pwm prescaler is 2 #define MAX_DELTA_U MAX_DUTY //the is the largest increment in duty cycle we can have #define MAXCNT 719 // maximum count for QEI encoders before interrupt, 720 counts per revolution #define MAX_U (double) MAX_DUTY #define MAX_ISUM 10000.0 #define max(A ,B) ((A) > (B) ? (A) : (B)) #define min(A, B) ((A) < (B) ? (A) : (B)) // Configuration Bits _FOSC(CSW_FSCM_OFF & FRC_PLL16); //FRC 7.37MHz * 16 = 117.92MHz/29.48MIPS Tcy=33.9nS _FWDT(WDT_OFF); // watchdog timer off _FBORPOR(PBOR_OFF & BORV27 & PWRT_16 & MCLR_EN); //brown out parameters _FGS(GWRP_OFF); // define some variables unsigned int AD_value; unsigned int GO; int encindex, p1, p2, r1, r2, Delta; /***********************************************************************/ // timer 1 interupt handler void __attribute__((interrupt,auto_psv)) _T1Interrupt( void ) { unsigned int ReadQEI( void ); extern unsigned int AD_value; extern unsigned int GO; extern int p2, r2, encindex; // read from the A/D channel ADCON1bits.SAMP = 1; // start the sampling while(!ADCON1bits.DONE); AD_value = ReadADC10(0); // update the position variables p2 = (int) ReadQEI(); r2 = encindex; // reset Timer 1 interrupt flag IFS0bits.T1IF = 0; // we have a new value GO = 1; } /***********************************************************************/ // QEI interupt handler void __attribute__((interrupt,no_auto_psv)) _QEIInterrupt( void ) { extern int encindex; // update the encoder count every time the counter POSCNT gets to MAXCNT if (QEICONbits.UPDN ) { encindex++; } else { encindex--; } IFS2bits.QEIIF = 0; // } /****************************************************************************************************/ // Initialize timer 1 void Init_Timer1( unsigned int period ) { unsigned int config; config = T1_INT_PRIOR_4 & // set interrupt priority to 2 T1_INT_ON; // enable the interrupts ConfigIntTimer1( config ); config = T1_ON & // turn on the timer T1_IDLE_CON & // operate during sleep T1_GATE_OFF & // timer gate accumulation is disabled T1_PS_1_256 & // timer prescale is 256 T1_SYNC_EXT_OFF & // don't synch with external clock T1_SOURCE_INT; // use the internal clock OpenTimer1( config, period ); return; } /***********************************************************************/ // setup ADC10 void adc_init(void){ unsigned int config1, config2, config3, configport, configscan; ADCON1bits.ADON = 0 ; // turn off ADC SetChanADC10( ADC_CH0_NEG_SAMPLEA_VREFN & // negative reference for channel 0 is VREF negative ADC_CH0_POS_SAMPLEA_AN3 // // ADC_CHX_NEG_SAMPLEA_VREFN & // negative reference for channel 1 is VREF negative // ADC_CHX_POS_SAMPLEA_AN3AN4AN5 ); ConfigIntADC10( ADC_INT_DISABLE ); // disable the interrupts config1 = ADC_MODULE_ON & //turn on ADC module ADC_IDLE_CONTINUE & // let it idle if not in use ADC_FORMAT_INTG & // unsigned integer format ADC_CLK_AUTO & // manual trigger source ADC_AUTO_SAMPLING_OFF & // do not continue sampling ADC_SAMPLE_SIMULTANEOUS & // sample both channels at the same time ADC_SAMP_ON; // enable sampling config2 = ADC_VREF_AVDD_AVSS & // voltage reference ADC_SCAN_OFF & // don't scan ADC_CONVERT_CH0 & // convert channel 0 ADC_SAMPLES_PER_INT_1 & // 1 samples per interupt ADC_ALT_BUF_OFF & // don't use the alternate buffer ADC_ALT_INPUT_OFF; // don't use an alternate input config3 = ADC_SAMPLE_TIME_2 & // auto sample time bits ADC_CONV_CLK_SYSTEM & // use the system clock ADC_CONV_CLK_13Tcy; // conversion clock speed (coeff of TCY is ADCS) configport = ENABLE_AN3_ANA; // parameters to be configured in the to ADPCFG configscan = SCAN_NONE; // scan select parameter for the ADCSSL register OpenADC10( config1, config2, config3, configport, configscan ); } /*****************************************************************/ // setup pwm void pwm_init(void){ unsigned int config1, config2, config3; unsigned int sptime; config1 = PWM_INT_DIS & // disable the interrupt PWM_FLTA_DIS_INT; // disable the interrupt on fault ConfigIntMCPWM( config1 ); config1 = PWM_EN & // enable the PWM module PWM_IPCLK_SCALE4 & // input prescaler set to 2 PWM_OP_SCALE1 & // post scalar set to 1 PWM_MOD_UPDN; // free running mode config2 = PWM_MOD1_IND & // pwm modules run independently PWM_MOD2_IND & PWM_MOD3_IND & PWM_MOD4_IND & PWM_PDIS1H & // disable 1 high PWM_PDIS2H & // disabale 2 high PWM_PEN3H & // enable 3 high PWM_PDIS4H & // disable 4 high PWM_PDIS1L & // disable 1 low PWM_PDIS2L & // disable 2 low PWM_PDIS3L & // disable 3 low PWM_PDIS4L ; //disable 4 low config3 = PWM_UEN; // enable updates sptime = 0x0; OpenMCPWM( PERIOD, sptime, config1, config2, config3 ); } /*****************************************************************/ // setup the UART void uart1_init(void) { unsigned int config1, config2, ubrg; config1 = UART_EN & // enable the UART UART_IDLE_CON & // set idle mode UART_DIS_WAKE & // disable wake-up on start UART_DIS_LOOPBACK & // disable loopback UART_DIS_ABAUD & // diable autobaud rate detect UART_NO_PAR_8BIT & // no parity, 8 bits UART_1STOPBIT; // one stop bit config2 = UART_INT_TX_BUF_EMPTY & // interrupt anytime a buffer is empty UART_TX_PIN_NORMAL & // set transmit break pin UART_TX_ENABLE & // enable UART transmission UART_INT_RX_CHAR & // recieve interrupt mode selected UART_ADR_DETECT_DIS & // disable address detect UART_RX_OVERRUN_CLEAR; // overrun bit clear ubrg = 15; // 11520 baud OpenUART1( config1, config2, ubrg); } /*****************************************************************/ // setup the QEI encoder void encoder_init(void) { unsigned int config1, config2; config1 = QEI_DIR_SEL_QEB & QEI_INT_CLK & QEI_INDEX_RESET_DISABLE & // QEI index pulse resets postion counter QEI_CLK_PRESCALE_1 & QEI_GATED_ACC_DISABLE & QEI_NORMAL_IO & QEI_INPUTS_NOSWAP & QEI_MODE_x2_MATCH & // reset on match QEI_DOWN_COUNT & // count up QEI_IDLE_CON; // continue on idle config2 = POS_CNT_ERR_INT_DISABLE & // diable error interrupts QEI_QE_CLK_DIVIDE_1_1 & //1_256 QEI_QE_OUT_ENABLE & // enable digital filter MATCH_INDEX_INPUT_PHASEA & MATCH_INDEX_INPUT_LOW; OpenQEI( config1, config2 ); config1 = QEI_INT_ENABLE & // enable the interups QEI_INT_PRI_2 ; // set the priority to two WriteQEI( (unsigned int )MAXCNT ); ConfigIntQEI( config1 ); } /*****************************************************************/ int main ( void ) { extern unsigned int AD_value; extern unsigned int GO; extern int r1, r2, p1, p2; unsigned int dutycyclereg, dutycycle, period; char updatedisable; double scale, time; double u, error, speed_scale, speed, AD_scale, dt; double R; // enable the input for A/D TRISBbits.TRISB3 = 1; // enable the input for QEI TRISBbits.TRISB4 = 1; TRISBbits.TRISB5 = 1; // set up the A/D parameters adc_init(); // set up the pwm pwm_init(); // set up the uart uart1_init(); // disable updates updatedisable = 0; // get some scaling out of the way, the maximum AD value is 1024 which should be mapped to the // maximum duty cycle scale = ((double) MAX_DUTY)/1024.0; r1 = 0; r2 = 0; p1 = 0; p2 = 0; encindex = 0; // initialize timer1 dt = 0.025; // the sampling inverval in seconds // dt = N*256/29,480,000; assuming a 256 prescalar. // so N = dt* 115156 period = (unsigned int) (dt*115156.0); period = min(period, 32768); // don't let the period get bigger than the counter Init_Timer1( period ); // set up the encoder encoder_init(); // enable the QEI interupt EnableIntQEI; // set up the parameter for the power amplifier dutycycle = (unsigned int)0; dutycyclereg = 3; SetDCMCPWM( dutycyclereg, dutycycle, updatedisable ); // duty cycle set to low // enable the output to control the motor TRISEbits.TRISE6 = 0; // IN1 TRISEbits.TRISE7 = 0; // IN2 PORTE = 0b01000000; // IN1 = 0, IN2 = 1; // set the scalinig from AD values to rad/sec AD_scale = 1.0; // estimated from multiple step responses, convert from A/D to rad/sec // set up the scaling to convert to radians per second speed_scale = (2*3.14159/720.0)/dt; // convert to radians/sec = 2*pi/720 counts per revolution // now just loop AD_value = 0; time = 0; while(1){ while(!GO ); GO = 0; time = time + dt; // get the speed in radians/sec speed = ((double)((r2-r1)*720+(p2-p1)))*speed_scale; R = (double) AD_value*AD_scale; // this is the reference input error = R; u = error; u = u*scale/AD_scale; // convert back to a pwm signal u = min(u,MAX_U); // don't let u get too large u = max(u,0.0); // don't let u get negative, since the motor cannot switch direction dutycycle = (unsigned int) u; dutycyclereg = 3; SetDCMCPWM( dutycyclereg, dutycycle, updatedisable); // use the following for determining the scaling printf("%8.4f %8.4f %6d %10.4f\n", time, R ,(int) dutycycle, speed ); // save the current positions r1 = r2; p1 = p2; } }