riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

EC.cpp@26:45a53e3c81b1, 2017-08-02 (annotated)

Committer:
jack0325suzu
Date:
Wed Aug 02 06:55:54 2017 +0000
Revision:
26:45a53e3c81b1
Parent:
11:b96009f8b9fd
Child:
32:297384f9d261
?????????????I+PD?????I??????????????????????????I???????????????; ???????setPIDparam()?setPDparam()??????; ...

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 5:4abba4f54406 42 void Ec::upZ(){
jack0325suzu 5:4abba4f54406 43 if(first==false){
ShotaroSenzaki 11:b96009f8b9fd 44 static int res_count;
jack0325suzu 5:4abba4f54406 45 rev++;
jack0325suzu 5:4abba4f54406 46 first=true;
jack0325suzu 5:4abba4f54406 47 now_time=timer.read();
jack0325suzu 5:4abba4f54406 48 RPM=60/(now_time-old_time);
jack0325suzu 10:216d5a573dc7 49 if((RPM_old-RPM)>RPM_th){
ShotaroSenzaki 11:b96009f8b9fd 50 if(res_count >= 5){
ShotaroSenzaki 11:b96009f8b9fd 51 printf("\r\n CAUTION : speed downed drastically\r\n");
ShotaroSenzaki 11:b96009f8b9fd 52 NVIC_SystemReset();
ShotaroSenzaki 11:b96009f8b9fd 53 res_count=0;
ShotaroSenzaki 11:b96009f8b9fd 54 }else{
ShotaroSenzaki 11:b96009f8b9fd 55 res_count++;
ShotaroSenzaki 11:b96009f8b9fd 56 }
ShotaroSenzaki 11:b96009f8b9fd 57 }else{
ShotaroSenzaki 11:b96009f8b9fd 58 res_count=0;
jack0325suzu 5:4abba4f54406 59 }
jack0325suzu 5:4abba4f54406 60 RPM_old=RPM;
jack0325suzu 5:4abba4f54406 61 old_time=timer.read();
jack0325suzu 5:4abba4f54406 62 }
jack0325suzu 5:4abba4f54406 63 }
jack0325suzu 5:4abba4f54406 64
jack0325suzu 5:4abba4f54406 65 void Ec::downZ(){
jack0325suzu 5:4abba4f54406 66 first=false;
jack0325suzu 5:4abba4f54406 67 }
jack0325suzu 5:4abba4f54406 68
jack0325suzu 5:4abba4f54406 69
jack0325suzu 0:20fc96400ca3 70 //コンストラクタの定義
jack0325suzu 0:20fc96400ca3 71 //main関数の前に必ず一度宣言する
jack0325suzu 0:20fc96400ca3 72 //第一・第二引数はエンコーダのA・B相のピン名
jack0325suzu 0:20fc96400ca3 73 //第三院数はエンコーダの分解能
jack0325suzu 0:20fc96400ca3 74
jack0325suzu 5:4abba4f54406 75 Ec::Ec(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) : signalA_(signalA),signalB_(signalB),signalZ_(signalZ)
jack0325suzu 0:20fc96400ca3 76 {
jack0325suzu 5:4abba4f54406 77
jack0325suzu 5:4abba4f54406 78 if((signalA!=NC)&&(signalB!=NC)){
jack0325suzu 5:4abba4f54406 79 S=0;stateA=0;stateB=0;count=0;pre_count=0.0;
jack0325suzu 26:45a53e3c81b1 80 timer.start();
jack0325suzu 5:4abba4f54406 81 signalA_.rise(this,&Ec::upA);
jack0325suzu 5:4abba4f54406 82 signalA_.fall(this,&Ec::downA);
jack0325suzu 5:4abba4f54406 83 signalB_.rise(this,&Ec::upB);
jack0325suzu 5:4abba4f54406 84 signalB_.fall(this,&Ec::downB);
jack0325suzu 5:4abba4f54406 85 }
jack0325suzu 5:4abba4f54406 86 if(signalZ!=NC){
jack0325suzu 5:4abba4f54406 87 first=false; rev=0; old_time=0; RPM=0; RPM_old=0;
jack0325suzu 5:4abba4f54406 88 signalZ_.rise(this,&Ec::upZ);
jack0325suzu 5:4abba4f54406 89 signalZ_.fall(this,&Ec::downZ);
jack0325suzu 5:4abba4f54406 90 }
jack0325suzu 0:20fc96400ca3 91 dt=t;
jack0325suzu 0:20fc96400ca3 92 solution=s;
jack0325suzu 0:20fc96400ca3 93 defsolution=s;
jack0325suzu 10:216d5a573dc7 94 RPM_th=250;
jack0325suzu 0:20fc96400ca3 95 }
jack0325suzu 0:20fc96400ca3 96
jack0325suzu 0:20fc96400ca3 97 int Ec::getCount(){
jack0325suzu 0:20fc96400ca3 98 return count;
jack0325suzu 0:20fc96400ca3 99 }
jack0325suzu 0:20fc96400ca3 100
jack0325suzu 0:20fc96400ca3 101 void Ec::CalOmega(){
jack0325suzu 0:20fc96400ca3 102 omega=(count-pre_count)*2*M_pi/(solution*dt);
jack0325suzu 0:20fc96400ca3 103 pre_count=count;
jack0325suzu 0:20fc96400ca3 104 }
jack0325suzu 0:20fc96400ca3 105
jack0325suzu 0:20fc96400ca3 106 double Ec::getOmega(){
jack0325suzu 0:20fc96400ca3 107 return omega;
jack0325suzu 0:20fc96400ca3 108 }
jack0325suzu 0:20fc96400ca3 109
jack0325suzu 0:20fc96400ca3 110 double Ec::getPreCount(){
jack0325suzu 0:20fc96400ca3 111 precount=count+S/4.0;
jack0325suzu 0:20fc96400ca3 112 return precount;
jack0325suzu 0:20fc96400ca3 113 }
jack0325suzu 0:20fc96400ca3 114 /*reset関数の定義*/
jack0325suzu 0:20fc96400ca3 115 /*エンコーダを初期状態に戻すことができる*/
jack0325suzu 0:20fc96400ca3 116 void Ec::reset(){
jack0325suzu 0:20fc96400ca3 117 S=0;stateA=0;stateB=0;count=0;pre_count=0.0,omega=0;
jack0325suzu 8:833757a1df66 118 rev=0;now_time=0;old_time=0;RPM=0;RPM_old=0;
jack0325suzu 0:20fc96400ca3 119 }
jack0325suzu 0:20fc96400ca3 120 /*setTime関数の定義*/
jack0325suzu 0:20fc96400ca3 121 /*自分で好きなように角速度計算の間隔を決めることができる（デフォルトは0.05秒）*/
jack0325suzu 0:20fc96400ca3 122 void Ec::setTime(double t){
jack0325suzu 0:20fc96400ca3 123 dt=t;
jack0325suzu 0:20fc96400ca3 124 }
jack0325suzu 0:20fc96400ca3 125
jack0325suzu 5:4abba4f54406 126 double Ec::getRPM(){
jack0325suzu 5:4abba4f54406 127 return RPM;
jack0325suzu 5:4abba4f54406 128 }
jack0325suzu 5:4abba4f54406 129
jack0325suzu 8:833757a1df66 130 int Ec::getRev(){
jack0325suzu 5:4abba4f54406 131 return rev;
jack0325suzu 5:4abba4f54406 132 }
jack0325suzu 5:4abba4f54406 133
jack0325suzu 10:216d5a573dc7 134 void Ec::changeRPM_th(int th){
jack0325suzu 10:216d5a573dc7 135 RPM_th=th;
jack0325suzu 10:216d5a573dc7 136 }