this is disco time
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of DagonFly__RoadToJapan_17Mei by
encoderKRAI.cpp
00001 /********************************************************/ 00002 /* Library untuk pembacaan Encoder */ 00003 /* Adapsi dari QEI */ 00004 /* */ 00005 /* Encoder yang sudah dicoba : */ 00006 /* 1. Autonics */ 00007 /* 2. Encoder bawaan Motor */ 00008 /* */ 00009 /* ______________________ */ 00010 /* |______Autonics______| */ 00011 /* | Out A = Input 1 | */ 00012 /* | Out B = Input 2 | */ 00013 /* | 5V | */ 00014 /* |_Gnd________________| */ 00015 /* */ 00016 /********************************************************/ 00017 00018 #include "mbed.h" 00019 00020 #include "encoderKRAI.h" 00021 00022 encoderKRAI::encoderKRAI(PinName channelA, 00023 PinName channelB, 00024 int pulsesPerRev, 00025 Encoding encoding) : channelA_(channelA), channelB_(channelB) 00026 { 00027 pulses_ = 0; 00028 revolutions_ = 0; 00029 pulsesPerRev_ = pulsesPerRev; 00030 encoding_ = encoding; 00031 00032 //Workout what the current state is. 00033 int chanA = channelA_.read(); 00034 int chanB = channelB_.read(); 00035 00036 //2-bit state. 00037 currState_ = (chanA << 1) | (chanB); 00038 prevState_ = currState_; 00039 00040 //X2 encoding uses interrupts on only channel A. 00041 //X4 encoding uses interrupts on channel A, 00042 //and on channel B. 00043 channelA_.rise(this, &encoderKRAI::encode); 00044 channelA_.fall(this, &encoderKRAI::encode); 00045 00046 //If we're using X4 encoding, then attach interrupts to channel B too. 00047 if (encoding == X4_ENCODING) { 00048 channelB_.rise(this, &encoderKRAI::encode); 00049 channelB_.fall(this, &encoderKRAI::encode); 00050 } 00051 } 00052 00053 void encoderKRAI::reset(void) { 00054 00055 pulses_ = 0; 00056 revolutions_ = 0; 00057 00058 } 00059 00060 /*int encoderKRAI::getCurrentState(void) { 00061 00062 return currState_; 00063 00064 }*/ 00065 00066 int encoderKRAI::getPulses(void) { 00067 00068 return pulses_; 00069 00070 } 00071 00072 int encoderKRAI::getRevolutions(void) { 00073 00074 revolutions_ = pulses_ / pulsesPerRev_; 00075 return revolutions_; 00076 00077 } 00078 00079 //////////////////////////////////////////////////////// 00080 00081 void encoderKRAI::encode(void) { 00082 00083 int change = 0; 00084 int chanA = channelA_.read(); 00085 int chanB = channelB_.read(); 00086 00087 //2-bit state. 00088 currState_ = (chanA << 1) | (chanB); 00089 00090 if (encoding_ == X2_ENCODING) { 00091 00092 //11->00->11->00 is counter clockwise rotation or "forward". 00093 if ((prevState_ == 0x3 && currState_ == 0x0) || 00094 (prevState_ == 0x0 && currState_ == 0x3)) { 00095 00096 pulses_++; 00097 00098 } 00099 //10->01->10->01 is clockwise rotation or "backward". 00100 else if ((prevState_ == 0x2 && currState_ == 0x1) || 00101 (prevState_ == 0x1 && currState_ == 0x2)) { 00102 00103 pulses_--; 00104 00105 } 00106 00107 } else if (encoding_ == X4_ENCODING) { 00108 00109 //Entered a new valid state. 00110 if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) { 00111 //2 bit state. Right hand bit of prev XOR left hand bit of current 00112 //gives 0 if clockwise rotation and 1 if counter clockwise rotation. 00113 change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1); 00114 00115 if (change == 0) { 00116 change = -1; 00117 } 00118 00119 pulses_ -= change; 00120 } 00121 00122 } 00123 00124 prevState_ = currState_; 00125 00126 }
Generated on Thu Jul 14 2022 11:58:13 by 1.7.2