Revision 0:b455cd43929c, committed 2016-11-07

Comitter:

gustavaditya

Date:

Mon Nov 07 12:24:01 2016 +0000

Commit message:

Coba-riset-PID

Changed in this revision

diff -r 000000000000 -r b455cd43929c PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Mon Nov 07 12:24:01 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

diff -r 000000000000 -r b455cd43929c QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Mon Nov 07 12:24:01 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

diff -r 000000000000 -r b455cd43929c encoderKRAI.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.cpp	Mon Nov 07 12:24:01 2016 +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

diff -r 000000000000 -r b455cd43929c encoderKRAI.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.h	Mon Nov 07 12:24:01 2016 +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

diff -r 000000000000 -r b455cd43929c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 07 12:24:01 2016 +0000
@@ -0,0 +1,122 @@
+/****************************************************************************/
+/*                  PROGRAM UNTUK PID CLOSED LOOP                           */
+/*                                                                          */
+/*  - Digunakan encoder autonics                                            */
+/*  - Konfigurasi Motor dan Encoder sbb :                                   */
+/*        _________________                                                 */
+/*        |     DEPAN      |                                                */
+/*        | 1.    e     .2 |    Angka ==> Motor                             */
+/*        | `            ` |    e     ==> Encoder                           */
+/*        | e            e |                                                */
+/*        | .            . |                                                */
+/*        | 4`    e     `3 |                                                */
+/*        |________________|                                                */
+/*                                                                          */
+/*   SETTINGS (WAJIB!) :                                                    */
+/*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h      */
+/*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
+/*                                                                          */
+/****************************************************************************/
+
+#include "mbed.h"
+#include "Motor.h"
+#include "omniPos.h"
+
+#define diameterRoda 10
+#define delta_t 0.001
+
+// Deklarasi variabel motor
+Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev
+Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev
+Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev
+Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev
+
+// Deklarasi variabel encoder
+//~ Dkiri untuk Motor 1
+//~ Dkanan untuk Motor 2        
+//~ Bkanan untuk Motor 3        
+//~ Bkiri untuk Motor 4
+omniPos omni1(Dkiri);
+omniPos omni2(Dkanan);
+omniPos omni3(Bkiri);
+omniPos omni4(Bkiri);
+
+// Inisialisasi  Pin TX-RX Joystik dan PC
+Serial pc(USBTX,USBRX);
+
+// Deklarasi Variabel Global
+/*
+ * posX dan posY berdasarkan arah robot
+ * encoder Depan & Belakang sejajar sumbu Y
+ * encoder Kanan & Kiri sejajar sumbu X 
+ */
+float jarak, posX, posY;
+float keliling = pi*diameterRoda;
+
+void detect_encoder()
+{
+    int pv;
+    // Motor1
+    
+    PID.setProcessValue(omni1.getVel(delta_t));
+    PID.setSetPoint();
+}
+
+int main()
+{
+    pc.baud(115200);
+    
+}
+
+void gerakKanan()
+{
+    if(vcurr<0.1) {
+        vcurr=0.1;
+    } else {
+        vcurr+=ax;
+    }
+    //perlambatan=0;
+            } else {
+                //perlambatan=1;
+            } 
+            
+            if (vcurr>=vmax) { 
+                vcurr=vmax; 
+            }
+            
+            if(joystick.R2==255 && joystick.L2==0) { 
+                koef=2;  
+            } else if (joystick.L2==255 && joystick.R2==0)  { 
+                koef=0.5;
+            } else { 
+                koef=1;  
+            }
+            
+            s1 =(float)(-1*koef*vcurr);
+            s2 =(float)(-1.0*koef*vcurr); 
+            s3 =(float)(1*koef*vcurr); 
+            s4 =(float)(1.0*koef*vcurr);           
+            
+            kanan=true; 
+            maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            
+            pc.printf("Kanan\n");
+            
+            motor1.speed(s1);
+            motor2.speed(s2);
+            motor3.speed(s3);
+            motor4.speed(s4);
+            break;
+        }    
+}
+
+void gerakKiri()
+{
+    float revDepan, revBelakang, revKanan, revKiri;
+    float tempX, tempY;
+    
+    revDepan    = encoderDepan.getRevolutions();
+    revBelakang = encoderBelakang.getRevolutions();
+    revKanan    = encoderKanan.getRevolutions();
+    revKiri     = encoderKiri.getRevolutions();    
+}
\ No newline at end of file

diff -r 000000000000 -r b455cd43929c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Nov 07 12:24:01 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9bcdf88f62b0
\ No newline at end of file

diff -r 000000000000 -r b455cd43929c omniPos.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omniPos.cpp	Mon Nov 07 12:24:01 2016 +0000
@@ -0,0 +1,100 @@
+//Includes
+#include "omniPos.h"
+#define pi 22/7
+#define diaEncoder 0.058
+#define PPR 1000
+#define diaRobot 0.64
+
+omniPos::omniPos(Wheel wheel)
+{
+    int indeks;
+    //int alpha[4] = {0,90,180,270};
+    float K_enc = pi*diaEncoder;
+    float K_robot = pi*diaRobot;
+    
+    switch (wheel)
+    { 
+        case (Dkiri) : {indeks=1; break;}
+        case (Dkanan) : {indeks=2; break;}
+        case (Bkanan) : {indeks=3; break;}
+        case (Bkiri) : {indeks=4; break;}
+    }
+}
+
+void omniPos::setCenter(void)
+{
+    encoderDepan.reset();
+    encoderBelakang.reset();
+    encoderKanan.reset();
+    encoderKiri.reset();
+}
+
+float getX(void)
+{
+    float  jarakEncDpn, jarakEncBlk;
+    jarakEncDpn = encoderDepan.getRevolutions()*Kel + (encoderDepan.getPulses()%(2*PPR))*Kel/(2*PPR);
+    jarakEncBlk = encoderBelakang.getRevolutions()*Kel + (encoderBelakang.getPulses()%(2*PPR))*Kel/(2*PPR);
+    if (jarakEncDpn>=0  && jarakEncBlk<0)
+    {
+        return (abs(jarakEncDpn)+abs(jarakEncBlk))/2;
+    }
+    else if (jarakEncDpn<0  && jarakEncBlk>0)
+    {
+        return -(abs(jarakEncDpn)+abs(jarakEncBlk))/2;
+    }
+}
+
+float getY(void)
+{
+    float  jarakEncKir, jarakEncKan;
+    jarakEncKir = encoderKiri.getRevolutions()*Kel + (encoderKiri.getPulses()%(2*PPR))*Kel/(2*PPR);
+    jarakEncKan = encoderKanan.getRevolutions()*Kel + (encoderKanan.getPulses()%(2*PPR))*Kel/(2*PPR);
+    if (jarakEncKir>=0  && jarakEncKan<0)
+    {
+        return (abs(jarakEncKir)+abs(jarakEncKan))/2;
+    }
+    else if (jarakEncKir<0  && jarakEncKan>0)
+    {
+        return -(abs(jarakEncKir)+abs(jarakEncKan))/2;
+    }   
+}
+
+float getTetha(void)
+{
+    float jarakEncDpn, jarakEncBlk, jarakEncKir, jarakEncKan;
+    float busurDpn, busurBlk, busurKir, busurKan;
+    jarakEncDpn = encoderDepan.getRevolutions()*Kel + (encoderDepan.getPulses()%(2*PPR))*Kel/(2*PPR);
+    jarakEncBlk = encoderBelakang.getRevolutions()*Kel + (encoderBelakang.getPulses()%(2*PPR))*Kel/(2*PPR);
+    jarakEncKir = encoderKiri.getRevolutions()*Kel + (encoderKiri.getPulses()%(2*PPR))*Kel/(2*PPR);
+    jarakEncKan = encoderKanan.getRevolutions()*Kel + (encoderKanan.getPulses()%(2*PPR))*Kel/(2*PPR);
+    
+    busurDpn = (jarakEncDpn/K_robot)*360;
+    busurBlk = (jarakEncBlk/K_robot)*360;
+    busurKir = (jarakEncKir/K_robot)*360;
+    busurKan = (jarakEncKan/K_robot)*360;
+    
+    return -(busurDpn+busurBlk+busurKir+busurKan)/4; 
+      
+}
+
+float getPos()
+{
+    switch indeks
+    {
+        case(1) : {return getTetha()+90; break;}
+        case(2) : {return getTetha(); break;}
+        case(3) : {return getTetha()+270; break;}
+        case(4) : {return getTetha()+180; break;}    
+    }
+}
+
+float getVel(int delta_t)
+{
+       switch indeks
+    {
+        case(1) : {return getPos(1)/delta_t; break;}
+        case(2) : {return getPos(2)/delta_t; break;}
+        case(3) : {return getPos(3)/delta_t; break;}
+        case(4) : {return getPos(4)/delta_t; break;}    
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r b455cd43929c omniPos.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omniPos.h	Mon Nov 07 12:24:01 2016 +0000
@@ -0,0 +1,64 @@
+#ifndef OMNIPOS_H
+#define OMNIPOS_H
+
+#include "mbed.h"
+#include "QEI.h"
+
+#define diameterRoda 10
+
+// Deklarasi variabel encoder
+// Lebih jelas baca file QEI.h
+QEI encoderDepan( outA, outB, 2000, X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+QEI encoderBelakang( outA, outB, 2000, X2_ENCODING);
+QEI encoderKanan( outA, outB, 2000, X2_ENCODING);
+QEI encoderKiri( outA, outB, 2000, X2_ENCODING);
+
+class omniPos {
+public:
+
+    /** Dkiri untuk Motor 1
+     *  Dkanan untuk Motor 2        
+     *  Bkanan untuk Motor 3        
+     *  Bkiri untuk Motor 4
+     *      _________________
+     *      |     DEPAN     |
+     *      | 1.         .2 |
+     *      | `           ` |
+     *      | .           . |
+     *      | 4`         `3 |
+     *      |_______________|
+     *      
+     */
+    typedef enum Wheel {
+
+        Dkiri,
+        Dkanan,
+        Bkanan,
+        Bkiri
+
+    } Wheel;
+    
+    // Membuat interface dari omniPos
+    omniPos(Wheel wheel);
+    
+    // Set center/pusat posisi omni.
+    // Posisi sekarang saat di reset akan dijadikan pusat (0,0)
+    void setCenter(void);
+    
+    // Membaca posisi X-axis dari satu omni wheel
+    float getX(void);
+    
+    // Membaca posisi Y-axis dari satu omni wheel
+    float getY(void);
+    
+    // Membaca tetha atau sudut rotasi robot
+    float getTetha(void);
+    
+    // Membaca posisi 1 omni wheel ==> fi (rad)
+    float getPos();
+    
+    // Membaca kecepatan angular 1 omni wheel  ==> fi dot (rad/s)
+    float getVel(int delta_t);
+};
+
+#endif
\ No newline at end of file