Diff: EncoderMotor.cpp

Revision:

0:cd1a027f038d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderMotor.cpp	Mon Sep 16 10:06:23 2019 +0000
@@ -0,0 +1,131 @@
+/********************************************************/
+/*          Library untuk pembacaan Encoder             */
+/*                  Adapsi dari QEI                     */
+/*                                                      */
+/*  Encoder yang sudah dicoba :                         */
+/*  1. Autonics                                         */
+/*  2. Encoder bawaan Motor                             */
+/*                                                      */
+/*  ______________________                              */
+/*  |______Autonics______|                              */
+/*  | Out A = Input 1    |                              */
+/*  | Out B = Input 2    |                              */
+/*  | 5V                 |                              */
+/*  |_Gnd________________|                              */
+/*                                                      */
+/********************************************************/
+
+#include "mbed.h"
+#include "EncoderMotor.h"
+
+EncoderMotor::EncoderMotor(PinName channelA, PinName channelB, float pulsesPerRev,
+    Encoding encoding): channelA_(channelA), channelB_(channelB)
+{
+    pulses_ = 0;
+    revolutions_ = 0;
+    pulsesPerRev_ = pulsesPerRev;
+    encoding_ = encoding;
+
+    //Workout what the current state is.
+    int chanA = channelA_.read();
+    int chanB = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+    prevState_ = currState_;
+
+    //X2 encoding uses interrupts on only channel A.
+    //X4 encoding uses interrupts on      channel A,
+    //and on channel B.
+    channelA_.rise(this, &EncoderMotor::encode);
+    channelA_.fall(this, &EncoderMotor::encode);
+
+    //If we're using X4 encoding, then attach interrupts to channel B too.
+    if (encoding == X4_ENCODING) 
+    {
+        channelB_.rise(this, &EncoderMotor::encode);
+        channelB_.fall(this, &EncoderMotor::encode);
+    }
+}
+
+void EncoderMotor::reset(void)
+{
+    pulses_      = 0;
+    revolutions_ = 0;
+}
+
+/*int EncoderMotor::getCurrentState(void)
+{
+    return currState_;
+}*/
+
+int EncoderMotor::getPulses(void)
+{
+    return pulses_;
+}
+
+float EncoderMotor::getRevolutions(void)
+{
+    revolutions_ = (float) pulses_ / pulsesPerRev_;
+    double x = revolutions_ ;
+    pulses_ = 0;
+    revolutions_ = 0;    
+    return x;   
+}
+
+void EncoderMotor::encode(void)
+{
+    int change = 0;
+    int chanA = channelA_.read();
+    int chanB = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+
+    if (encoding_ == X2_ENCODING)
+    {
+        //11->00->11->00 is counter clockwise rotation or "forward".
+        if ((prevState_ == 0x3 && currState_ == 0x0) ||
+                (prevState_ == 0x0 && currState_ == 0x3))
+        {
+            pulses_++;
+        }
+        //10->01->10->01 is clockwise rotation or "backward".
+        else if ((prevState_ == 0x2 && currState_ == 0x1) ||
+            (prevState_ == 0x1 && currState_ == 0x2)) 
+        {
+            pulses_--;
+        }
+
+    } 
+    else if (encoding_ == X4_ENCODING)
+    {
+        //Entered a new valid state.
+        if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) 
+        {
+            //2 bit state. Right hand bit of prev XOR left hand bit of current
+            //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
+            change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
+            if (change == 0)
+            {
+                change = -1;
+            }
+
+            pulses_ -= change;
+        }
+    }
+
+    prevState_ = currState_;
+}
+
+void EncoderMotor::disableInterrupts(void) 
+{
+    channelA_.disable_irq();
+    channelB_.disable_irq();
+}
+
+void EncoderMotor::enableInterrupts(void)
+{
+    channelA_.enable_irq();
+    channelB_.enable_irq();    
+}