Yuta Uenodai / encoder

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   backdrive backdrive_3

encoder.cpp@6:36805e006b09, 2019-06-19 (annotated)

Committer:
takenowa
Date:
Wed Jun 19 01:46:13 2019 +0000
Revision:
6:36805e006b09
Parent:
5:32ee588c735d
Child:
7:1b81a9773bdf
0619(add return "count")

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takenowa 0:4b99060621fd 1 #include "encoder.h"
takenowa 0:4b99060621fd 2
takenowa 2:4bb7c8730d91 3 Encoder::Encoder(PinName a_phase , PinName b_phase ) : aPhase(a_phase) , bPhase(b_phase) {}
takenowa 0:4b99060621fd 4
takenowa 5:32ee588c735d 5 void Encoder::calculate(){
takenowa 0:4b99060621fd 6 //パルスの差分を更新
takenowa 1:c4643cba43a9 7 count[DELTA][ONE_BEFORE] = count[NORMAL][ONE_BEFORE] - count[NORMAL][TWO_BEFORE];
takenowa 3:6dce9d79da13 8 count[DELTA][CURRENT] = count[NORMAL][CURRENT] - count[NORMAL][ONE_BEFORE];
takenowa 0:4b99060621fd 9
takenowa 2:4bb7c8730d91 10 distance = count[NORMAL][CURRENT] * circle / ppr;
takenowa 2:4bb7c8730d91 11 rpm = (count[DELTA][CURRENT]/ppr) * 60 / dt;
takenowa 2:4bb7c8730d91 12 velocity = (count[DELTA][CURRENT]/ppr)*circle/dt;
takenowa 3:6dce9d79da13 13 //acceleration = (count[DELTA][CURRENT]-count[DELTA][ONE_BEFORE])/(dt_square*ppr) * circle;
takenowa 3:6dce9d79da13 14 //omega = (2*PI*count[DELTA][CURRENT])/(ppr*dt);
takenowa 2:4bb7c8730d91 15 delta_distance = (count[DELTA][CURRENT]/ppr)*circle;
takenowa 0:4b99060621fd 16 //1つ前・2つ前のデータを更新
takenowa 0:4b99060621fd 17 count[NORMAL][TWO_BEFORE] = count[NORMAL][ONE_BEFORE];
takenowa 2:4bb7c8730d91 18 count[NORMAL][ONE_BEFORE] = count[NORMAL][CURRENT];
takenowa 0:4b99060621fd 19 }
takenowa 0:4b99060621fd 20
takenowa 0:4b99060621fd 21 float Encoder::getState(int ch){
takenowa 0:4b99060621fd 22 float output;
takenowa 0:4b99060621fd 23 switch(ch){
takenowa 6:36805e006b09 24 case COUNT:
takenowa 6:36805e006b09 25 output = count[NORMAL][CURRENT];
takenowa 0:4b99060621fd 26 case DISTANCE:
takenowa 0:4b99060621fd 27 output = distance;
takenowa 0:4b99060621fd 28 break;
takenowa 0:4b99060621fd 29 case RPM:
takenowa 4:c99eb511ac08 30 output = rpm;
takenowa 0:4b99060621fd 31 break;
takenowa 0:4b99060621fd 32 case VELOCITY:
takenowa 0:4b99060621fd 33 output = velocity;
takenowa 0:4b99060621fd 34 break;
takenowa 0:4b99060621fd 35 case ACCELERATION:
takenowa 0:4b99060621fd 36 output = acceleration;
takenowa 0:4b99060621fd 37 break;
takenowa 0:4b99060621fd 38 case OMEGA:
takenowa 0:4b99060621fd 39 output = omega;
takenowa 0:4b99060621fd 40 break;
takenowa 0:4b99060621fd 41 case DELTA_DISTANCE:
takenowa 0:4b99060621fd 42 output = delta_distance;
takenowa 0:4b99060621fd 43 break;
takenowa 0:4b99060621fd 44 }
takenowa 0:4b99060621fd 45 return output;
takenowa 0:4b99060621fd 46 }
takenowa 0:4b99060621fd 47
takenowa 0:4b99060621fd 48 void Encoder::reset(){
takenowa 2:4bb7c8730d91 49 for(int i=CURRENT;i<BEFORE_NUMBER;i++){
takenowa 0:4b99060621fd 50 count[NORMAL][i] = 0;
takenowa 0:4b99060621fd 51 count[DELTA][i] = 0;
takenowa 0:4b99060621fd 52 }
takenowa 0:4b99060621fd 53 }
takenowa 0:4b99060621fd 54
takenowa 3:6dce9d79da13 55 void Encoder::readAphaseRise(){
takenowa 3:6dce9d79da13 56 if(bPhase){
takenowa 3:6dce9d79da13 57 count[NORMAL][CURRENT]--;
takenowa 3:6dce9d79da13 58 } else {
takenowa 3:6dce9d79da13 59 count[NORMAL][CURRENT]++;
takenowa 3:6dce9d79da13 60 }
takenowa 3:6dce9d79da13 61 }
takenowa 3:6dce9d79da13 62
takenowa 3:6dce9d79da13 63 void Encoder::readAphaseFall(){
takenowa 0:4b99060621fd 64 if(bPhase){
takenowa 2:4bb7c8730d91 65 count[NORMAL][CURRENT]++;
takenowa 0:4b99060621fd 66 } else {
takenowa 2:4bb7c8730d91 67 count[NORMAL][CURRENT]--;
takenowa 0:4b99060621fd 68 }
takenowa 0:4b99060621fd 69 }
takenowa 0:4b99060621fd 70
takenowa 4:c99eb511ac08 71 void Encoder::readBphaseRise(){
takenowa 4:c99eb511ac08 72 if(aPhase){
takenowa 4:c99eb511ac08 73 count[NORMAL][CURRENT]++;
takenowa 4:c99eb511ac08 74 } else {
takenowa 4:c99eb511ac08 75 count[NORMAL][CURRENT]--;
takenowa 4:c99eb511ac08 76 }
takenowa 4:c99eb511ac08 77 }
takenowa 4:c99eb511ac08 78
takenowa 4:c99eb511ac08 79 void Encoder::readBphaseFall(){
takenowa 4:c99eb511ac08 80 if(aPhase){
takenowa 4:c99eb511ac08 81 count[NORMAL][CURRENT]--;
takenowa 4:c99eb511ac08 82 } else {
takenowa 4:c99eb511ac08 83 count[NORMAL][CURRENT]++;
takenowa 4:c99eb511ac08 84 }
takenowa 4:c99eb511ac08 85 }
takenowa 4:c99eb511ac08 86
takenowa 4:c99eb511ac08 87
takenowa 3:6dce9d79da13 88 void Encoder::init(float _ppr, float _radius,float _dt,int mode = X1_ENCODE){
takenowa 0:4b99060621fd 89 ppr = _ppr;
takenowa 0:4b99060621fd 90 dt = _dt;
takenowa 0:4b99060621fd 91 dt_square = dt*dt;
takenowa 0:4b99060621fd 92 circle = PI*2.0f*_radius;
takenowa 0:4b99060621fd 93 radius = _radius;
takenowa 3:6dce9d79da13 94
takenowa 3:6dce9d79da13 95 switch(mode){
takenowa 3:6dce9d79da13 96 case X1_ENCODE:
takenowa 3:6dce9d79da13 97 aPhase.rise(this,&Encoder::readAphaseRise);
takenowa 3:6dce9d79da13 98 ppr = _ppr;
takenowa 3:6dce9d79da13 99 break;
takenowa 3:6dce9d79da13 100 case X2_ENCODE:
takenowa 3:6dce9d79da13 101 aPhase.rise(this,&Encoder::readAphaseRise);
takenowa 3:6dce9d79da13 102 aPhase.fall(this,&Encoder::readAphaseFall);
takenowa 3:6dce9d79da13 103 ppr = _ppr*2;
takenowa 3:6dce9d79da13 104 break;
takenowa 3:6dce9d79da13 105 case X4_ENCODE:
takenowa 4:c99eb511ac08 106 aPhase.rise(this,&Encoder::readAphaseRise);
takenowa 4:c99eb511ac08 107 aPhase.fall(this,&Encoder::readAphaseFall);
takenowa 4:c99eb511ac08 108 bPhase.rise(this,&Encoder::readBphaseRise);
takenowa 4:c99eb511ac08 109 bPhase.fall(this,&Encoder::readBphaseFall);
takenowa 3:6dce9d79da13 110 ppr = _ppr*4;
takenowa 3:6dce9d79da13 111 break;
takenowa 3:6dce9d79da13 112 }
takenowa 0:4b99060621fd 113 }