File content as of revision 0:e83b840a5f86:

#include "QEI.h"
#include "mbed.h"

QEI::QEI(PinName A, PinName B, int pulses, float t) :
		A_(A), B_(B) {
	count_ = 0;
	over_count_ = 0;
	oldcount_ = 0;
	speed_ = 0;
	oldspeed_ = 0;
	pulses_ = pulses * 4;
	revolutions_ = 0;
	t_ = t;

	x_.attach(this, &QEI::_sa, t_);

	A_.rise(this, &QEI::_AR);
	A_.fall(this, &QEI::_AF);
	B_.rise(this, &QEI::_BR);
	B_.fall(this, &QEI::_BF);
}

double QEI::read() {
	return angle();
}

void QEI::reset() {
	count_ = 0;
	over_count_ = 0;
	oldcount_ = 0;
}

void QEI::RevorutionCounter() {
	if (count_ == pulses_) {
		count_ = 0;
		revolutions_++;
	} else if (count_ < 0) {
		count_ = pulses_ - 1;
		revolutions_--;
	}

	if(over_count_ >= pulses_ * 20){
		over_count_ = pulses_ * 20;
	} else if (over_count_ <= -1.0 * pulses_ * 20) {
		over_count_ = -1.0 * pulses_ * 20;
	}

}

float QEI::over_angle(){
	return ((float) over_count_ / (float) pulses_);
}

int QEI::over_count(){
	return (over_count_);
}

int QEI::count() {
	return (count_);
}

int QEI::revolution() {
	return (revolutions_);
}

float QEI::angle() {
	return ((float) count_ / (float) pulses_);
}

void QEI::_sa() {
	oldspeed_ = speed_;
	speed_ = ((float) ((revolutions_ * pulses_) + count_) - oldcount_) / t_
			/ pulses_;
	acceleration_ = (speed_ - oldspeed_) / t_;
	oldcount_ = (revolutions_ * pulses_) + count_;
}

float QEI::speed() {
	return (speed_);
}

float QEI::acceleration() {
	return (acceleration_);
}

void QEI::_AR() {
	if (B_.read()) {
		count_--;
		over_count_--;
	} else if (!B_.read()) {
		count_++;
		over_count_++;
	}
	this->QEI::RevorutionCounter();
}

void QEI::_AF() {
	if (B_.read()) {
		count_++;
		over_count_++;
	} else if (!B_.read()) {
		count_--;
		over_count_--;
	}
	this->QEI::RevorutionCounter();
}
void QEI::_BR() {
	if (A_.read()) {
		count_++;
		over_count_++;
	} else if (!A_.read()) {
		count_--;
		over_count_--;
	}
	this->QEI::RevorutionCounter();
}
void QEI::_BF() {
	if (A_.read()) {
		count_--;
		over_count_--;
	} else if (!A_.read()) {
		count_++;
		over_count_++;
	}
	this->QEI::RevorutionCounter();
}