00001 /****************************************************************************** 00002 ** Copyright (C) 2007 Pablo Garcia 00003 ** 00004 ** This library is free software; you can redistribute it and/or 00005 ** modify it under the terms of the GNU Lesser General Public 00006 ** License as published by the Free Software Foundation; either 00007 ** version 2.1 of the License, or (at your option) any later version. 00008 00009 ** This library is distributed in the hope that it will be useful, 00010 ** but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00012 ** Lesser General Public License for more details. 00013 00014 ** You should have received a copy of the GNU Lesser General Public 00015 ** License along with this library; if not, write to the Free Software 00016 ** Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 00017 ******************************************************************************/ 00018 00019 00020 00021 // ---------------------------------------------------------------------------- 00022 // Includes 00023 // ---------------------------------------------------------------------------- 00024 #include "qep.h" 00025 #include "cpu.h" 00026 //#include "motorConstants.h" 00027 00028 00029 // ---------------------------------------------------------------------------- 00030 // Variables 00031 // ---------------------------------------------------------------------------- 00032 volatile struct QEP qep1; 00033 volatile struct QEP qep2; 00034 00035 // ---------------------------------------------------------------------------- 00036 // Functions declaration 00037 // ---------------------------------------------------------------------------- 00038 void InitQEP1(void(*interruptFCN)(void), Uint16 qep_us, Uint16 lines, Uint16 polePairs); 00039 void InitQEP2(void(*interruptFCN)(void), Uint16 qep_us, Uint16 lines, Uint16 polePairs); 00040 void InitEQep1Gpio(void); 00041 void InitEQep2Gpio(void); 00042 00043 00044 // ---------------------------------------------------------------------------- 00045 // Functions 00046 // ---------------------------------------------------------------------------- 00047 void InitQEP1(void(*interruptFCN)(void), Uint16 qep_us, Uint16 lines, Uint16 polePairs) 00048 { 00049 Uint16 PRRSC = 1; 00050 // if additional preescaler is needed --> QCAPCTL (spru790a.pdf, pag. 33) 00051 // PRRSC << EQep1Regs.QCAPCTL.bit.CCPS; 00052 // enable peripheral clock 00053 EALLOW; 00054 SysCtrlRegs.PCLKCR1.bit.EQEP1ENCLK=1; 00055 EDIS; 00056 // configure inputs to be used as A-B-Z encoder signals 00057 InitEQep1Gpio(); 00058 EQep1Regs.QDECCTL.bit.QSRC=00; // QEP quadrature count mode 00059 EQep1Regs.QEPCTL.bit.FREE_SOFT=2; // avoid simulation reset 00060 EQep1Regs.QUPRD = US2CLOCK(qep_us, PRRSC); // configure Unit Timer 00061 EQep1Regs.QEPCTL.bit.PCRM=00; // PCRM=00 mode - QPOSCNT reset on index event 00062 EQep1Regs.QEPCTL.bit.UTE=1; // Unit Timer Enable 00063 EQep1Regs.QCAPCTL.bit.UPPS=0; // Unit position event prescaler 00064 EQep1Regs.QEPCTL.bit.QCLM=1; // Latch on unit timer timeout 00065 EQep1Regs.QPOSCNT=0; // Reset counter 00066 EQep1Regs.QPOSMAX=(lines << 2) -1; // 4*lines -1 00067 if(interruptFCN != 0){ 00068 // TODO: Interrupts are not still fully configured. QEINT register must be 00069 // used in order to setup interrupts (spru790a, pag. 37) 00070 EALLOW; 00071 //EQep1Regs.QEINT.bit.IEL=1; // enable interrupt on index event 00072 EQep1Regs.QCLR.bit.INT=1; // clear all interrupt flags 00073 EQep1Regs.QCLR.bit.UTO=1; // clear interrupt flag 00074 EQep1Regs.QEINT.all=0; // disable all interrupts 00075 EQep1Regs.QEINT.bit.UTO=1; // enable interrupt on unit time out 00076 PieVectTable.EQEP1_INT = interruptFCN; // assign ISR 00077 EDIS; 00078 PieCtrlRegs.PIEIER5.bit.INTx1 = 1; // Enable EQEP1_INT in PIE 00079 IER |= M_INT5; // Enable CPU Interrupt 5 00080 } 00081 // store configuration 00082 qep1.lines = lines; 00083 qep1.polePairs = polePairs; 00084 qep1.lastPos = 0; 00085 qep1.theta_mech = 0; 00086 qep1.theta_elec = 0; 00087 qep1.w_mech = 0; 00088 qep1.w_elec = 0; 00089 qep1.w_elec_lpf = 0; 00090 // calculate Hz/lines for the velocity loop. In real time we calculate the 00091 // velocity as the difference in counts every time the Unit Timer reach its value 00092 // so, we have counts/unit_timer_interrupt. To get rad/s we appy this transformation 00093 qep1.wLPFden=_IQdiv(_IQ(CPUINHZ()/(EQep1Regs.QUPRD*PRRSC)),_IQ(lines)); 00094 EQep1Regs.QEPCTL.bit.QPEN=1; // QEP enable 00095 } 00096 00097 // ---------------------------------------------------------------------------- 00098 00099 void InitQEP2(void(*interruptFCN)(void), Uint16 qep_us, Uint16 lines, Uint16 polePairs) 00100 { 00101 Uint16 PRRSC = 1; 00102 // if additional preescaler is needed --> QCAPCTL (spru790a.pdf, pag. 33) 00103 //PRRSC << EQep2Regs.QCAPCTL.bit.CCPS; 00104 // enable clock 00105 EALLOW; 00106 SysCtrlRegs.PCLKCR1.bit.EQEP2ENCLK=1; 00107 EDIS; 00108 // configure inputs to be used as A-B-Z encoder signals 00109 InitEQep2Gpio(); 00110 EQep2Regs.QDECCTL.bit.QSRC=00; // QEP quadrature count mode 00111 EQep2Regs.QEPCTL.bit.FREE_SOFT=2; // avoid simulation reset 00112 EQep2Regs.QUPRD = US2CLOCK(qep_us, PRRSC); // configure Unit Timer 00113 EQep2Regs.QEPCTL.bit.PCRM=00; // PCRM=00 mode - QPOSCNT reset on index event 00114 EQep2Regs.QEPCTL.bit.UTE=1; // Unit Timer Enable 00115 EQep2Regs.QCAPCTL.bit.UPPS=0; // Unit position event prescaler 00116 EQep2Regs.QEPCTL.bit.QCLM=1; // Latch on unit timer timeout 00117 EQep2Regs.QPOSCNT=0; // Reset counter 00118 EQep2Regs.QPOSMAX=(lines << 2) -1; // 4*lines -1 00119 if(interruptFCN != 0){ 00120 // TODO: Interrupts are not still fully configured. QEINT register must be 00121 // used in order to setup interrupts (spru790a, pag. 37) 00122 EALLOW; 00123 //EQep2Regs.QEINT.bit.IEL=1; // enable interrupt on index event 00124 EQep2Regs.QCLR.bit.INT=1; // clear all interrupt flags 00125 EQep2Regs.QCLR.bit.UTO=1; // clear interrupt flag 00126 EQep2Regs.QEINT.all=0; // disable all interrupts 00127 EQep2Regs.QEINT.bit.UTO=1; // enable interrupt on unit time out 00128 PieVectTable.EQEP2_INT = interruptFCN; // ISR 00129 EDIS; 00130 PieCtrlRegs.PIEIER5.bit.INTx2 = 1; // Enable EQEP2_INT in PIE 00131 IER |= M_INT5; // Enable CPU Interrupt 5 00132 } 00133 // store configuration 00134 qep2.lines = lines; 00135 qep2.polePairs = polePairs; 00136 qep2.lastPos = 0; 00137 qep2.theta_mech = 0; 00138 qep2.theta_elec = 0; 00139 qep2.w_mech = 0; 00140 qep2.w_elec = 0; 00141 qep2.w_elec_lpf = 0; 00142 // calculate Hz/lines for the velocity loop. In real time we calculate the 00143 // velocity as the difference in counts every time the Unit Timer reach its value 00144 // so, we have counts/unit_timer_interrupt. To get rad/s we appy this transformation 00145 qep2.wLPFden=_IQdiv(_IQ(CPUINHZ()/(EQep2Regs.QUPRD*PRRSC)),_IQ(lines)); 00146 EQep2Regs.QEPCTL.bit.QPEN=1; // QEP enable 00147 } 00148 00149 // ---------------------------------------------------------------------------- 00150 00151 void InitEQep1Gpio(void) 00152 { 00153 EALLOW; 00154 00155 /* Enable internal pull-up for the selected pins */ 00156 // Pull-ups can be enabled or disabled by the user. 00157 // This will enable the pullups for the specified pins. 00158 // Comment out other unwanted lines. 00159 00160 GpioCtrlRegs.GPAPUD.bit.GPIO20 = 0; // Enable pull-up on GPIO20 (EQEP1A) 00161 GpioCtrlRegs.GPAPUD.bit.GPIO21 = 0; // Enable pull-up on GPIO21 (EQEP1B) 00162 //GpioCtrlRegs.GPAPUD.bit.GPIO22 = 0; // Enable pull-up on GPIO22 (EQEP1S) 00163 GpioCtrlRegs.GPAPUD.bit.GPIO23 = 0; // Enable pull-up on GPIO23 (EQEP1I) 00164 00165 00166 // Inputs are synchronized to SYSCLKOUT by default. 00167 // Comment out other unwanted lines. 00168 00169 GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 0; // Sync to SYSCLKOUT GPIO20 (EQEP1A) 00170 GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 0; // Sync to SYSCLKOUT GPIO21 (EQEP1B) 00171 //GpioCtrlRegs.GPAQSEL2.bit.GPIO22 = 0; // Sync to SYSCLKOUT GPIO22 (EQEP1S) 00172 GpioCtrlRegs.GPAQSEL2.bit.GPIO23 = 0; // Sync to SYSCLKOUT GPIO23 (EQEP1I) 00173 00174 /* Configure eQEP-1 pins using GPIO regs*/ 00175 // This specifies which of the possible GPIO pins will be eQEP1 functional pins. 00176 // Comment out other unwanted lines. 00177 00178 GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 1; // Configure GPIO20 as EQEP1A 00179 GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 1; // Configure GPIO21 as EQEP1B 00180 //GpioCtrlRegs.GPAMUX2.bit.GPIO22 = 1; // Configure GPIO22 as EQEP1S 00181 GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 1; // Configure GPIO23 as EQEP1I 00182 00183 00184 EDIS; 00185 } 00186 00187 // ---------------------------------------------------------------------------- 00188 00189 void InitEQep2Gpio(void) 00190 { 00191 EALLOW; 00192 00193 /* Enable internal pull-up for the selected pins */ 00194 // Pull-ups can be enabled or disabled by the user. 00195 // This will enable the pullups for the specified pins. 00196 // Comment out other unwanted lines. 00197 00198 GpioCtrlRegs.GPAPUD.bit.GPIO24 = 0; // Enable pull-up on GPIO24 (EQEP2A) 00199 GpioCtrlRegs.GPAPUD.bit.GPIO25 = 0; // Enable pull-up on GPIO25 (EQEP2B) 00200 GpioCtrlRegs.GPAPUD.bit.GPIO26 = 0; // Enable pull-up on GPIO26 (EQEP2I) 00201 //GpioCtrlRegs.GPAPUD.bit.GPIO27 = 0; // Enable pull-up on GPIO27 (EQEP2S) 00202 00203 // Inputs are synchronized to SYSCLKOUT by default. 00204 // Comment out other unwanted lines. 00205 00206 GpioCtrlRegs.GPAQSEL2.bit.GPIO24 = 0; // Sync to SYSCLKOUT GPIO24 (EQEP2A) 00207 GpioCtrlRegs.GPAQSEL2.bit.GPIO25 = 0; // Sync to SYSCLKOUT GPIO25 (EQEP2B) 00208 GpioCtrlRegs.GPAQSEL2.bit.GPIO26 = 0; // Sync to SYSCLKOUT GPIO26 (EQEP2I) 00209 //GpioCtrlRegs.GPAQSEL2.bit.GPIO27 = 0; // Sync to SYSCLKOUT GPIO27 (EQEP2S) 00210 00211 /* Configure eQEP-2 pins using GPIO regs*/ 00212 // This specifies which of the possible GPIO pins will be eQEP2 functional pins. 00213 // Comment out other unwanted lines. 00214 00215 GpioCtrlRegs.GPAMUX2.bit.GPIO24 = 2; // Configure GPIO24 as EQEP2A 00216 GpioCtrlRegs.GPAMUX2.bit.GPIO25 = 2; // Configure GPIO25 as EQEP2B 00217 GpioCtrlRegs.GPAMUX2.bit.GPIO26 = 2; // Configure GPIO26 as EQEP2I 00218 //GpioCtrlRegs.GPAMUX2.bit.GPIO27 = 2; // Configure GPIO27 as EQEP2S 00219 00220 00221 EDIS; 00222 } 00223 00224 // ---------------------------------------------------------------------------- 00225 00226 00227 //----------------------------------------------------------------------------- 00228 // End Of File 00229 //-----------------------------------------------------------------------------