Revision 0:e2d42e66534c, committed 2018-08-22

Comitter:

Waaaaat

Date:

Wed Aug 22 13:41:41 2018 +0000

Commit message:

encoder board mbed;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.cpp	Wed Aug 22 13:41:41 2018 +0000
@@ -0,0 +1,289 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |  ^  |     |     |     |     |
+ *            ---+  ^  +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  +-----+     +-----+     +-----+
+ * Channel B     ^  |     |     |     |     |     |
+ *            ------+     +-----+     +-----+     +-----
+ *               ^  ^
+ *               ^  ^
+ *               90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^     ^     ^     ^     ^
+ *               ^     ^     ^     ^     ^
+ * Pulse count 0 1     2     3     4     5  ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ *               +-----+     +-----+     +-----+
+ * Channel A     |     |     |     |     |     |
+ *            ---+     +-----+     +-----+     +-----
+ *               ^     ^     ^     ^     ^
+ *               ^  +-----+  ^  +-----+  ^  +-----+
+ * Channel B     ^  |  ^  |  ^  |  ^  |  ^  |     |
+ *            ------+  ^  +-----+  ^  +-----+     +--
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ *               ^  ^  ^  +--+  ^  ^  +--+  ^
+ *               ^  ^  ^  |  |  ^  ^  |  |  ^
+ * Index      ------------+  +--------+  +-----------
+ *               ^  ^  ^  ^  ^  ^  ^  ^  ^  ^
+ * Pulse count 0 1  2  3  4  5  6  7  8  9  ...
+ * Rev.  count 0          1           2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+/**
+ * Includes
+ */
+#include "QEI.h"
+
+QEI::QEI(PinName channelA,
+         PinName channelB,
+         PinName index,
+         int pulsesPerRev,
+         Encoding encoding) : channelA_(channelA), channelB_(channelB),
+        index_(index) {
+
+    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, &QEI::encode);
+    channelA_.fall(this, &QEI::encode);
+
+    //If we're using X4 encoding, then attach interrupts to channel B too.
+    if (encoding == X4_ENCODING) {
+        channelB_.rise(this, &QEI::encode);
+        channelB_.fall(this, &QEI::encode);
+    }
+    //Index is optional.
+    if (index !=  NC) {
+        index_.rise(this, &QEI::index);
+    }
+
+}
+
+void QEI::reset(void) {
+
+    pulses_      = 0;
+    revolutions_ = 0;
+
+}
+
+int QEI::getCurrentState(void) {
+
+    return currState_;
+
+}
+
+long QEI::getPulses(void) {
+
+    return pulses_;
+
+}
+
+int QEI::getRevolutions(void) {
+
+    return revolutions_;
+
+}
+
+// +-------------+
+// | 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 QEI::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 QEI::index(void) {
+
+    revolutions_++;
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.h	Wed Aug 22 13:41:41 2018 +0000
@@ -0,0 +1,75 @@
+
+
+#ifndef QEI_H
+#define QEI_H
+
+
+#include "mbed.h"
+
+#define PREV_MASK 0x1 //Mask for the previous state in determining direction
+//of rotation.
+#define CURR_MASK 0x2 //Mask for the current state in determining direction
+//of rotation.
+#define INVALID   0x3 //XORing two states where both bits have changed.
+
+class QEI {
+
+public:
+
+    typedef enum Encoding {
+
+        X2_ENCODING,
+        X4_ENCODING
+
+    } Encoding;
+
+
+    QEI(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+
+    void reset(void);
+
+    int getCurrentState(void);
+
+
+    void set(int pul , int rev);  
+     
+    long getPulses(void);
+
+    int getRevolutions(void);
+    
+    int getAng_rev();
+    
+    double getAngle();
+    double getSumangle();
+    double getRPM();
+    double getRPS();
+    double getRPMS();
+    double getRPUS();
+    int          pulsesPerRev_;
+private:
+    Timer Mper , Rper ,MSper ,USper; 
+    Ticker Tick;
+    double RPM , RPS ,RPMS , RPUS;
+
+    void encode(void);
+
+ 
+    void index(void);
+
+    Encoding encoding_;
+
+    InterruptIn channelA_;
+    InterruptIn channelB_;
+    InterruptIn index_;
+    int          round_rev;
+
+    int          prevState_;
+    int          currState_;
+    double angle_ , sumangle;
+    int angle_pulses;
+    volatile int pulses_;
+    volatile int revolutions_;
+
+};
+
+#endif 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Aug 22 13:41:41 2018 +0000
@@ -0,0 +1,102 @@
+#include "mbed.h"
+#include"QEI.h"
+#include"I2CSlave.h"
+
+#define ROTATE_PER_REVOLUTIONS  2048 //エンコーダ分解能
+
+// エンコーダ読み取りピン
+#define CHANNNEL1_A D2
+#define CHANNNEL1_B D3
+
+#define CHANNNEL2_A D7
+#define CHANNNEL2_B D6
+
+#define CHANNNEL3_A D8
+#define CHANNNEL3_B D9
+
+#define CHANNNEL4_A D10
+#define CHANNNEL4_B D11
+
+//i2c
+#define SDA D4
+#define SCL D5
+
+#define ADDR 0x02 //i2cアドレス
+
+//inputにセット
+DigitalIn Ch1A(CHANNNEL1_A),Ch1B(CHANNNEL1_B);
+DigitalIn Ch2A(CHANNNEL2_A),Ch2B(CHANNNEL2_B);
+DigitalIn Ch3A(CHANNNEL3_A),Ch3B(CHANNNEL3_B);
+DigitalIn Ch4A(CHANNNEL4_A),Ch4B(CHANNNEL4_B);
+
+Serial pc(USBTX, USBRX);
+I2CSlave slave(SDA,SCL);
+
+QEI enc1(CHANNNEL1_A,CHANNNEL1_B,NC,ROTATE_PER_REVOLUTIONS);
+QEI enc2(CHANNNEL2_A,CHANNNEL2_B,NC,ROTATE_PER_REVOLUTIONS);
+QEI enc3(CHANNNEL3_A,CHANNNEL3_B,NC,ROTATE_PER_REVOLUTIONS);
+QEI enc4(CHANNNEL4_A,CHANNNEL4_B,NC,ROTATE_PER_REVOLUTIONS);
+
+void setup(){ 
+    
+    enc1.reset();
+    enc2.reset();
+    enc3.reset();
+    enc4.reset();
+    
+    Ch1A.mode(PullUp);
+    Ch1B.mode(PullUp);
+    Ch2A.mode(PullUp);
+    Ch2B.mode(PullUp);
+    Ch3A.mode(PullUp);
+    Ch3B.mode(PullUp);
+    Ch4A.mode(PullUp);
+    Ch4B.mode(PullUp);
+}
+int main(){
+    
+    setup();
+    slave.frequency(400000);
+    slave.address(ADDR);
+    
+    static long enc1Data,enc2Data,enc3Data,enc4Data;
+    
+    char buf[16];
+    
+    while(1) {
+      
+    int  i=slave.receive();
+      
+        enc1Data=enc1.getPulses();
+        enc2Data=-enc2.getPulses();
+        enc3Data=enc3.getPulses();
+        enc4Data=enc4.getPulses();
+                  
+          buf[0]=enc1Data>>24 & 0x00ff;
+          buf[1]=enc1Data>>16 & 0x00ff;
+          buf[2]=enc1Data>>8 & 0x00ff;
+          buf[3]=enc1Data & 0x00ff;
+          
+          buf[4]=enc2Data>>24 & 0x00ff;
+          buf[5]=enc2Data>>16 & 0x00ff;
+          buf[6]=enc2Data>>8 & 0x00ff;
+          buf[7]=enc2Data & 0x00ff;
+          
+          buf[8]=enc3Data>>24 & 0x00ff;
+          buf[9]=enc3Data>>16 & 0x00ff;
+          buf[10]=enc3Data>>8 & 0x00ff;
+          buf[11]=enc3Data & 0x00ff;
+          
+          buf[12]=enc4Data>>24 & 0x00ff;
+          buf[13]=enc4Data>>16 & 0x00ff;
+          buf[14]=enc4Data>>8 & 0x00ff;
+          buf[15]=enc4Data & 0x00ff;
+          
+        pc.printf("%d %d %d %d\n",enc1Data,enc2Data,enc3Data,enc4Data);
+        
+        //マスターからのリクエストが来た際にデータを送信
+        if(i==I2CSlave::ReadAddressed){
+        slave.write(buf,16);      
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Aug 22 13:41:41 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/675da3299148
\ No newline at end of file