Revision 4:8e6dfc416ecf, committed 2017-02-09

Comitter:

Joshua23

Date:

Thu Feb 09 14:34:10 2017 +0000

Parent:

3:e4cbba0706b0

Commit message:

coba

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.cpp	Thu Feb 09 14:34:10 2017 +0000
@@ -0,0 +1,126 @@
+/********************************************************/
+/*          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 "encoderKRAI.h"
+
+encoderKRAI::encoderKRAI(PinName channelA,
+                         PinName channelB,
+                         int 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, &encoderKRAI::encode);
+    channelA_.fall(this, &encoderKRAI::encode);
+
+    //If we're using X4 encoding, then attach interrupts to channel B too.
+    if (encoding == X4_ENCODING) {
+        channelB_.rise(this, &encoderKRAI::encode);
+        channelB_.fall(this, &encoderKRAI::encode);
+    }
+}
+
+void encoderKRAI::reset(void) {
+
+    pulses_      = 0;
+    revolutions_ = 0;
+
+}
+
+/*int encoderKRAI::getCurrentState(void) {
+
+    return currState_;
+
+}*/
+
+int encoderKRAI::getPulses(void) {
+
+    return pulses_;
+
+}
+
+int encoderKRAI::getRevolutions(void) {
+
+    revolutions_ = pulses_ / pulsesPerRev_;
+    return revolutions_;
+
+}
+
+////////////////////////////////////////////////////////
+
+void encoderKRAI::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_;
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.h	Thu Feb 09 14:34:10 2017 +0000
@@ -0,0 +1,106 @@
+#ifndef ENCODERKRAI_H
+#define ENCODERKRAI_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#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.
+
+/**
+ * Quadrature Encoder Interface.
+ */
+class encoderKRAI {
+
+public:
+
+    typedef enum Encoding {
+
+        X2_ENCODING,
+        X4_ENCODING
+
+    } Encoding;
+    
+    /** Membuat interface dari encoder    
+     *
+     * @param inA DigitalIn, out A dari encoder
+     * @param inB DigitalIn, out B dari encoder
+     */
+    encoderKRAI(PinName channelA, PinName channelB, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+    
+    /**
+     * Reset encoder.
+     *
+     * Menset pulse dan revolusi/putaran menjadi 0
+     */
+    void reset(void);
+    
+    /**
+     * Membaca pulse yang didapat oleh encoder
+     *
+     * @return Nilai pulse yang telah dilalui.
+     */
+    int getPulses(void);
+    
+    /**
+     * Membaca putaran yang didapat oleh encoder
+     *
+     * @return Nilai revolusi/putaran yang telah dilalui.
+     */
+    int getRevolutions(void);
+    
+    /**
+     * Membaca pulse yang didapat oleh encoder
+     *
+     * @return Nilai pulse yang telah dilalui.
+     */
+    //int readDeltaPulses(void);
+    
+    /**
+     * Membaca putaran yang didapat oleh encoder
+     *
+     * @return Nilai revolusi/putaran yang telah dilalui.
+     */
+    //int readDeltaRevolutions(void);
+
+private:
+
+    /**
+     * Menghitung pulse
+     *
+     * Digunakan setiap rising/falling edge baik channel A atau B
+     * Membaca putaran CW atau CCW => mengakibatkan pertambahan/pengurangan pulse
+     */
+    void encode(void);
+
+    /**
+     * Indeks setiap rising edge untuk menghitung putaran
+     * Nilai bertambah 1
+     */
+    //void index(void);
+
+    Encoding encoding_;
+
+    InterruptIn channelA_;
+    InterruptIn channelB_;
+    //InterruptIn index_;
+
+    int          pulsesPerRev_;
+    int          prevState_;
+    int          currState_;
+
+    volatile int pulses_;
+    volatile int revolutions_;
+
+
+};
+
+#endif /* ENCODERKRAI_H */
\ No newline at end of file

--- a/main.cpp	Tue Oct 18 12:28:57 2016 +0000
+++ b/main.cpp	Thu Feb 09 14:34:10 2017 +0000
@@ -16,16 +16,16 @@
     unsigned long int previousMillis = 0;
     
     //Variabel Motor
-    Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev
+    Motor motor1(PA_8, PC_2, PC_1); // pwm, fwd, rev
     
     // Variabel PID    
-    int errorP, last_errorD;
-    float pwm, pwm_last;
+    int errorP = 0, last_errorD = 0;
+    float pwm = 0, pwm_last;
     float P, Kp;
     float D, Kd;
 
     // Variabel Sabeb
-    int desired_rpm = 100;
+    int desired_rpm = 50;
     
     // Test Variabel
     int selisiherror;
@@ -33,17 +33,17 @@
 // Main Program
  int main(void) { 
  
-   DigitalIn encoder0PinA(PA_5);
-   DigitalIn encoder0PinB(PA_6);
-   Serial pc(USBTX,USBRX);
-   pc.baud(9600);
+    DigitalIn encoder0PinA(PB_14);
+    DigitalIn encoder0PinB(PB_13);
+    Serial pc(USBTX,USBRX);
+    pc.baud(9600);
     startMillis();
     
     // Deklarasi awal PID
     last_errorD = 0;
     pwm_last = 0;
-    Kp = 0.00000007;
-    Kd = 0.000000007;
+    Kp = 0.000000033;               //0.000000037
+    Kd = 0.000000297;               //0.000000270
     
     while(1){
     
@@ -58,9 +58,11 @@
         pwm = 1;
         }
     
+    
     motor1.speed(pwm);
     pwm_last = pwm;
     
+    //motor1.speed(0.8);
     unsigned long int currentMillis = millis();
 //    int counter;
 //    counter=currentMillis/100;
@@ -69,7 +71,7 @@
     {
         previousMillis = currentMillis;
         rpm = encoder0Pos;
-        pc.printf ("pwm = %.5f | rpm = %.0f | P = %.10f | D = %.10f | errorP = %.d | last_errorD = %.d \n",pwm,rpm,P,D, errorP, last_errorD);
+        pc.printf ("pwm = %.5f | rpm = %.0f | P = %.10f | D = %.10f | errorP = %.d | last_errorD = %.d \n",pwm,(rpm),P,D, errorP, last_errorD);
         encoder0Pos = 0;
            
         last_errorD = errorP;
@@ -86,7 +88,7 @@
          } else {
            encoder0Pos++;
          }
-         //pc.printf ("%d \n",encoder0Pos);
+        //pc.printf ("%d \n",encoder0Pos);
         }
         encoder0PinALast = n;
     }