#include "Settings.h"
#include "qep.h"
namespace nsRESOLVER{
const int POLES = 8; // Number of poles
}
// Variables for Position Sensor Suite
float posEncElecTheta; // Inhalt geht an MotorControl.cpp
float posEncMechTheta;
Uint16 lsw;
// Instance a QEP interface driver
static QEP qep( // C++ constructor
EQep1Regs, // Hardware-Registersatz
nsRESOLVER::POLES/2, // (elektrische) Polpaarzahl
512); // Striche pro Umdrehung
// ***************************************************************
// Periodic action, called from MotorControl ISR at some runlevels
// - Reads QEP
// - Decodes RESOLVER (core algo is available in resolver.lib)
// - Angles are in range 0.0 to < 1.0
// ***************************************************************
void posEncoderSuite() {
if (ENCODERS&1<<QEP_ENCODER) {
// ----------------------------------
// lsw = 0 ---> Alignment Routine
// ----------------------------------
if (!lsw) qep.resetPos(); // remove known absolute position
// ******************************************************************************
// Detect calibration angle and call the QEP module
// ******************************************************************************
// for once the QEP index pulse is found, go to lsw=2
if (lsw==1 && qep.isAbsolute()) ++lsw; // Check the index occurrence
// Keep the latched pos. at the first index
if (lsw) qep();
posEncElecTheta = qep.getElecTheta();
posEncMechTheta = qep.getMechTheta();
}
if (ENCODERS&bit(SINCOS_ENCODER)) {
// TODO: Position ermitteln
}
}
void initPosEncoderSuite() {
if (ENCODERS&1<<QEP_ENCODER) {
// Eingänge zuweisen
GpioCtrlRegs.GPAMUX2.all |= 1UL<<8 | 1UL<<10; // GPIO20 und GPIO21 an QEP1
GpioCtrlRegs.GPDGMUX1.all |= 1UL<<6;
GpioCtrlRegs.GPDMUX1.all |= 1UL<<6; // GPIO99 an QEP1
GpioCtrlRegs.GPBGMUX2.all |= 1UL<<12 | 1UL<<14 | 1UL<<18;
GpioCtrlRegs.GPBMUX2.all |= 1UL<<12 | 1UL<<14 | 1UL<<18; // GPIO54,55,57 an QEP2
qep.init(0,true); // Init QEP parameters
}
if (ENCODERS&bit(SINCOS_ENCODER)) {
// TODO: A/D-Wandler zuweisen usw.
}
}
Detected encoding: UTF-8 | 0
|