Diff: src/encoder.cpp

Revision:

0:441289ea4e29

Child:

1:7c355adbc977

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/encoder.cpp	Thu May 27 18:36:23 2021 +0000
@@ -0,0 +1,199 @@
+#ifndef ENCODER_H
+#define ENCODER_H
+
+/* Karbot encoder class
+ * Written by Simon Krogedal
+ * 27/05/21
+ * Team 9 4th Year project
+ * 
+ * for NUCLEO-F401RE
+ * 
+ */
+ 
+#include "QEI.h"
+#include "encoder.h"
+ 
+ 
+encoder::encoder(
+    PinName chanA,
+    PinName chanB,
+    int CPR,
+    bool c,
+    double p,
+    double ec
+    ) : channelA_(channelA, PullUp),
+        channelB_(channelB, PullUp),
+        c_d(c),
+        period(p),
+        enc_const(ec) {
+                        
+    pulses_       = 0;
+    revolutions_  = 0;
+    pulsesPerRev_ = CPR;
+    tot_clicks    = 0;
+    click_rate    = 0;
+    temp_tot      = 0;
+    
+    //Workout what the current state is.
+    int chanA = channelA_.read();
+    int chanB = channelB_.read();
+    
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+    prevState_ = currState_;
+    
+    //X4 encoding uses interrupts on channel A,
+    //and on channel B.
+    channelA_.rise(callback(this, &QEI::encode));
+    channelA_.fall(callback(this, &QEI::encode));
+    channelB_.rise(callback(this, &QEI::encode));
+    channelB_.fall(callback(this, &QEI::encode));
+}
+
+void encode::reset(void) {
+
+    pulses_      = 0;
+    revolutions_ = 0;
+
+}
+
+int encode::getCurrentState(void) {
+
+    return currState_;
+
+}
+
+int encode::getPulses(void) {
+
+    return pulses_;
+
+}
+
+// +-------------+
+// | X2 Encoding |
+// +-------------+
+//
+// When observing states two patterns will appear:
+//
+// Counter clockwise rotation:
+//
+// 10 -> 01 -> 10 -> 01 -> ...
+//
+// Clockwise rotation:
+//
+// 11 -> 00 -> 11 -> 00 -> ...
+//
+// We consider counter clockwise rotation to be "forward" and
+// counter clockwise to be "backward". Therefore pulse count will increase
+// during counter clockwise rotation and decrease during clockwise rotation.
+//
+// +-------------+
+// | X4 Encoding |
+// +-------------+
+//
+// There are four possible states for a quadrature encoder which correspond to
+// 2-bit gray code.
+//
+// A state change is only valid if of only one bit has changed.
+// A state change is invalid if both bits have changed.
+//
+// Clockwise Rotation ->
+//
+//    00 01 11 10 00
+//
+// <- Counter Clockwise Rotation
+//
+// If we observe any valid state changes going from left to right, we have
+// moved one pulse clockwise [we will consider this "backward" or "negative"].
+//
+// If we observe any valid state changes going from right to left we have
+// moved one pulse counter clockwise [we will consider this "forward" or
+// "positive"].
+//
+// We might enter an invalid state for a number of reasons which are hard to
+// predict - if this is the case, it is generally safe to ignore it, update
+// the state and carry on, with the error correcting itself shortly after.
+void encoder::encode(void) {
+
+    int change = 0;
+    int chanA  = channelA_.read();
+    int chanB  = channelB_.read();
+
+    //2-bit state.
+    currState_ = (chanA << 1) | (chanB);
+
+        //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 encoder::sample_func(void) {
+    int clicks = getPulses();
+    click_rate = ((double)clicks / period);
+    reset();                                                    //reset clicks
+    tot_clicks += clicks;
+    temp_tot += clicks;
+}
+
+double encoder::getClicks(void) {
+//            test_sample();
+    if(c_d)
+        return click_rate;
+    else
+        return -click_rate;
+}
+
+double encoder::getSpeed(void) {
+    double s = click_rate * enc_const;                          //angular frequency = 2pi*CPS/CPR and v = omega*r
+    if(c_d)
+        return s;
+    else
+        return -s;
+}
+
+double encoder::getDistance(void) {                                      //calculates total distance and returns it
+//            test_sample();
+    double d = ((double)click_store * enc_const);
+    if(c_d)
+        return d;
+    else
+        return -d;
+}
+
+double encoder::tempDist(void) {                                     //calculates total distance and returns it
+//            test_sample();
+    double d = ((double)temp_tot * enc_const);
+    if(c_d)
+        return d;
+    else
+        return -d;
+}
+
+void encoder::distRst(void) {
+    int temp;
+    if(c_d)
+        temp = tot_clicks;
+    else
+        temp = -tot_clicks;
+    if(temp > click_store)
+        click_store = temp;
+    tot_clicks = 0;
+}
+
+void encoder::tempRst(void) {temp_tot = 0;}
+
+void encoder::start(void) {sampler.attach(callback(this, &encoder::sample_func), period);}
+
+#endif
\ No newline at end of file