yuto kawamura / Mbed 2 deprecated George_Slave_BOTHMOVE

right and left move at the same time

Dependencies:   mbed robot

ec_temp/EC.cpp@13:678870d8f851, 2019-05-15 (annotated)

Committer:
yuto17320508
Date:
Wed May 15 10:19:13 2019 +0000
Revision:
13:678870d8f851
Parent:
0:411ab20ce87d 
l

Who changed what in which revision?

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