Hiroki Tanaka / Mbed 2 deprecated foot_F446RE_nomal

STM32足回りプログラム

Dependencies:   mbed ros_lib_kinetic

library/RotaryInc.cpp@0:a202e1bc38a1, 2019-06-17 (annotated)

Committer:
TanakaRobo
Date:
Mon Jun 17 13:41:24 2019 +0000
Revision:
0:a202e1bc38a1
Child:
1:7b0db5ea0ab7
STM32 foot;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 0:a202e1bc38a1 1 #include "RotaryInc.hpp"
TanakaRobo 0:a202e1bc38a1 2
TanakaRobo 0:a202e1bc38a1 3 RotaryInc::RotaryInc(PinName pinA, PinName pinB,int mode):mode(mode){
TanakaRobo 0:a202e1bc38a1 4 measur = false;
TanakaRobo 0:a202e1bc38a1 5 init(pinA,pinB);
TanakaRobo 0:a202e1bc38a1 6 }
TanakaRobo 0:a202e1bc38a1 7
TanakaRobo 0:a202e1bc38a1 8 RotaryInc::RotaryInc(PinName pinA,PinName pinB,double circumference,int Resolution,int mode)
TanakaRobo 0:a202e1bc38a1 9 :mode(mode),Resolution(Resolution),circumference(circumference){
TanakaRobo 0:a202e1bc38a1 10 measur = true;
TanakaRobo 0:a202e1bc38a1 11 zero();
TanakaRobo 0:a202e1bc38a1 12 init(pinA,pinB);
TanakaRobo 0:a202e1bc38a1 13 }
TanakaRobo 0:a202e1bc38a1 14
TanakaRobo 0:a202e1bc38a1 15 void RotaryInc::init(PinName pinA,PinName pinB){
TanakaRobo 0:a202e1bc38a1 16 A = new InterruptIn(pinA,PullUp);
TanakaRobo 0:a202e1bc38a1 17 B = new InterruptIn(pinB,PullUp);
TanakaRobo 0:a202e1bc38a1 18 A->rise(callback(this,&RotaryInc::riseA));
TanakaRobo 0:a202e1bc38a1 19
TanakaRobo 0:a202e1bc38a1 20 if(mode == 2){
TanakaRobo 0:a202e1bc38a1 21 A->fall(callback(this,&RotaryInc::fallA));
TanakaRobo 0:a202e1bc38a1 22 }else if(mode == 4){
TanakaRobo 0:a202e1bc38a1 23 A->fall(callback(this,&RotaryInc::fallA));
TanakaRobo 0:a202e1bc38a1 24 B->rise(callback(this,&RotaryInc::riseB));
TanakaRobo 0:a202e1bc38a1 25 B->fall(callback(this,&RotaryInc::fallB));
TanakaRobo 0:a202e1bc38a1 26 }else{
TanakaRobo 0:a202e1bc38a1 27 mode = 1;
TanakaRobo 0:a202e1bc38a1 28 }
TanakaRobo 0:a202e1bc38a1 29 }
TanakaRobo 0:a202e1bc38a1 30
TanakaRobo 0:a202e1bc38a1 31 void RotaryInc::zero(){
TanakaRobo 0:a202e1bc38a1 32 time.stop();
TanakaRobo 0:a202e1bc38a1 33 time.reset();
TanakaRobo 0:a202e1bc38a1 34 startflag = false;
TanakaRobo 0:a202e1bc38a1 35 flag = false;
TanakaRobo 0:a202e1bc38a1 36 last[0] = pulse;
TanakaRobo 0:a202e1bc38a1 37 speed = 0;
TanakaRobo 0:a202e1bc38a1 38 count = 0;
TanakaRobo 0:a202e1bc38a1 39 sum = 0;
TanakaRobo 0:a202e1bc38a1 40 now = 0;
TanakaRobo 0:a202e1bc38a1 41 }
TanakaRobo 0:a202e1bc38a1 42
TanakaRobo 0:a202e1bc38a1 43 void RotaryInc::calcu(){
TanakaRobo 0:a202e1bc38a1 44 if(!startflag){
TanakaRobo 0:a202e1bc38a1 45 time.start();
TanakaRobo 0:a202e1bc38a1 46 startflag = true;
TanakaRobo 0:a202e1bc38a1 47 last[0] = pulse;
TanakaRobo 0:a202e1bc38a1 48 pre_t[0] = 0;
TanakaRobo 0:a202e1bc38a1 49 count = 1;
TanakaRobo 0:a202e1bc38a1 50 }else if(flag){
TanakaRobo 0:a202e1bc38a1 51 now = time.read();
TanakaRobo 0:a202e1bc38a1 52 time.reset();
TanakaRobo 0:a202e1bc38a1 53 sum -= pre_t[count];
TanakaRobo 0:a202e1bc38a1 54 pre_t[count] = now;
TanakaRobo 0:a202e1bc38a1 55 sum += now;
TanakaRobo 0:a202e1bc38a1 56 speed = (double)(pulse - last[count]) / sum;
TanakaRobo 0:a202e1bc38a1 57 last[count] = pulse;
TanakaRobo 0:a202e1bc38a1 58 if(count < 19){
TanakaRobo 0:a202e1bc38a1 59 count++;
TanakaRobo 0:a202e1bc38a1 60 }else{
TanakaRobo 0:a202e1bc38a1 61 count = 0;
TanakaRobo 0:a202e1bc38a1 62 }
TanakaRobo 0:a202e1bc38a1 63 }else{
TanakaRobo 0:a202e1bc38a1 64 now = time.read();
TanakaRobo 0:a202e1bc38a1 65 time.reset();
TanakaRobo 0:a202e1bc38a1 66 pre_t[count] = now;
TanakaRobo 0:a202e1bc38a1 67 sum += now;
TanakaRobo 0:a202e1bc38a1 68 speed = (double)(pulse - last[0]) / sum;
TanakaRobo 0:a202e1bc38a1 69 last[count] = pulse;
TanakaRobo 0:a202e1bc38a1 70 count++;
TanakaRobo 0:a202e1bc38a1 71 if(count > 19){
TanakaRobo 0:a202e1bc38a1 72 count = 0;
TanakaRobo 0:a202e1bc38a1 73 flag = true;
TanakaRobo 0:a202e1bc38a1 74 }
TanakaRobo 0:a202e1bc38a1 75 }
TanakaRobo 0:a202e1bc38a1 76 }
TanakaRobo 0:a202e1bc38a1 77
TanakaRobo 0:a202e1bc38a1 78 void RotaryInc::riseA(){
TanakaRobo 0:a202e1bc38a1 79 B->read() ? pulse-- : pulse++;
TanakaRobo 0:a202e1bc38a1 80 if(measur)calcu();
TanakaRobo 0:a202e1bc38a1 81 }
TanakaRobo 0:a202e1bc38a1 82
TanakaRobo 0:a202e1bc38a1 83 void RotaryInc::fallA(){
TanakaRobo 0:a202e1bc38a1 84 B->read() ? pulse++ : pulse--;
TanakaRobo 0:a202e1bc38a1 85 if(measur)calcu();
TanakaRobo 0:a202e1bc38a1 86 }
TanakaRobo 0:a202e1bc38a1 87
TanakaRobo 0:a202e1bc38a1 88 void RotaryInc::riseB(){
TanakaRobo 0:a202e1bc38a1 89 A->read() ? pulse++ : pulse--;
TanakaRobo 0:a202e1bc38a1 90 if(measur)calcu();
TanakaRobo 0:a202e1bc38a1 91 }
TanakaRobo 0:a202e1bc38a1 92
TanakaRobo 0:a202e1bc38a1 93 void RotaryInc::fallB(){
TanakaRobo 0:a202e1bc38a1 94 A->read() ? pulse-- : pulse++;
TanakaRobo 0:a202e1bc38a1 95 if(measur)calcu();
TanakaRobo 0:a202e1bc38a1 96 }
TanakaRobo 0:a202e1bc38a1 97
TanakaRobo 0:a202e1bc38a1 98 long long RotaryInc::get(){
TanakaRobo 0:a202e1bc38a1 99 return pulse;
TanakaRobo 0:a202e1bc38a1 100 }
TanakaRobo 0:a202e1bc38a1 101
TanakaRobo 0:a202e1bc38a1 102 double RotaryInc::getSpeed(){
TanakaRobo 0:a202e1bc38a1 103 if(!measur)return 0;
TanakaRobo 0:a202e1bc38a1 104 if(time.read_ms() > 150){
TanakaRobo 0:a202e1bc38a1 105 zero();
TanakaRobo 0:a202e1bc38a1 106 }
TanakaRobo 0:a202e1bc38a1 107 return speed / Resolution / mode * circumference;
TanakaRobo 0:a202e1bc38a1 108 }
TanakaRobo 0:a202e1bc38a1 109
TanakaRobo 0:a202e1bc38a1 110 int RotaryInc::diff(){
TanakaRobo 0:a202e1bc38a1 111 int diff = pulse - prev;
TanakaRobo 0:a202e1bc38a1 112 prev = pulse;
TanakaRobo 0:a202e1bc38a1 113 return diff;
TanakaRobo 0:a202e1bc38a1 114 }
TanakaRobo 0:a202e1bc38a1 115
TanakaRobo 0:a202e1bc38a1 116 void RotaryInc::reset(){
TanakaRobo 0:a202e1bc38a1 117 pulse = 0;
TanakaRobo 0:a202e1bc38a1 118 prev = 0;
TanakaRobo 0:a202e1bc38a1 119 if(measur)zero();
TanakaRobo 0:a202e1bc38a1 120 }
TanakaRobo 0:a202e1bc38a1 121
TanakaRobo 0:a202e1bc38a1 122 RotaryInc::~RotaryInc(){
TanakaRobo 0:a202e1bc38a1 123 A->disable_irq();
TanakaRobo 0:a202e1bc38a1 124 B->disable_irq();
TanakaRobo 0:a202e1bc38a1 125 delete A;
TanakaRobo 0:a202e1bc38a1 126 delete B;
TanakaRobo 0:a202e1bc38a1 127 }