Source file: /~heha/ewa/Reluktanzmotor/maweig-Motor-200831.zip/qep.h

#pragma once
// Include "Settings.h" first

inline float frac(float x) {return x-int(x);}

class QEP{	// Quadrature Encoder Driver object
public:
  inline float getElecTheta() const{	// Output: Motor Electrical Angle (0..<1)
	return frac(PolePairs*MechTheta);
  }
  inline float getMechTheta() const{	// Output: Motor Mechanical Angle (0..<1)
	return MechTheta;
  }
  inline bool getDirection() const{	// Output: Motor rotation direction
	return regs.QEPSTS.bit.QDF;
  }
private:
//  Uint16 QepPeriod;	// Output: Capture period of QEP signal in number of EQEP capture timer (QCTMR) period
//  Uint32 QepCountIndex;	// Variable: Encoder counter index
  float MechScaler;	// Parameter: 1/lines
  int32_t LineEncoder;	// Constant: Lines of encoder × 4
  uint16_t PolePairs;	// Parameter: Number of pole pairs
  float MechTheta;	// Output: Motor Mechanical Angle
  volatile EQEP_REGS&regs;

public:
  inline void init(int32_t a4i=0, bool invert=false) {	// was QEP_INIT_MACRO(m,v)
    regs.QEPCTL.all = 0x821E;	// FREE_SOFT[1]|IEI[1]|IEL[0]|QPEM|QLCM|UTE
    regs.QUPRD = F_CPU/100;	// Unit Timer for 100 Hz
    regs.QCAPCTL.all = 0x8075;	// CEN|CCPS[2:0]|UPPS[2,0]
    regs.QPOSINIT = a4i;	// set index marker's angle (for rising edge)
    if (invert) regs.QDECCTL.bit.SWAP = 1;
// Unit Position Event Prescaler = 32,
// capture timer clock prescaler = 128
    regs.QPOSMAX = LineEncoder-1;
  }

  inline void resetPos() {
    regs.QCLR.bit.IEL = 1;	// Absolute position is now unknown
    //setPos();
  }
  
  inline int32_t getPos() const {return regs.QPOSCNT;}

  inline void setPos(int32_t newpos=0, bool relative=false, bool moveReference=false) {
    regs.QEPCTL.bit.QPEN = 0;	// don't count while twiddeling on position
    int32_t diff = newpos;
    if (relative) newpos+=regs.QPOSCNT;
    else diff-=regs.QPOSCNT;
    regs.QPOSCNT = newpos;
    regs.QEPCTL.bit.QPEN = 1;	// now continue counting
    if (moveReference) {
      int32_t n = regs.QPOSINIT + diff;
      int32_t lines = regs.QPOSMAX+1;
      while (n<0) n+=lines;
      while (n>=lines) n-=lines;
      regs.QPOSINIT = n;
    }
  }

  inline bool isAbsolute() const {return regs.QFLG.bit.IEL;}

  inline void operator() () {	// was QEP_MACRO(m,v)
// Compute the mechanical angle
    MechTheta = MechScaler*regs.QPOSCNT;
// Check unit Time out-event for speed calculation:
// Unit Timer is configured for 100Hz in INIT function
    if (regs.QFLG.bit.UTO) {
// Low Speed Calculation	— Capture Counter overflowed, hence do no compute speed
      if (regs.QEPSTS.bit.COEF || regs.QEPSTS.bit.CDEF) regs.QEPSTS.all = 0x000C;
//      else if (regs.QCPRDLAT!=0xffff) QepPeriod = regs.QCPRDLAT;
    }	// Compute lowspeed using capture counter value
  }

  QEP(volatile EQEP_REGS&r,Uint16 polepairs,Uint16 lines,float gear=1):
    regs(r),
    PolePairs(polepairs),
    LineEncoder(lines<<2),
    MechScaler(gear/(lines<<2))	// gear = (optionales) Getriebe
	{}
};