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@7:1b81a9773bdf, 2019-06-23 (annotated)

Committer:
takenowa
Date:
Sun Jun 23 06:14:19 2019 +0000
Revision:
7:1b81a9773bdf
Parent:
6:36805e006b09 
0623

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 7:1b81a9773bdf 26 break;
takenowa 0:4b99060621fd 27 case DISTANCE:
takenowa 0:4b99060621fd 28 output = distance;
takenowa 0:4b99060621fd 29 break;
takenowa 0:4b99060621fd 30 case RPM:
takenowa 4:c99eb511ac08 31 output = rpm;
takenowa 0:4b99060621fd 32 break;
takenowa 0:4b99060621fd 33 case VELOCITY:
takenowa 0:4b99060621fd 34 output = velocity;
takenowa 0:4b99060621fd 35 break;
takenowa 0:4b99060621fd 36 case ACCELERATION:
takenowa 0:4b99060621fd 37 output = acceleration;
takenowa 0:4b99060621fd 38 break;
takenowa 0:4b99060621fd 39 case OMEGA:
takenowa 0:4b99060621fd 40 output = omega;
takenowa 0:4b99060621fd 41 break;
takenowa 0:4b99060621fd 42 case DELTA_DISTANCE:
takenowa 0:4b99060621fd 43 output = delta_distance;
takenowa 0:4b99060621fd 44 break;
takenowa 0:4b99060621fd 45 }
takenowa 0:4b99060621fd 46 return output;
takenowa 0:4b99060621fd 47 }
takenowa 0:4b99060621fd 48
takenowa 0:4b99060621fd 49 void Encoder::reset(){
takenowa 2:4bb7c8730d91 50 for(int i=CURRENT;i<BEFORE_NUMBER;i++){
takenowa 0:4b99060621fd 51 count[NORMAL][i] = 0;
takenowa 0:4b99060621fd 52 count[DELTA][i] = 0;
takenowa 0:4b99060621fd 53 }
takenowa 0:4b99060621fd 54 }
takenowa 0:4b99060621fd 55
takenowa 3:6dce9d79da13 56 void Encoder::readAphaseRise(){
takenowa 3:6dce9d79da13 57 if(bPhase){
takenowa 3:6dce9d79da13 58 count[NORMAL][CURRENT]--;
takenowa 3:6dce9d79da13 59 } else {
takenowa 3:6dce9d79da13 60 count[NORMAL][CURRENT]++;
takenowa 3:6dce9d79da13 61 }
takenowa 3:6dce9d79da13 62 }
takenowa 3:6dce9d79da13 63
takenowa 3:6dce9d79da13 64 void Encoder::readAphaseFall(){
takenowa 0:4b99060621fd 65 if(bPhase){
takenowa 2:4bb7c8730d91 66 count[NORMAL][CURRENT]++;
takenowa 0:4b99060621fd 67 } else {
takenowa 2:4bb7c8730d91 68 count[NORMAL][CURRENT]--;
takenowa 0:4b99060621fd 69 }
takenowa 0:4b99060621fd 70 }
takenowa 0:4b99060621fd 71
takenowa 4:c99eb511ac08 72 void Encoder::readBphaseRise(){
takenowa 4:c99eb511ac08 73 if(aPhase){
takenowa 4:c99eb511ac08 74 count[NORMAL][CURRENT]++;
takenowa 4:c99eb511ac08 75 } else {
takenowa 4:c99eb511ac08 76 count[NORMAL][CURRENT]--;
takenowa 4:c99eb511ac08 77 }
takenowa 4:c99eb511ac08 78 }
takenowa 4:c99eb511ac08 79
takenowa 4:c99eb511ac08 80 void Encoder::readBphaseFall(){
takenowa 4:c99eb511ac08 81 if(aPhase){
takenowa 4:c99eb511ac08 82 count[NORMAL][CURRENT]--;
takenowa 4:c99eb511ac08 83 } else {
takenowa 4:c99eb511ac08 84 count[NORMAL][CURRENT]++;
takenowa 4:c99eb511ac08 85 }
takenowa 4:c99eb511ac08 86 }
takenowa 4:c99eb511ac08 87
takenowa 4:c99eb511ac08 88
takenowa 3:6dce9d79da13 89 void Encoder::init(float _ppr, float _radius,float _dt,int mode = X1_ENCODE){
takenowa 0:4b99060621fd 90 ppr = _ppr;
takenowa 0:4b99060621fd 91 dt = _dt;
takenowa 0:4b99060621fd 92 dt_square = dt*dt;
takenowa 0:4b99060621fd 93 circle = PI*2.0f*_radius;
takenowa 0:4b99060621fd 94 radius = _radius;
takenowa 3:6dce9d79da13 95
takenowa 3:6dce9d79da13 96 switch(mode){
takenowa 3:6dce9d79da13 97 case X1_ENCODE:
takenowa 3:6dce9d79da13 98 aPhase.rise(this,&Encoder::readAphaseRise);
takenowa 3:6dce9d79da13 99 ppr = _ppr;
takenowa 3:6dce9d79da13 100 break;
takenowa 3:6dce9d79da13 101 case X2_ENCODE:
takenowa 3:6dce9d79da13 102 aPhase.rise(this,&Encoder::readAphaseRise);
takenowa 3:6dce9d79da13 103 aPhase.fall(this,&Encoder::readAphaseFall);
takenowa 3:6dce9d79da13 104 ppr = _ppr*2;
takenowa 3:6dce9d79da13 105 break;
takenowa 3:6dce9d79da13 106 case X4_ENCODE:
takenowa 4:c99eb511ac08 107 aPhase.rise(this,&Encoder::readAphaseRise);
takenowa 4:c99eb511ac08 108 aPhase.fall(this,&Encoder::readAphaseFall);
takenowa 4:c99eb511ac08 109 bPhase.rise(this,&Encoder::readBphaseRise);
takenowa 4:c99eb511ac08 110 bPhase.fall(this,&Encoder::readBphaseFall);
takenowa 3:6dce9d79da13 111 ppr = _ppr*4;
takenowa 3:6dce9d79da13 112 break;
takenowa 3:6dce9d79da13 113 }
takenowa 0:4b99060621fd 114 }