riko-ten / EC2

ec

Dependents:   F3RC

Fork of EC by ROBOSTEP_LIBRARY

EC.cpp@34:b34dc495b3c8, 2018-07-05 (annotated)

Committer:
shimizuta
Date:
Thu Jul 05 01:23:58 2018 +0000
Revision:
34:b34dc495b3c8
Parent:
32:297384f9d261 
add error coment in distance

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 //ピン変化割り込み関数の定義
shimizuta 32:297384f9d261 7 void Ec::upA()
shimizuta 32:297384f9d261 8 {
jack0325suzu 0:20fc96400ca3 9 stateA=1;
jack0325suzu 0:20fc96400ca3 10 if(stateB==0&&S==0) {
jack0325suzu 0:20fc96400ca3 11 S=1;
jack0325suzu 0:20fc96400ca3 12 } else if(stateB==1&&S==3) {
jack0325suzu 0:20fc96400ca3 13 S=2;
jack0325suzu 0:20fc96400ca3 14 }
jack0325suzu 0:20fc96400ca3 15 }
shimizuta 32:297384f9d261 16 void Ec::downA()
shimizuta 32:297384f9d261 17 {
jack0325suzu 0:20fc96400ca3 18 stateA=0;
jack0325suzu 0:20fc96400ca3 19 if(stateB==1&&S==2) {
jack0325suzu 0:20fc96400ca3 20 S=3;
jack0325suzu 0:20fc96400ca3 21 } else if(stateB==0&&S==1) {
jack0325suzu 0:20fc96400ca3 22 S=0;
jack0325suzu 0:20fc96400ca3 23 count--;
jack0325suzu 0:20fc96400ca3 24 }
jack0325suzu 0:20fc96400ca3 25 }
shimizuta 32:297384f9d261 26 void Ec::upB()
shimizuta 32:297384f9d261 27 {
jack0325suzu 0:20fc96400ca3 28 stateB=1;
jack0325suzu 0:20fc96400ca3 29 if(stateA==1&&S==1) {
jack0325suzu 0:20fc96400ca3 30 S=2;
jack0325suzu 0:20fc96400ca3 31 } else if(stateA==0&&S==0) {
jack0325suzu 0:20fc96400ca3 32 S=3;
jack0325suzu 0:20fc96400ca3 33 }
jack0325suzu 0:20fc96400ca3 34 }
shimizuta 32:297384f9d261 35 void Ec::downB()
shimizuta 32:297384f9d261 36 {
jack0325suzu 0:20fc96400ca3 37 stateB=0;
jack0325suzu 0:20fc96400ca3 38 if(stateA==0&&S==3) {
jack0325suzu 0:20fc96400ca3 39 count++;
jack0325suzu 0:20fc96400ca3 40 S=0;
jack0325suzu 0:20fc96400ca3 41 } else if(stateA==1&&S==2) {
jack0325suzu 0:20fc96400ca3 42 S=1;
jack0325suzu 0:20fc96400ca3 43 }
jack0325suzu 0:20fc96400ca3 44 }
jack0325suzu 0:20fc96400ca3 45
shimizuta 32:297384f9d261 46 void Ec::upZ()
shimizuta 32:297384f9d261 47 {
shimizuta 32:297384f9d261 48 if(first==false) {
ShotaroSenzaki 11:b96009f8b9fd 49 static int res_count;
jack0325suzu 5:4abba4f54406 50 rev++;
jack0325suzu 5:4abba4f54406 51 first=true;
jack0325suzu 5:4abba4f54406 52 now_time=timer.read();
jack0325suzu 5:4abba4f54406 53 RPM=60/(now_time-old_time);
shimizuta 32:297384f9d261 54 if((RPM_old-RPM)>RPM_th) {
shimizuta 32:297384f9d261 55 if(res_count >= 5) {
ShotaroSenzaki 11:b96009f8b9fd 56 printf("\r\n CAUTION : speed downed drastically\r\n");
ShotaroSenzaki 11:b96009f8b9fd 57 NVIC_SystemReset();
ShotaroSenzaki 11:b96009f8b9fd 58 res_count=0;
shimizuta 32:297384f9d261 59 } else {
ShotaroSenzaki 11:b96009f8b9fd 60 res_count++;
ShotaroSenzaki 11:b96009f8b9fd 61 }
shimizuta 32:297384f9d261 62 } else {
ShotaroSenzaki 11:b96009f8b9fd 63 res_count=0;
jack0325suzu 5:4abba4f54406 64 }
jack0325suzu 5:4abba4f54406 65 RPM_old=RPM;
jack0325suzu 5:4abba4f54406 66 old_time=timer.read();
shimizuta 32:297384f9d261 67 }
jack0325suzu 5:4abba4f54406 68 }
jack0325suzu 5:4abba4f54406 69
shimizuta 32:297384f9d261 70 void Ec::downZ()
shimizuta 32:297384f9d261 71 {
jack0325suzu 5:4abba4f54406 72 first=false;
jack0325suzu 5:4abba4f54406 73 }
jack0325suzu 5:4abba4f54406 74
jack0325suzu 5:4abba4f54406 75
jack0325suzu 0:20fc96400ca3 76 //コンストラクタの定義
jack0325suzu 0:20fc96400ca3 77 //main関数の前に必ず一度宣言する
jack0325suzu 0:20fc96400ca3 78 //第一・第二引数はエンコーダのA・B相のピン名
jack0325suzu 0:20fc96400ca3 79 //第三院数はエンコーダの分解能
jack0325suzu 0:20fc96400ca3 80
jack0325suzu 5:4abba4f54406 81 Ec::Ec(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) : signalA_(signalA),signalB_(signalB),signalZ_(signalZ)
jack0325suzu 0:20fc96400ca3 82 {
shimizuta 32:297384f9d261 83
shimizuta 32:297384f9d261 84 if((signalA!=NC)&&(signalB!=NC)) {
shimizuta 32:297384f9d261 85 S=0;
shimizuta 32:297384f9d261 86 stateA=0;
shimizuta 32:297384f9d261 87 stateB=0;
shimizuta 32:297384f9d261 88 count=0;
shimizuta 32:297384f9d261 89 pre_count=0.0;
jack0325suzu 26:45a53e3c81b1 90 timer.start();
shimizuta 32:297384f9d261 91 signalA_.rise(callback(this,&Ec::upA));
shimizuta 32:297384f9d261 92 signalA_.fall(callback(this,&Ec::downA));
shimizuta 32:297384f9d261 93 signalB_.rise(callback(this,&Ec::upB));
shimizuta 32:297384f9d261 94 signalB_.fall(callback(this,&Ec::downB));
jack0325suzu 5:4abba4f54406 95 }
shimizuta 32:297384f9d261 96 if(signalZ!=NC) {
shimizuta 32:297384f9d261 97 first=false;
shimizuta 32:297384f9d261 98 rev=0;
shimizuta 32:297384f9d261 99 old_time=0;
shimizuta 32:297384f9d261 100 RPM=0;
shimizuta 32:297384f9d261 101 RPM_old=0;
shimizuta 32:297384f9d261 102 signalZ_.rise(callback(this,&Ec::upZ));
shimizuta 32:297384f9d261 103 signalZ_.fall(callback(this,&Ec::downZ));
jack0325suzu 5:4abba4f54406 104 }
jack0325suzu 0:20fc96400ca3 105 dt=t;
jack0325suzu 0:20fc96400ca3 106 solution=s;
jack0325suzu 0:20fc96400ca3 107 defsolution=s;
jack0325suzu 10:216d5a573dc7 108 RPM_th=250;
shimizuta 32:297384f9d261 109 count_to_distance_mm_ = 0;
shimizuta 32:297384f9d261 110 gear_rate_ = 1;
shimizuta 34:b34dc495b3c8 111 is_diameter_set = 0;
jack0325suzu 0:20fc96400ca3 112 }
jack0325suzu 0:20fc96400ca3 113
shimizuta 32:297384f9d261 114 int Ec::getCount()
shimizuta 32:297384f9d261 115 {
jack0325suzu 0:20fc96400ca3 116 return count;
jack0325suzu 0:20fc96400ca3 117 }
jack0325suzu 0:20fc96400ca3 118
shimizuta 32:297384f9d261 119 void Ec::CalOmega()
shimizuta 32:297384f9d261 120 {
jack0325suzu 0:20fc96400ca3 121 omega=(count-pre_count)*2*M_pi/(solution*dt);
jack0325suzu 0:20fc96400ca3 122 pre_count=count;
jack0325suzu 0:20fc96400ca3 123 }
jack0325suzu 0:20fc96400ca3 124
shimizuta 32:297384f9d261 125 double Ec::getOmega()
shimizuta 32:297384f9d261 126 {
jack0325suzu 0:20fc96400ca3 127 return omega;
jack0325suzu 0:20fc96400ca3 128 }
jack0325suzu 0:20fc96400ca3 129
shimizuta 32:297384f9d261 130 double Ec::getPreCount()
shimizuta 32:297384f9d261 131 {
jack0325suzu 0:20fc96400ca3 132 precount=count+S/4.0;
jack0325suzu 0:20fc96400ca3 133 return precount;
jack0325suzu 0:20fc96400ca3 134 }
jack0325suzu 0:20fc96400ca3 135 /*reset関数の定義*/
jack0325suzu 0:20fc96400ca3 136 /*エンコーダを初期状態に戻すことができる*/
shimizuta 32:297384f9d261 137 void Ec::reset()
shimizuta 32:297384f9d261 138 {
shimizuta 32:297384f9d261 139 S=0;
shimizuta 32:297384f9d261 140 stateA=0;
shimizuta 32:297384f9d261 141 stateB=0;
shimizuta 32:297384f9d261 142 count=0;
shimizuta 32:297384f9d261 143 pre_count=0.0,omega=0;
shimizuta 32:297384f9d261 144 rev=0;
shimizuta 32:297384f9d261 145 now_time=0;
shimizuta 32:297384f9d261 146 old_time=0;
shimizuta 32:297384f9d261 147 RPM=0;
shimizuta 32:297384f9d261 148 RPM_old=0;
jack0325suzu 0:20fc96400ca3 149 }
jack0325suzu 0:20fc96400ca3 150 /*setTime関数の定義*/
jack0325suzu 0:20fc96400ca3 151 /*自分で好きなように角速度計算の間隔を決めることができる（デフォルトは0.05秒）*/
shimizuta 32:297384f9d261 152 void Ec::setTime(double t)
shimizuta 32:297384f9d261 153 {
jack0325suzu 0:20fc96400ca3 154 dt=t;
jack0325suzu 0:20fc96400ca3 155 }
jack0325suzu 0:20fc96400ca3 156
shimizuta 32:297384f9d261 157 double Ec::getRPM()
shimizuta 32:297384f9d261 158 {
jack0325suzu 5:4abba4f54406 159 return RPM;
jack0325suzu 5:4abba4f54406 160 }
jack0325suzu 5:4abba4f54406 161
shimizuta 32:297384f9d261 162 int Ec::getRev()
shimizuta 32:297384f9d261 163 {
jack0325suzu 5:4abba4f54406 164 return rev;
jack0325suzu 5:4abba4f54406 165 }
jack0325suzu 5:4abba4f54406 166
shimizuta 32:297384f9d261 167 void Ec::changeRPM_th(int th)
shimizuta 32:297384f9d261 168 {
jack0325suzu 10:216d5a573dc7 169 RPM_th=th;
shimizuta 32:297384f9d261 170 }
shimizuta 32:297384f9d261 171
shimizuta 32:297384f9d261 172 double Ec::getDistance_mm()
shimizuta 32:297384f9d261 173 {
shimizuta 34:b34dc495b3c8 174 if(is_diameter_set == 0) printf("please set diameter\r\n");
shimizuta 32:297384f9d261 175 distance_mm_= count * count_to_distance_mm_;
shimizuta 32:297384f9d261 176 return distance_mm_;
shimizuta 32:297384f9d261 177 }
shimizuta 32:297384f9d261 178 void Ec::setDiameter_mm(double diameter_mm)
shimizuta 32:297384f9d261 179 {
shimizuta 32:297384f9d261 180 diameter_mm_ = diameter_mm;
shimizuta 32:297384f9d261 181 setCountToDistance_mm();
shimizuta 34:b34dc495b3c8 182 is_diameter_set = 1;
shimizuta 32:297384f9d261 183 }
shimizuta 32:297384f9d261 184 void Ec::setGearRate(double gear_rate){
shimizuta 32:297384f9d261 185 gear_rate_ = gear_rate;
shimizuta 32:297384f9d261 186 setCountToDistance_mm();
shimizuta 32:297384f9d261 187 }
shimizuta 32:297384f9d261 188 void Ec::setCountToDistance_mm(){
shimizuta 32:297384f9d261 189 count_to_distance_mm_ = 1 / (double)solution * M_pi * diameter_mm_ * gear_rate_;//カウント分解能＊2Pi=角度（rad)。角度*直径/2 距離を整理した式
jack0325suzu 10:216d5a573dc7 190 }