Yolanda Tania / Mbed 2 deprecated PID_tangan

udah bisa looo

Dependencies:   mbed

encoder_krai/encoderKRAI.cpp@1:0122c72f6e1b, 2020-02-27 (annotated)

Committer:
Yolandataniaa
Date:
Thu Feb 27 13:10:57 2020 +0000
Revision:
1:0122c72f6e1b
Parent:
0:aa8e05bc0533 
tangan kanan kamis 27 feb

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yolandataniaa 0:aa8e05bc0533 1 /********************************************************/
Yolandataniaa 0:aa8e05bc0533 2 /* Library untuk pembacaan Encoder */
Yolandataniaa 0:aa8e05bc0533 3 /* Adopsi dari QEI */
Yolandataniaa 0:aa8e05bc0533 4 /* Bismillahirahmanirrahim */
Yolandataniaa 0:aa8e05bc0533 5 /* Encoder yang sudah dicoba : */
Yolandataniaa 0:aa8e05bc0533 6 /* 1. Autonics */
Yolandataniaa 0:aa8e05bc0533 7 /* 2. Encoder bawaan Motor */
Yolandataniaa 0:aa8e05bc0533 8 /* */
Yolandataniaa 0:aa8e05bc0533 9 /* ______________________ */
Yolandataniaa 0:aa8e05bc0533 10 /* |______Autonics______| */
Yolandataniaa 0:aa8e05bc0533 11 /* | Out A = Input 1 | */
Yolandataniaa 0:aa8e05bc0533 12 /* | Out B = Input 2 | */
Yolandataniaa 0:aa8e05bc0533 13 /* | 5V | */
Yolandataniaa 0:aa8e05bc0533 14 /* |_Gnd________________| */
Yolandataniaa 0:aa8e05bc0533 15 /* */
Yolandataniaa 0:aa8e05bc0533 16 /********************************************************/
Yolandataniaa 0:aa8e05bc0533 17
Yolandataniaa 0:aa8e05bc0533 18 #include "mbed.h"
Yolandataniaa 0:aa8e05bc0533 19 #include "encoderKRAI.h"
Yolandataniaa 0:aa8e05bc0533 20
Yolandataniaa 0:aa8e05bc0533 21 encoderKRAI::encoderKRAI(PinName channelA,
Yolandataniaa 0:aa8e05bc0533 22 PinName channelB,
Yolandataniaa 0:aa8e05bc0533 23 int pulsesPerRev,
Yolandataniaa 0:aa8e05bc0533 24 Encoding encoding) : channelA_(channelA), channelB_(channelB)
Yolandataniaa 0:aa8e05bc0533 25 {
Yolandataniaa 0:aa8e05bc0533 26 pulses_ = 0;
Yolandataniaa 0:aa8e05bc0533 27 revolutions_ = 0;
Yolandataniaa 0:aa8e05bc0533 28 pulsesPerRev_ = pulsesPerRev;
Yolandataniaa 0:aa8e05bc0533 29 encoding_ = encoding;
Yolandataniaa 0:aa8e05bc0533 30
Yolandataniaa 0:aa8e05bc0533 31 //Workout what the current state is.
Yolandataniaa 0:aa8e05bc0533 32 int chanA = channelA_.read();
Yolandataniaa 0:aa8e05bc0533 33 int chanB = channelB_.read();
Yolandataniaa 0:aa8e05bc0533 34
Yolandataniaa 0:aa8e05bc0533 35 //2-bit state.
Yolandataniaa 0:aa8e05bc0533 36 currState_ = (chanA << 1) | (chanB);
Yolandataniaa 0:aa8e05bc0533 37 prevState_ = currState_;
Yolandataniaa 0:aa8e05bc0533 38
Yolandataniaa 0:aa8e05bc0533 39 //X2 encoding uses interrupts on only channel A.
Yolandataniaa 0:aa8e05bc0533 40 //X4 encoding uses interrupts on channel A,
Yolandataniaa 0:aa8e05bc0533 41 //and on channel B.
Yolandataniaa 0:aa8e05bc0533 42 channelA_.rise(this, &encoderKRAI::encode);
Yolandataniaa 0:aa8e05bc0533 43 channelA_.fall(this, &encoderKRAI::encode);
Yolandataniaa 0:aa8e05bc0533 44
Yolandataniaa 0:aa8e05bc0533 45 //If we're using X4 encoding, then attach interrupts to channel B too.
Yolandataniaa 0:aa8e05bc0533 46 if (encoding == X4_ENCODING) {
Yolandataniaa 0:aa8e05bc0533 47 channelB_.rise(this, &encoderKRAI::encode);
Yolandataniaa 0:aa8e05bc0533 48 channelB_.fall(this, &encoderKRAI::encode);
Yolandataniaa 0:aa8e05bc0533 49 }
Yolandataniaa 0:aa8e05bc0533 50 }
Yolandataniaa 0:aa8e05bc0533 51
Yolandataniaa 0:aa8e05bc0533 52 void encoderKRAI::reset(void) {
Yolandataniaa 0:aa8e05bc0533 53
Yolandataniaa 0:aa8e05bc0533 54 pulses_ = 0;
Yolandataniaa 0:aa8e05bc0533 55 revolutions_ = 0;
Yolandataniaa 0:aa8e05bc0533 56
Yolandataniaa 0:aa8e05bc0533 57 }
Yolandataniaa 0:aa8e05bc0533 58
Yolandataniaa 0:aa8e05bc0533 59 int encoderKRAI::getPulses(void) {
Yolandataniaa 0:aa8e05bc0533 60
Yolandataniaa 0:aa8e05bc0533 61 return pulses_;
Yolandataniaa 0:aa8e05bc0533 62
Yolandataniaa 0:aa8e05bc0533 63 }
Yolandataniaa 0:aa8e05bc0533 64
Yolandataniaa 0:aa8e05bc0533 65 int encoderKRAI::getRevolutions(void) {
Yolandataniaa 0:aa8e05bc0533 66
Yolandataniaa 0:aa8e05bc0533 67 revolutions_ = (float)pulses_ / pulsesPerRev_;
Yolandataniaa 0:aa8e05bc0533 68 double x = revolutions_ ;
Yolandataniaa 0:aa8e05bc0533 69 pulses_ = 0;
Yolandataniaa 0:aa8e05bc0533 70 revolutions_ = 0;
Yolandataniaa 0:aa8e05bc0533 71 return x;
Yolandataniaa 0:aa8e05bc0533 72 }
Yolandataniaa 0:aa8e05bc0533 73
Yolandataniaa 0:aa8e05bc0533 74 /***************************************
Yolandataniaa 0:aa8e05bc0533 75 * Perhitungan Pulse Encoder
Yolandataniaa 0:aa8e05bc0533 76 ***************************************/
Yolandataniaa 0:aa8e05bc0533 77
Yolandataniaa 0:aa8e05bc0533 78 void encoderKRAI::encode(void) {
Yolandataniaa 0:aa8e05bc0533 79
Yolandataniaa 0:aa8e05bc0533 80 int change = 0;
Yolandataniaa 0:aa8e05bc0533 81 int chanA = channelA_.read();
Yolandataniaa 0:aa8e05bc0533 82 int chanB = channelB_.read();
Yolandataniaa 0:aa8e05bc0533 83
Yolandataniaa 0:aa8e05bc0533 84 //2-bit state.
Yolandataniaa 0:aa8e05bc0533 85 currState_ = (chanA << 1) | (chanB);
Yolandataniaa 0:aa8e05bc0533 86
Yolandataniaa 0:aa8e05bc0533 87 if (encoding_ == X2_ENCODING) {
Yolandataniaa 0:aa8e05bc0533 88
Yolandataniaa 0:aa8e05bc0533 89 //11->00->11->00 is counter clockwise rotation or "forward".
Yolandataniaa 0:aa8e05bc0533 90 if ((prevState_ == 0x3 && currState_ == 0x0) ||
Yolandataniaa 0:aa8e05bc0533 91 (prevState_ == 0x0 && currState_ == 0x3)) {
Yolandataniaa 0:aa8e05bc0533 92
Yolandataniaa 0:aa8e05bc0533 93 pulses_++;
Yolandataniaa 0:aa8e05bc0533 94
Yolandataniaa 0:aa8e05bc0533 95 }
Yolandataniaa 0:aa8e05bc0533 96 //10->01->10->01 is clockwise rotation or "backward".
Yolandataniaa 0:aa8e05bc0533 97 else if ((prevState_ == 0x2 && currState_ == 0x1) ||
Yolandataniaa 0:aa8e05bc0533 98 (prevState_ == 0x1 && currState_ == 0x2)) {
Yolandataniaa 0:aa8e05bc0533 99
Yolandataniaa 0:aa8e05bc0533 100 pulses_--;
Yolandataniaa 0:aa8e05bc0533 101
Yolandataniaa 0:aa8e05bc0533 102 }
Yolandataniaa 0:aa8e05bc0533 103
Yolandataniaa 0:aa8e05bc0533 104 } else if (encoding_ == X4_ENCODING) {
Yolandataniaa 0:aa8e05bc0533 105
Yolandataniaa 0:aa8e05bc0533 106 //Entered a new valid state.
Yolandataniaa 0:aa8e05bc0533 107 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
Yolandataniaa 0:aa8e05bc0533 108 //2 bit state. Right hand bit of prev XOR left hand bit of current
Yolandataniaa 0:aa8e05bc0533 109 //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
Yolandataniaa 0:aa8e05bc0533 110 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
Yolandataniaa 0:aa8e05bc0533 111
Yolandataniaa 0:aa8e05bc0533 112 if (change == 0) {
Yolandataniaa 0:aa8e05bc0533 113 change = -1;
Yolandataniaa 0:aa8e05bc0533 114 }
Yolandataniaa 0:aa8e05bc0533 115
Yolandataniaa 0:aa8e05bc0533 116 pulses_ -= change;
Yolandataniaa 0:aa8e05bc0533 117 }
Yolandataniaa 0:aa8e05bc0533 118
Yolandataniaa 0:aa8e05bc0533 119 }
Yolandataniaa 0:aa8e05bc0533 120
Yolandataniaa 0:aa8e05bc0533 121 prevState_ = currState_;
Yolandataniaa 0:aa8e05bc0533 122
Yolandataniaa 0:aa8e05bc0533 123 }