FtEncoder.cpp

00001 #include "FtEncoder.h"
00002 
00003 FtEncoder::FtEncoder(PinName pwm, unsigned int standStillTimeout)
00004     :m_encIRQ(pwm),
00005      m_cnt(0),
00006      m_standStillTimeout(standStillTimeout),
00007      m_cursor(0)
00008 {
00009     // initialize the edge timestamp ringbuffer
00010     unsigned int tmStmp = getCurrentTimeStamp() - c_nTmStmps * m_standStillTimeout;
00011     for(int i=0; i<c_nTmStmps; ++i)
00012         m_timeStamps[i] = tmStmp + i*m_standStillTimeout;
00013     m_cursor = c_nTmStmps-1;
00014 
00015     m_encIRQ.mode(PullUp); // set it to pullup, but an external pullup is needed, the internal isn't sufficient
00016     m_encIRQ.rise(this, &FtEncoder::encoderISR);
00017     m_encIRQ.fall(this, &FtEncoder::encoderISR);
00018     m_tmOut.attach_us(this, &FtEncoder::timeoutISR, m_standStillTimeout);
00019 }
00020 
00021 unsigned int FtEncoder::getLastPeriod() const
00022 {
00023     unsigned int period = 0;
00024     unsigned int cursor, cnt;
00025     do {
00026         cursor = m_cursor;
00027         cnt = m_cnt;
00028         period = m_timeStamps[m_cursor] - m_timeStamps[(m_cursor-2)&(c_nTmStmps-1)];
00029     } while(cursor!=m_cursor || cnt!=m_cnt); // stay in loop until we have a non intrrupted reading
00030     return period;
00031 }
00032 
00033 float FtEncoder::getSpeed() const
00034 {
00035     unsigned int period = getLastPeriod();
00036     return (period!=0 && period<m_standStillTimeout) ?
00037            c_speedFactor/period : 0.0;
00038 }
00039 
00040 void FtEncoder::encoderISR()
00041 {
00042     ++m_cursor;
00043     m_cursor &= c_nTmStmps-1;
00044     m_timeStamps[m_cursor] = getCurrentTimeStamp();
00045     // restart the timeout
00046     m_tmOut.remove();
00047     m_tmOut.insert(m_timeStamps[m_cursor]+m_standStillTimeout);
00048     ++m_cnt;
00049     m_callBack.call();
00050 }
00051 
00052 void FtEncoder::timeoutISR(){
00053     --m_cnt;
00054     encoderISR();
00055 }