yuto kawamura / Mbed 2 deprecated MR2_n_1_class

l

Dependencies:   mbed

ec_temp/EC.cpp@6:75cfa1a66382, 2019-04-27 (annotated)

Committer:
yuto17320508
Date:
Sat Apr 27 11:22:00 2019 +0000
Revision:
6:75cfa1a66382
Parent:
0:111abd91b0cb 
l

Who changed what in which revision?

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