riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

EC.cpp@0:20fc96400ca3, 2016-06-16 (annotated)

Committer:
jack0325suzu
Date:
Thu Jun 16 07:36:15 2016 +0000
Revision:
0:20fc96400ca3
Child:
5:4abba4f54406
????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jack0325suzu 0:20fc96400ca3 1 #include "mbed.h"
jack0325suzu 0:20fc96400ca3 2 #include "EC.h"
jack0325suzu 0:20fc96400ca3 3
jack0325suzu 0:20fc96400ca3 4 int Ec::defsolution;
jack0325suzu 0:20fc96400ca3 5 double Ec::deftime;
jack0325suzu 0:20fc96400ca3 6 //ピン変化割り込み関数の定義
jack0325suzu 0:20fc96400ca3 7 void Ec::upA(){
jack0325suzu 0:20fc96400ca3 8 stateA=1;
jack0325suzu 0:20fc96400ca3 9 if(stateB==0&&S==0) {
jack0325suzu 0:20fc96400ca3 10 S=1;
jack0325suzu 0:20fc96400ca3 11 } else if(stateB==1&&S==3) {
jack0325suzu 0:20fc96400ca3 12 S=2;
jack0325suzu 0:20fc96400ca3 13 }
jack0325suzu 0:20fc96400ca3 14 }
jack0325suzu 0:20fc96400ca3 15 void Ec::downA(){
jack0325suzu 0:20fc96400ca3 16 stateA=0;
jack0325suzu 0:20fc96400ca3 17 if(stateB==1&&S==2) {
jack0325suzu 0:20fc96400ca3 18 S=3;
jack0325suzu 0:20fc96400ca3 19 } else if(stateB==0&&S==1) {
jack0325suzu 0:20fc96400ca3 20 S=0;
jack0325suzu 0:20fc96400ca3 21 count--;
jack0325suzu 0:20fc96400ca3 22 }
jack0325suzu 0:20fc96400ca3 23 }
jack0325suzu 0:20fc96400ca3 24 void Ec::upB(){
jack0325suzu 0:20fc96400ca3 25 stateB=1;
jack0325suzu 0:20fc96400ca3 26 if(stateA==1&&S==1) {
jack0325suzu 0:20fc96400ca3 27 S=2;
jack0325suzu 0:20fc96400ca3 28 } else if(stateA==0&&S==0) {
jack0325suzu 0:20fc96400ca3 29 S=3;
jack0325suzu 0:20fc96400ca3 30 }
jack0325suzu 0:20fc96400ca3 31 }
jack0325suzu 0:20fc96400ca3 32 void Ec::downB(){
jack0325suzu 0:20fc96400ca3 33 stateB=0;
jack0325suzu 0:20fc96400ca3 34 if(stateA==0&&S==3) {
jack0325suzu 0:20fc96400ca3 35 count++;
jack0325suzu 0:20fc96400ca3 36 S=0;
jack0325suzu 0:20fc96400ca3 37 } else if(stateA==1&&S==2) {
jack0325suzu 0:20fc96400ca3 38 S=1;
jack0325suzu 0:20fc96400ca3 39 }
jack0325suzu 0:20fc96400ca3 40 }
jack0325suzu 0:20fc96400ca3 41
jack0325suzu 0:20fc96400ca3 42 //コンストラクタの定義
jack0325suzu 0:20fc96400ca3 43 //main関数の前に必ず一度宣言する
jack0325suzu 0:20fc96400ca3 44 //第一・第二引数はエンコーダのA・B相のピン名
jack0325suzu 0:20fc96400ca3 45 //第三院数はエンコーダの分解能
jack0325suzu 0:20fc96400ca3 46
jack0325suzu 0:20fc96400ca3 47 Ec::Ec(PinName signalA,PinName signalB,int s=defsolution,double t=deftime) : signalA_(signalA),signalB_(signalB)
jack0325suzu 0:20fc96400ca3 48 {
jack0325suzu 0:20fc96400ca3 49 S=0;stateA=0;stateB=0;count=0;pre_count=0.0;
jack0325suzu 0:20fc96400ca3 50 signalA_.rise(this,&Ec::upA);
jack0325suzu 0:20fc96400ca3 51 signalA_.fall(this,&Ec::downA);
jack0325suzu 0:20fc96400ca3 52 signalB_.rise(this,&Ec::upB);
jack0325suzu 0:20fc96400ca3 53 signalB_.fall(this,&Ec::downB);
jack0325suzu 0:20fc96400ca3 54 dt=t;
jack0325suzu 0:20fc96400ca3 55 solution=s;
jack0325suzu 0:20fc96400ca3 56 defsolution=s;
jack0325suzu 0:20fc96400ca3 57 }
jack0325suzu 0:20fc96400ca3 58
jack0325suzu 0:20fc96400ca3 59 int Ec::getCount(){
jack0325suzu 0:20fc96400ca3 60 return count;
jack0325suzu 0:20fc96400ca3 61 }
jack0325suzu 0:20fc96400ca3 62
jack0325suzu 0:20fc96400ca3 63 void Ec::CalOmega(){
jack0325suzu 0:20fc96400ca3 64 omega=(count-pre_count)*2*M_pi/(solution*dt);
jack0325suzu 0:20fc96400ca3 65 pre_count=count;
jack0325suzu 0:20fc96400ca3 66 }
jack0325suzu 0:20fc96400ca3 67
jack0325suzu 0:20fc96400ca3 68 double Ec::getOmega(){
jack0325suzu 0:20fc96400ca3 69 return omega;
jack0325suzu 0:20fc96400ca3 70 }
jack0325suzu 0:20fc96400ca3 71
jack0325suzu 0:20fc96400ca3 72 double Ec::getPreCount(){
jack0325suzu 0:20fc96400ca3 73 precount=count+S/4.0;
jack0325suzu 0:20fc96400ca3 74 return precount;
jack0325suzu 0:20fc96400ca3 75 }
jack0325suzu 0:20fc96400ca3 76 /*reset関数の定義*/
jack0325suzu 0:20fc96400ca3 77 /*エンコーダを初期状態に戻すことができる*/
jack0325suzu 0:20fc96400ca3 78 void Ec::reset(){
jack0325suzu 0:20fc96400ca3 79 S=0;stateA=0;stateB=0;count=0;pre_count=0.0,omega=0;
jack0325suzu 0:20fc96400ca3 80 }
jack0325suzu 0:20fc96400ca3 81 /*setTime関数の定義*/
jack0325suzu 0:20fc96400ca3 82 /*自分で好きなように角速度計算の間隔を決めることができる（デフォルトは0.05秒）*/
jack0325suzu 0:20fc96400ca3 83 void Ec::setTime(double t){
jack0325suzu 0:20fc96400ca3 84 dt=t;
jack0325suzu 0:20fc96400ca3 85 }
jack0325suzu 0:20fc96400ca3 86