ロボステ6期 / Encoder_

ai

Dependents:   ball_snatch NHK2020-arm-sub NHK2020-arm-sub NHK2020-arm-sub2 ... more

EC.cpp@13:9a21d4def4c6, 2019-11-09 (annotated)

Committer:
aoikoizumi
Date:
Sat Nov 09 06:53:58 2019 +0000
Revision:
13:9a21d4def4c6
Parent:
11:acbbd742df47 
20191109

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MazeTaka 0:eb20667a5a27 1 #include "mbed.h"
MazeTaka 0:eb20667a5a27 2 #include "EC.h"
MazeTaka 0:eb20667a5a27 3
MazeTaka 0:eb20667a5a27 4
MazeTaka 0:eb20667a5a27 5 Ec::Ec(int res,int multi):
MazeTaka 0:eb20667a5a27 6 count_(0),pre_count_(0),resolution_(res),multiplication_(multi)
MazeTaka 0:eb20667a5a27 7 {
MazeTaka 0:eb20667a5a27 8 timer_.start();
MazeTaka 0:eb20667a5a27 9 }
MazeTaka 0:eb20667a5a27 10
MazeTaka 8:17aff8f21d19 11 int Ec::getCount()const
MazeTaka 0:eb20667a5a27 12 {
MazeTaka 0:eb20667a5a27 13 return count_;
aoikoizumi 13:9a21d4def4c6 14 //return pre_count_;
MazeTaka 0:eb20667a5a27 15 }
MazeTaka 0:eb20667a5a27 16
MazeTaka 11:acbbd742df47 17 double Ec::getRad()const
MazeTaka 10:ad0f13db7b92 18 {
MazeTaka 10:ad0f13db7b92 19 return count_*2.0f*M_PI/(multiplication_*resolution_);
MazeTaka 10:ad0f13db7b92 20 }
MazeTaka 10:ad0f13db7b92 21
MazeTaka 0:eb20667a5a27 22 void Ec::calOmega()
MazeTaka 0:eb20667a5a27 23 {
MazeTaka 0:eb20667a5a27 24 double t=timer_.read();
MazeTaka 0:eb20667a5a27 25 omega_=(count_-pre_count_)*2.0f*M_PI/(multiplication_*resolution_*(t-ptw_));
aoikoizumi 13:9a21d4def4c6 26 // omega_=(getCount()-pre_count_)*2.0f*M_PI/(multiplication_*resolution_*(t-ptw_));
aoikoizumi 13:9a21d4def4c6 27 //pre_count_=getCount();
MazeTaka 0:eb20667a5a27 28 pre_count_=count_;
MazeTaka 0:eb20667a5a27 29 ptw_=t;
MazeTaka 0:eb20667a5a27 30 }
MazeTaka 0:eb20667a5a27 31
MazeTaka 8:17aff8f21d19 32 double Ec::getOmega()const
MazeTaka 0:eb20667a5a27 33 {
MazeTaka 0:eb20667a5a27 34 return omega_;
MazeTaka 0:eb20667a5a27 35 }
MazeTaka 10:ad0f13db7b92 36 void Ec::setResolution(int res)
MazeTaka 10:ad0f13db7b92 37 {
MazeTaka 0:eb20667a5a27 38 resolution_=res;
MazeTaka 0:eb20667a5a27 39 }
MazeTaka 0:eb20667a5a27 40
MazeTaka 0:eb20667a5a27 41 /*reset関数の定義*/
MazeTaka 0:eb20667a5a27 42 /*エンコーダを初期状態に戻すことができる*/
MazeTaka 0:eb20667a5a27 43 void Ec::reset()
MazeTaka 0:eb20667a5a27 44 {
MazeTaka 0:eb20667a5a27 45 count_=0;
MazeTaka 0:eb20667a5a27 46 pre_count_=0,omega_=0;
MazeTaka 0:eb20667a5a27 47 ptw_=0;
MazeTaka 1:b7e957c01761 48 timer_.stop();
MazeTaka 1:b7e957c01761 49 timer_.reset();
MazeTaka 1:b7e957c01761 50 timer_.start();
MazeTaka 0:eb20667a5a27 51 }
MazeTaka 0:eb20667a5a27 52
MazeTaka 0:eb20667a5a27 53
MazeTaka 0:eb20667a5a27 54 ////////////////////////////////////////////////////1逓倍//////////////////////////////////////////////////////////////////
MazeTaka 0:eb20667a5a27 55 Ec1multi::Ec1multi(PinName signalA,PinName signalB,int res) : Ec(res,1),signalA_(signalA),signalB_(signalB)
MazeTaka 0:eb20667a5a27 56 {
MazeTaka 0:eb20667a5a27 57 signalA_.rise(callback(this,&Ec1multi::upA));
MazeTaka 0:eb20667a5a27 58 }
MazeTaka 0:eb20667a5a27 59
MazeTaka 0:eb20667a5a27 60 //ピン変化割り込み関数の定義
MazeTaka 0:eb20667a5a27 61 void Ec1multi::upA()
MazeTaka 0:eb20667a5a27 62 {
MazeTaka 0:eb20667a5a27 63 if(signalB_.read())count_++;
MazeTaka 0:eb20667a5a27 64 else count_--;
MazeTaka 0:eb20667a5a27 65 }
MazeTaka 0:eb20667a5a27 66 ////////////////////////////////////////////////////2逓倍//////////////////////////////////////////////////////////////////
MazeTaka 0:eb20667a5a27 67 Ec2multi::Ec2multi(PinName signalA,PinName signalB,int res) : Ec(res,2),signalA_(signalA),signalB_(signalB)
MazeTaka 0:eb20667a5a27 68 {
MazeTaka 0:eb20667a5a27 69 signalA_.rise(callback(this,&Ec2multi::upA));
MazeTaka 0:eb20667a5a27 70 signalA_.fall(callback(this,&Ec2multi::downA));
MazeTaka 0:eb20667a5a27 71 }
MazeTaka 0:eb20667a5a27 72
MazeTaka 0:eb20667a5a27 73 //ピン変化割り込み関数の定義
MazeTaka 0:eb20667a5a27 74 void Ec2multi::upA()
MazeTaka 0:eb20667a5a27 75 {
MazeTaka 0:eb20667a5a27 76 if(signalB_.read())count_++;
MazeTaka 0:eb20667a5a27 77 else count_--;
MazeTaka 0:eb20667a5a27 78 }
MazeTaka 0:eb20667a5a27 79 void Ec2multi::downA()
MazeTaka 0:eb20667a5a27 80 {
MazeTaka 0:eb20667a5a27 81 if(signalB_.read())count_--;
MazeTaka 0:eb20667a5a27 82 else count_++;
MazeTaka 0:eb20667a5a27 83 }
MazeTaka 0:eb20667a5a27 84
MazeTaka 0:eb20667a5a27 85 ////////////////////////////////////////////////////4逓倍//////////////////////////////////////////////////////////////////
MazeTaka 0:eb20667a5a27 86 Ec4multi::Ec4multi(PinName signalA,PinName signalB,int res) : Ec(res,4),signalA_(signalA),signalB_(signalB),pa_(0),pb_(0)
MazeTaka 0:eb20667a5a27 87 {
MazeTaka 0:eb20667a5a27 88 signalA_.rise(callback(this,&Ec4multi::upA));
MazeTaka 0:eb20667a5a27 89 signalA_.fall(callback(this,&Ec4multi::downA));
MazeTaka 0:eb20667a5a27 90 signalB_.rise(callback(this,&Ec4multi::upB));
MazeTaka 0:eb20667a5a27 91 signalB_.fall(callback(this,&Ec4multi::downB));
MazeTaka 0:eb20667a5a27 92 }
MazeTaka 0:eb20667a5a27 93 void Ec4multi::upA()
MazeTaka 0:eb20667a5a27 94 {
MazeTaka 0:eb20667a5a27 95 pa_=1;
MazeTaka 0:eb20667a5a27 96 if(pb_==1)count_++;
MazeTaka 0:eb20667a5a27 97 else count_--;
MazeTaka 0:eb20667a5a27 98 }
MazeTaka 0:eb20667a5a27 99 void Ec4multi::downA()
MazeTaka 0:eb20667a5a27 100 {
MazeTaka 0:eb20667a5a27 101 pa_=0;
MazeTaka 0:eb20667a5a27 102 if(pb_==1)count_--;
MazeTaka 0:eb20667a5a27 103 else count_++;
MazeTaka 0:eb20667a5a27 104 }
MazeTaka 0:eb20667a5a27 105 void Ec4multi::upB()
MazeTaka 0:eb20667a5a27 106 {
MazeTaka 0:eb20667a5a27 107 pb_=1;
MazeTaka 0:eb20667a5a27 108 if(pa_==1)count_--;
MazeTaka 0:eb20667a5a27 109 else count_++;
MazeTaka 0:eb20667a5a27 110 }
MazeTaka 0:eb20667a5a27 111 void Ec4multi::downB()
MazeTaka 0:eb20667a5a27 112 {
MazeTaka 0:eb20667a5a27 113 pb_=0;
MazeTaka 0:eb20667a5a27 114 if(pa_==1)count_++;
MazeTaka 0:eb20667a5a27 115 else count_--;
MazeTaka 0:eb20667a5a27 116 }