7th_DENSOU / Mbed 2 deprecated nhk_2021motorAB_

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_melodic

EC/EC.cpp@1:163629043391, 2021-05-15 (annotated)

Committer:
koheim
Date:
Sat May 15 08:34:17 2021 +0000
Revision:
1:163629043391
Parent:
0:cca61e773cbb 
nhk_motor(AB)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:cca61e773cbb 1 #include "mbed.h"
la00noix 0:cca61e773cbb 2 #include "EC.h"
la00noix 0:cca61e773cbb 3
la00noix 0:cca61e773cbb 4 int Ec::defsolution;
la00noix 0:cca61e773cbb 5 double Ec::deftime;
la00noix 0:cca61e773cbb 6 //ピン変化割り込み関数の定義
la00noix 0:cca61e773cbb 7 void Ec::upA(){
la00noix 0:cca61e773cbb 8 stateA=1;
la00noix 0:cca61e773cbb 9 if(stateB==0&&S==0) {
la00noix 0:cca61e773cbb 10 S=1;
la00noix 0:cca61e773cbb 11 } else if(stateB==1&&S==3) {
la00noix 0:cca61e773cbb 12 S=2;
la00noix 0:cca61e773cbb 13 }
la00noix 0:cca61e773cbb 14 }
la00noix 0:cca61e773cbb 15 void Ec::downA(){
la00noix 0:cca61e773cbb 16 stateA=0;
la00noix 0:cca61e773cbb 17 if(stateB==1&&S==2) {
la00noix 0:cca61e773cbb 18 S=3;
la00noix 0:cca61e773cbb 19 } else if(stateB==0&&S==1) {
la00noix 0:cca61e773cbb 20 S=0;
la00noix 0:cca61e773cbb 21 count--;
la00noix 0:cca61e773cbb 22 }
la00noix 0:cca61e773cbb 23 }
la00noix 0:cca61e773cbb 24 void Ec::upB(){
la00noix 0:cca61e773cbb 25 stateB=1;
la00noix 0:cca61e773cbb 26 if(stateA==1&&S==1) {
la00noix 0:cca61e773cbb 27 S=2;
la00noix 0:cca61e773cbb 28 } else if(stateA==0&&S==0) {
la00noix 0:cca61e773cbb 29 S=3;
la00noix 0:cca61e773cbb 30 }
la00noix 0:cca61e773cbb 31 }
la00noix 0:cca61e773cbb 32 void Ec::downB(){
la00noix 0:cca61e773cbb 33 stateB=0;
la00noix 0:cca61e773cbb 34 if(stateA==0&&S==3) {
la00noix 0:cca61e773cbb 35 count++;
la00noix 0:cca61e773cbb 36 S=0;
la00noix 0:cca61e773cbb 37 } else if(stateA==1&&S==2) {
la00noix 0:cca61e773cbb 38 S=1;
la00noix 0:cca61e773cbb 39 }
la00noix 0:cca61e773cbb 40 }
la00noix 0:cca61e773cbb 41
la00noix 0:cca61e773cbb 42 void Ec::upZ(){
la00noix 0:cca61e773cbb 43 if(first==false){
la00noix 0:cca61e773cbb 44 static int res_count;
la00noix 0:cca61e773cbb 45 rev++;
la00noix 0:cca61e773cbb 46 first=true;
la00noix 0:cca61e773cbb 47 now_time=timer.read();
la00noix 0:cca61e773cbb 48 RPM=60/(now_time-old_time);
la00noix 0:cca61e773cbb 49 if((RPM_old-RPM)>RPM_th){
la00noix 0:cca61e773cbb 50 if(res_count >= 5){
la00noix 0:cca61e773cbb 51 printf("\r\n CAUTION : speed downed drastically\r\n");
la00noix 0:cca61e773cbb 52 NVIC_SystemReset();
la00noix 0:cca61e773cbb 53 res_count=0;
la00noix 0:cca61e773cbb 54 }else{
la00noix 0:cca61e773cbb 55 res_count++;
la00noix 0:cca61e773cbb 56 }
la00noix 0:cca61e773cbb 57 }else{
la00noix 0:cca61e773cbb 58 res_count=0;
la00noix 0:cca61e773cbb 59 }
la00noix 0:cca61e773cbb 60 RPM_old=RPM;
la00noix 0:cca61e773cbb 61 old_time=timer.read();
la00noix 0:cca61e773cbb 62 }
la00noix 0:cca61e773cbb 63 }
la00noix 0:cca61e773cbb 64
la00noix 0:cca61e773cbb 65 void Ec::downZ(){
la00noix 0:cca61e773cbb 66 first=false;
la00noix 0:cca61e773cbb 67 }
la00noix 0:cca61e773cbb 68
la00noix 0:cca61e773cbb 69
la00noix 0:cca61e773cbb 70 //コンストラクタの定義
la00noix 0:cca61e773cbb 71 //main関数の前に必ず一度宣言する
la00noix 0:cca61e773cbb 72 //第一・第二引数はエンコーダのA・B相のピン名
la00noix 0:cca61e773cbb 73 //第三院数はエンコーダの分解能
la00noix 0:cca61e773cbb 74
la00noix 0:cca61e773cbb 75 Ec::Ec(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) : signalA_(signalA),signalB_(signalB),signalZ_(signalZ)
la00noix 0:cca61e773cbb 76 {
la00noix 0:cca61e773cbb 77
la00noix 0:cca61e773cbb 78 if((signalA!=NC)&&(signalB!=NC)){
la00noix 0:cca61e773cbb 79 S=0;stateA=0;stateB=0;count=0;pre_count=0.0;
la00noix 0:cca61e773cbb 80 timer.start();
la00noix 0:cca61e773cbb 81 signalA_.rise(this,&Ec::upA);
la00noix 0:cca61e773cbb 82 signalA_.fall(this,&Ec::downA);
la00noix 0:cca61e773cbb 83 signalB_.rise(this,&Ec::upB);
la00noix 0:cca61e773cbb 84 signalB_.fall(this,&Ec::downB);
la00noix 0:cca61e773cbb 85 }
la00noix 0:cca61e773cbb 86 if(signalZ!=NC){
la00noix 0:cca61e773cbb 87 first=false; rev=0; old_time=0; RPM=0; RPM_old=0;
la00noix 0:cca61e773cbb 88 signalZ_.rise(this,&Ec::upZ);
la00noix 0:cca61e773cbb 89 signalZ_.fall(this,&Ec::downZ);
la00noix 0:cca61e773cbb 90 }
la00noix 0:cca61e773cbb 91 dt=t;
la00noix 0:cca61e773cbb 92 solution=s;
la00noix 0:cca61e773cbb 93 C_ec=1/(dt*solution);
la00noix 0:cca61e773cbb 94 defsolution=s;
la00noix 0:cca61e773cbb 95 RPM_th=250;
la00noix 0:cca61e773cbb 96 }
la00noix 0:cca61e773cbb 97
la00noix 0:cca61e773cbb 98 int Ec::getCount(){
la00noix 0:cca61e773cbb 99 return count;
la00noix 0:cca61e773cbb 100 }
la00noix 0:cca61e773cbb 101
la00noix 0:cca61e773cbb 102 void Ec::CalOmega(){
la00noix 0:cca61e773cbb 103 omega=(count+S/4.0-pre_count)*2*M_pi*C_ec;
la00noix 0:cca61e773cbb 104 pre_count=count+S/4.0;
la00noix 0:cca61e773cbb 105 }
la00noix 0:cca61e773cbb 106
la00noix 0:cca61e773cbb 107 double Ec::getOmega(){
la00noix 0:cca61e773cbb 108 return omega;
la00noix 0:cca61e773cbb 109 }
la00noix 0:cca61e773cbb 110
la00noix 0:cca61e773cbb 111 double Ec::getPreCount(){
la00noix 0:cca61e773cbb 112 precount=count+S/4.0;
la00noix 0:cca61e773cbb 113 return precount;
la00noix 0:cca61e773cbb 114 }
la00noix 0:cca61e773cbb 115 /*reset関数の定義*/
la00noix 0:cca61e773cbb 116 /*エンコーダを初期状態に戻すことができる*/
la00noix 0:cca61e773cbb 117 /*void Ec::reset(){
la00noix 0:cca61e773cbb 118 S=0;stateA=0;stateB=0;count=0;pre_count=0.0,omega=0;
la00noix 0:cca61e773cbb 119 rev=0;now_time=0;old_time=0;RPM=0;RPM_old=0;
la00noix 0:cca61e773cbb 120 }*/
la00noix 0:cca61e773cbb 121 /*setTime関数の定義*/
la00noix 0:cca61e773cbb 122 /*自分で好きなように角速度計算の間隔を決めることができる（デフォルトは0.05秒）*/
la00noix 0:cca61e773cbb 123 void Ec::setTime(double t){
la00noix 0:cca61e773cbb 124 dt=t;
la00noix 0:cca61e773cbb 125 }
la00noix 0:cca61e773cbb 126
la00noix 0:cca61e773cbb 127 double Ec::getRPM(){
la00noix 0:cca61e773cbb 128 return RPM;
la00noix 0:cca61e773cbb 129 }
la00noix 0:cca61e773cbb 130
la00noix 0:cca61e773cbb 131 int Ec::getRev(){
la00noix 0:cca61e773cbb 132 return rev;
la00noix 0:cca61e773cbb 133 }
la00noix 0:cca61e773cbb 134
la00noix 0:cca61e773cbb 135 void Ec::changeRPM_th(int th){
la00noix 0:cca61e773cbb 136 RPM_th=th;
la00noix 0:cca61e773cbb 137 }