Diff: lib/QEI.cpp

Revision:

0:e83b840a5f86

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/QEI.cpp	Tue Sep 18 03:11:01 2018 +0000
@@ -0,0 +1,126 @@
+#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();
+}