taiyou komazawa / Mbed 2 deprecated Nucleo_fliping_arm

2018 HongoMechaTech A

Dependencies:   mbed

lib/QEI.cpp@0:e83b840a5f86, 2018-09-18 (annotated)

Committer:
Komazawa_sun
Date:
Tue Sep 18 03:11:01 2018 +0000
Revision:
0:e83b840a5f86
????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Komazawa_sun 0:e83b840a5f86 1 #include "QEI.h"
Komazawa_sun 0:e83b840a5f86 2 #include "mbed.h"
Komazawa_sun 0:e83b840a5f86 3
Komazawa_sun 0:e83b840a5f86 4 QEI::QEI(PinName A, PinName B, int pulses, float t) :
Komazawa_sun 0:e83b840a5f86 5 A_(A), B_(B) {
Komazawa_sun 0:e83b840a5f86 6 count_ = 0;
Komazawa_sun 0:e83b840a5f86 7 over_count_ = 0;
Komazawa_sun 0:e83b840a5f86 8 oldcount_ = 0;
Komazawa_sun 0:e83b840a5f86 9 speed_ = 0;
Komazawa_sun 0:e83b840a5f86 10 oldspeed_ = 0;
Komazawa_sun 0:e83b840a5f86 11 pulses_ = pulses * 4;
Komazawa_sun 0:e83b840a5f86 12 revolutions_ = 0;
Komazawa_sun 0:e83b840a5f86 13 t_ = t;
Komazawa_sun 0:e83b840a5f86 14
Komazawa_sun 0:e83b840a5f86 15 x_.attach(this, &QEI::_sa, t_);
Komazawa_sun 0:e83b840a5f86 16
Komazawa_sun 0:e83b840a5f86 17 A_.rise(this, &QEI::_AR);
Komazawa_sun 0:e83b840a5f86 18 A_.fall(this, &QEI::_AF);
Komazawa_sun 0:e83b840a5f86 19 B_.rise(this, &QEI::_BR);
Komazawa_sun 0:e83b840a5f86 20 B_.fall(this, &QEI::_BF);
Komazawa_sun 0:e83b840a5f86 21 }
Komazawa_sun 0:e83b840a5f86 22
Komazawa_sun 0:e83b840a5f86 23 double QEI::read() {
Komazawa_sun 0:e83b840a5f86 24 return angle();
Komazawa_sun 0:e83b840a5f86 25 }
Komazawa_sun 0:e83b840a5f86 26
Komazawa_sun 0:e83b840a5f86 27 void QEI::reset() {
Komazawa_sun 0:e83b840a5f86 28 count_ = 0;
Komazawa_sun 0:e83b840a5f86 29 over_count_ = 0;
Komazawa_sun 0:e83b840a5f86 30 oldcount_ = 0;
Komazawa_sun 0:e83b840a5f86 31 }
Komazawa_sun 0:e83b840a5f86 32
Komazawa_sun 0:e83b840a5f86 33 void QEI::RevorutionCounter() {
Komazawa_sun 0:e83b840a5f86 34 if (count_ == pulses_) {
Komazawa_sun 0:e83b840a5f86 35 count_ = 0;
Komazawa_sun 0:e83b840a5f86 36 revolutions_++;
Komazawa_sun 0:e83b840a5f86 37 } else if (count_ < 0) {
Komazawa_sun 0:e83b840a5f86 38 count_ = pulses_ - 1;
Komazawa_sun 0:e83b840a5f86 39 revolutions_--;
Komazawa_sun 0:e83b840a5f86 40 }
Komazawa_sun 0:e83b840a5f86 41
Komazawa_sun 0:e83b840a5f86 42 if(over_count_ >= pulses_ * 20){
Komazawa_sun 0:e83b840a5f86 43 over_count_ = pulses_ * 20;
Komazawa_sun 0:e83b840a5f86 44 } else if (over_count_ <= -1.0 * pulses_ * 20) {
Komazawa_sun 0:e83b840a5f86 45 over_count_ = -1.0 * pulses_ * 20;
Komazawa_sun 0:e83b840a5f86 46 }
Komazawa_sun 0:e83b840a5f86 47
Komazawa_sun 0:e83b840a5f86 48 }
Komazawa_sun 0:e83b840a5f86 49
Komazawa_sun 0:e83b840a5f86 50 float QEI::over_angle(){
Komazawa_sun 0:e83b840a5f86 51 return ((float) over_count_ / (float) pulses_);
Komazawa_sun 0:e83b840a5f86 52 }
Komazawa_sun 0:e83b840a5f86 53
Komazawa_sun 0:e83b840a5f86 54 int QEI::over_count(){
Komazawa_sun 0:e83b840a5f86 55 return (over_count_);
Komazawa_sun 0:e83b840a5f86 56 }
Komazawa_sun 0:e83b840a5f86 57
Komazawa_sun 0:e83b840a5f86 58 int QEI::count() {
Komazawa_sun 0:e83b840a5f86 59 return (count_);
Komazawa_sun 0:e83b840a5f86 60 }
Komazawa_sun 0:e83b840a5f86 61
Komazawa_sun 0:e83b840a5f86 62 int QEI::revolution() {
Komazawa_sun 0:e83b840a5f86 63 return (revolutions_);
Komazawa_sun 0:e83b840a5f86 64 }
Komazawa_sun 0:e83b840a5f86 65
Komazawa_sun 0:e83b840a5f86 66 float QEI::angle() {
Komazawa_sun 0:e83b840a5f86 67 return ((float) count_ / (float) pulses_);
Komazawa_sun 0:e83b840a5f86 68 }
Komazawa_sun 0:e83b840a5f86 69
Komazawa_sun 0:e83b840a5f86 70 void QEI::_sa() {
Komazawa_sun 0:e83b840a5f86 71 oldspeed_ = speed_;
Komazawa_sun 0:e83b840a5f86 72 speed_ = ((float) ((revolutions_ * pulses_) + count_) - oldcount_) / t_
Komazawa_sun 0:e83b840a5f86 73 / pulses_;
Komazawa_sun 0:e83b840a5f86 74 acceleration_ = (speed_ - oldspeed_) / t_;
Komazawa_sun 0:e83b840a5f86 75 oldcount_ = (revolutions_ * pulses_) + count_;
Komazawa_sun 0:e83b840a5f86 76 }
Komazawa_sun 0:e83b840a5f86 77
Komazawa_sun 0:e83b840a5f86 78 float QEI::speed() {
Komazawa_sun 0:e83b840a5f86 79 return (speed_);
Komazawa_sun 0:e83b840a5f86 80 }
Komazawa_sun 0:e83b840a5f86 81
Komazawa_sun 0:e83b840a5f86 82 float QEI::acceleration() {
Komazawa_sun 0:e83b840a5f86 83 return (acceleration_);
Komazawa_sun 0:e83b840a5f86 84 }
Komazawa_sun 0:e83b840a5f86 85
Komazawa_sun 0:e83b840a5f86 86 void QEI::_AR() {
Komazawa_sun 0:e83b840a5f86 87 if (B_.read()) {
Komazawa_sun 0:e83b840a5f86 88 count_--;
Komazawa_sun 0:e83b840a5f86 89 over_count_--;
Komazawa_sun 0:e83b840a5f86 90 } else if (!B_.read()) {
Komazawa_sun 0:e83b840a5f86 91 count_++;
Komazawa_sun 0:e83b840a5f86 92 over_count_++;
Komazawa_sun 0:e83b840a5f86 93 }
Komazawa_sun 0:e83b840a5f86 94 this->QEI::RevorutionCounter();
Komazawa_sun 0:e83b840a5f86 95 }
Komazawa_sun 0:e83b840a5f86 96
Komazawa_sun 0:e83b840a5f86 97 void QEI::_AF() {
Komazawa_sun 0:e83b840a5f86 98 if (B_.read()) {
Komazawa_sun 0:e83b840a5f86 99 count_++;
Komazawa_sun 0:e83b840a5f86 100 over_count_++;
Komazawa_sun 0:e83b840a5f86 101 } else if (!B_.read()) {
Komazawa_sun 0:e83b840a5f86 102 count_--;
Komazawa_sun 0:e83b840a5f86 103 over_count_--;
Komazawa_sun 0:e83b840a5f86 104 }
Komazawa_sun 0:e83b840a5f86 105 this->QEI::RevorutionCounter();
Komazawa_sun 0:e83b840a5f86 106 }
Komazawa_sun 0:e83b840a5f86 107 void QEI::_BR() {
Komazawa_sun 0:e83b840a5f86 108 if (A_.read()) {
Komazawa_sun 0:e83b840a5f86 109 count_++;
Komazawa_sun 0:e83b840a5f86 110 over_count_++;
Komazawa_sun 0:e83b840a5f86 111 } else if (!A_.read()) {
Komazawa_sun 0:e83b840a5f86 112 count_--;
Komazawa_sun 0:e83b840a5f86 113 over_count_--;
Komazawa_sun 0:e83b840a5f86 114 }
Komazawa_sun 0:e83b840a5f86 115 this->QEI::RevorutionCounter();
Komazawa_sun 0:e83b840a5f86 116 }
Komazawa_sun 0:e83b840a5f86 117 void QEI::_BF() {
Komazawa_sun 0:e83b840a5f86 118 if (A_.read()) {
Komazawa_sun 0:e83b840a5f86 119 count_--;
Komazawa_sun 0:e83b840a5f86 120 over_count_--;
Komazawa_sun 0:e83b840a5f86 121 } else if (!A_.read()) {
Komazawa_sun 0:e83b840a5f86 122 count_++;
Komazawa_sun 0:e83b840a5f86 123 over_count_++;
Komazawa_sun 0:e83b840a5f86 124 }
Komazawa_sun 0:e83b840a5f86 125 this->QEI::RevorutionCounter();
Komazawa_sun 0:e83b840a5f86 126 }