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@8:dd1732f51780, 2019-05-03 (annotated)

Committer:
takenowa
Date:
Fri May 03 08:36:12 2019 +0000
Revision:
8:dd1732f51780
Parent:
4:c99eb511ac08 
removed divide calculate

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