春ロボ1班(元F3RC4班+) / PathFollowing1-ver2

春ロボ2019 経路追従プログラム（ver.2）

Dependencies:   CruizCore_R1370P

Fork of PathFollowing1 by 春ロボ1班(元F3RC4班+)

EC.cpp@1:9858517eef98, 2018-10-31 (annotated)

Committer:
yuki0701
Date:
Wed Oct 31 10:13:49 2018 +0000
Revision:
1:9858517eef98
Parent:
0:3d9101dd0061 
a;

Who changed what in which revision?

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