yuto kawamura / Mbed 2 deprecated MR2_2n_1_saku

sls

Dependencies:   mbed

ec_temp/EC.cpp@0:c1476d342c13, 2019-04-26 (annotated)

Committer:
eri
Date:
Fri Apr 26 11:34:13 2019 +0000
Revision:
0:c1476d342c13
a

Who changed what in which revision?

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