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 00034 //----------------------------------------------------------------------------- 00035 // Define 00036 //----------------------------------------------------------------------------- 00037 00038 #ifndef QEP_H 00039 #define QEP_H 00040 00041 // ---------------------------------------------------------------------------- 00042 // Includes 00043 // ---------------------------------------------------------------------------- 00044 00045 #include "TIMotorLIB.h" 00046 #include GENERATE_FLEX_INC(Device.h) 00047 #include "FilterAndRotations.h" 00048 #include "Utils.h" 00049 00052 struct QEP { 00053 volatile struct EQEP_REGS* eQEPRegs; 00054 Uint16 lines; 00055 Uint16 lastPos; 00056 Uint16 polePairs; 00057 _iq theta_mech; 00058 _iq theta_elec; 00059 _iq w_mech; 00060 _iq w_elec; 00061 //_iq w_elec_lpf; //! elec. velocity low pass filtered (rad/s) 00062 _iq lines2Hz; 00063 }; 00064 00065 extern volatile struct QEP qep1; 00066 extern volatile struct QEP qep2; 00067 00069 typedef enum { 00070 PCRM_IE, 00071 PCRM_MAX_POS, 00072 PCRM_FIE, 00073 PCRM_UTE 00074 } PCRM; 00075 00076 00077 //----------------------------------------------------------------------------- 00078 // Implemented Functions 00079 //----------------------------------------------------------------------------- 00080 00081 //----------------------------------------------------------------------------- 00088 //----------------------------------------------------------------------------- 00089 extern void ConfigureQEP1(void(*interruptFCN)(void), Uint16 qep_us, 00090 PCRM pcrm, Uint16 lines, Uint16 polePairs); 00091 00092 //----------------------------------------------------------------------------- 00099 //----------------------------------------------------------------------------- 00100 extern void ConfigureQEP2(void(*interruptFCN)(void), Uint16 qep_us, 00101 PCRM pcrm, Uint16 lines, Uint16 polePairs); 00102 00103 00104 //----------------------------------------------------------------------------- 00106 //----------------------------------------------------------------------------- 00107 extern void InitEQep1Gpio(void); 00108 00109 00110 //----------------------------------------------------------------------------- 00112 //----------------------------------------------------------------------------- 00113 extern void InitEQep2Gpio(void); 00114 00115 //----------------------------------------------------------------------------- 00118 //----------------------------------------------------------------------------- 00119 extern void SwapDirection(volatile struct QEP *qep); 00120 00121 //----------------------------------------------------------------------------- 00130 //----------------------------------------------------------------------------- 00131 static inline void QEP1VelocityAndPosition(void); 00132 00133 //----------------------------------------------------------------------------- 00142 //----------------------------------------------------------------------------- 00143 static inline void QEP2VelocityAndPosition(void); 00144 00145 //----------------------------------------------------------------------------- 00152 //----------------------------------------------------------------------------- 00153 static inline void QEPVelocityAndPosition(Uint16 newPos, Uint16 oldPos, 00154 Uint16 direction, volatile struct QEP* qep); 00155 00156 //----------------------------------------------------------------------------- 00157 00158 //----------------------------------------------------------------------------- 00161 //----------------------------------------------------------------------------- 00162 static inline void EnableQEP(volatile struct QEP* qep); 00163 00164 //----------------------------------------------------------------------------- 00166 //----------------------------------------------------------------------------- 00167 static inline void EnableQEP1(void); 00168 00169 //----------------------------------------------------------------------------- 00171 //----------------------------------------------------------------------------- 00172 static inline void EnableQEP2(void); 00173 00174 //----------------------------------------------------------------------------- 00177 //----------------------------------------------------------------------------- 00178 static inline void EqepPIEAck(volatile struct EQEP_REGS* eQEPRegs); 00179 00180 //----------------------------------------------------------------------------- 00183 //----------------------------------------------------------------------------- 00184 static inline void Eqep1PIEAck(void); 00185 00186 //----------------------------------------------------------------------------- 00189 //----------------------------------------------------------------------------- 00190 static inline void Eqep2PIEAck(void); 00191 00192 00193 //----------------------------------------------------------------------------- 00194 // Inline function implementation 00195 //----------------------------------------------------------------------------- 00196 00199 static inline void QEP1VelocityAndPosition(void) 00200 { 00201 // position loop 00202 Uint16 direction = EQep1Regs.QEPSTS.bit.QDF; // Motor direction: 0=CCW/reverse, 1=CW/forward 00203 Uint16 newPos = (Uint16)(EQep1Regs.QPOSCNT); // position counter 00204 Uint16 oldPos; 00205 //**** High Speed Calcultation using QEP Position counter ****// 00206 // Check unit Time out-event for speed calculation: (velocity loop under sampling) 00207 if(EQep1Regs.QFLG.bit.UTO==1){ // If unit timeout 00208 newPos=(Uint16)EQep1Regs.QPOSLAT; // Latched POSCNT value 00209 oldPos=qep1.lastPos; // Latched POSCNT value 00210 //oldPos=(Uint16)EQep1Regs.QPOSLAT; // Latched POSCNT value 00211 QEPVelocityAndPosition(newPos, oldPos, direction, &qep1); 00212 EQep1Regs.QCLR.bit.UTO=1; // Clear interrupt flag 00213 } 00214 // PABLO: This should be ONLY calculated within the bandwidth of the UTO 00215 /*qep1.theta_mech = _IQmpy(TWOPI,_IQ18toIQ(_IQ18(newPos)/(qep1.lines << 2))); 00216 WRAP2PI(qep1.theta_mech); 00217 qep1.theta_elec = qep1.theta_mech*qep1.polePairs; 00218 while (qep1.theta_elec>PI){ 00219 qep1.theta_elec=qep1.theta_elec-TWOPI; 00220 } 00221 while (qep1.theta_elec<-PI){ 00222 qep1.theta_elec=qep1.theta_elec+TWOPI; 00223 }*/ 00224 } 00225 00226 // ---------------------------------------------------------------------------- 00227 00228 static inline void QEP2VelocityAndPosition(void) 00229 { 00230 // position loop 00231 Uint16 direction = EQep2Regs.QEPSTS.bit.QDF;// Motor direction: 0=CCW/reverse, 1=CW/forward 00232 Uint16 newPos = (Uint16)(EQep2Regs.QPOSCNT); // position counter 00233 Uint16 oldPos; 00234 // velocity loop under sampling 00235 if(EQep2Regs.QFLG.bit.UTO==1){ // If unit timeout 00236 newPos=(Uint16)EQep2Regs.QPOSLAT; // Latched POSCNT value 00237 oldPos=qep2.lastPos; // Latched POSCNT value 00238 //oldPos=(Uint16)EQep2Regs.QPOSLAT; // Latched POSCNT value 00239 QEPVelocityAndPosition(newPos, oldPos, direction, &qep2); 00240 EQep2Regs.QCLR.bit.UTO=1; // Clear interrupt flag 00241 } 00242 // PABLO: This should be ONLY calculated within the bandwidth of the UTO 00243 /*qep2.theta_mech = _IQmpy(TWOPI,_IQ18toIQ(_IQ18(newPos)/(qep2.lines << 2))); 00244 WRAP2PI(qep2.theta_mech); 00245 qep2.theta_elec = qep2.theta_mech*qep2.polePairs; 00246 while (qep2.theta_elec>PI){ 00247 qep2.theta_elec=qep2.theta_elec-TWOPI; 00248 } 00249 while (qep2.theta_elec<-PI){ 00250 qep2.theta_elec=qep2.theta_elec+TWOPI; 00251 }*/ 00252 } 00253 00254 // ---------------------------------------------------------------------------- 00255 00256 static inline void QEPVelocityAndPosition(Uint16 newPos, Uint16 oldPos, 00257 Uint16 direction, volatile struct QEP* qep) 00258 { 00259 int16 diff_pulses; 00260 //GpioDataRegs.GPATOGGLE.bit.GPIO18 = 1; 00261 if(direction==0){ // counting down 00262 if(newPos>oldPos) // counter reset 00263 diff_pulses = newPos-oldPos-(qep->lines<<2); 00264 else 00265 diff_pulses = newPos-oldPos; 00266 } 00267 else if(direction==1){ // counting up 00268 if(newPos<oldPos) // counter reset 00269 diff_pulses = newPos-oldPos+(qep->lines<<2); 00270 else 00271 diff_pulses = newPos-oldPos; 00272 } 00273 qep->w_mech = _IQmpy(TWOPI,_IQmpy(_IQ(diff_pulses),qep->lines2Hz)); 00274 qep->w_elec = qep->w_mech*qep->polePairs; 00275 // low-pass filter velocity estimation 00276 // PABLO: This filter need to be tested and make it configurable. 00277 // Also, variables qepConf->w_elec and qepConf->w_elec_lpf MUST have memory 00278 //FILTER_LPF(qep->w_elec,qep->w_elec_lpf,_IQ(0.03046),_IQ(0.93908)); 00279 qep->theta_mech = _IQmpy(TWOPI,_IQ18toIQ(_IQ18(newPos)/(qep->lines << 2))); 00280 WRAP2PI(qep->theta_mech); 00281 qep->theta_elec = qep->theta_mech*qep->polePairs; 00282 while (qep->theta_elec>PI){ 00283 qep->theta_elec=qep->theta_elec-TWOPI; 00284 } 00285 while (qep->theta_elec<-PI){ 00286 qep->theta_elec=qep->theta_elec+TWOPI; 00287 } 00288 // update values 00289 qep->lastPos = newPos; 00290 } 00291 00292 // ---------------------------------------------------------------------------- 00293 00294 static inline void EnableQEP(volatile struct QEP* qep) 00295 { 00296 qep->eQEPRegs->QEPCTL.bit.QPEN=1; // QEP enable 00297 } 00298 00299 //----------------------------------------------------------------------------- 00300 00301 static inline void EnableQEP1(void) 00302 { 00303 EnableQEP(&qep1); // QEP enable 00304 } 00305 00306 //----------------------------------------------------------------------------- 00307 00308 static inline void EnableQEP2(void) 00309 { 00310 EnableQEP(&qep2); // QEP enable 00311 } 00312 00313 //----------------------------------------------------------------------------- 00314 00315 static inline void EqepPIEAck(volatile struct EQEP_REGS* eQEPRegs) 00316 { 00317 // Acknowledge this interrupt to receive more interrupts from group 5 00318 PieCtrlRegs.PIEACK.all |= PIEACK_GROUP5; 00319 // clear all interrupt flags 00320 eQEPRegs->QCLR.bit.INT=1; 00321 // clear interrupt flag 00322 eQEPRegs->QCLR.bit.UTO=1; 00323 } 00324 00325 //----------------------------------------------------------------------------- 00326 00327 static inline void Eqep1PIEAck(void) 00328 { 00329 EqepPIEAck(qep1.eQEPRegs); 00330 } 00331 00332 //----------------------------------------------------------------------------- 00333 00334 static inline void Eqep2PIEAck(void) 00335 { 00336 EqepPIEAck(qep2.eQEPRegs); 00337 } 00338 00339 //----------------------------------------------------------------------------- 00340 00341 #endif 00342 00343 //----------------------------------------------------------------------------- 00344 // End Of File 00345 //----------------------------------------------------------------------------- 00346