Revision 4:cd5de3b14797, committed 2017-12-12

Comitter:

be_bryan

Date:

Tue Dec 12 07:48:20 2017 +0000

Parent:

3:a03ce2084ceb

Commit message:

llala

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Tue Dec 12 07:48:20 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/Motor/#c75b234558af

--- a/Motordriver.lib	Sat Dec 09 10:20:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/osmeest/code/Motordriver/#83245b45ea70

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.cpp	Tue Dec 12 07:48:20 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	Tue Dec 12 07:48:20 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	Sat Dec 09 10:20:20 2017 +0000
+++ b/main.cpp	Tue Dec 12 07:48:20 2017 +0000
@@ -2,23 +2,31 @@
 #include "mbed.h"
 #include "TextLCD.h"
 #include "PS_PAD.h"
-#include "motordriver.h"
+#include "Motor.h"
+#include "encoderKRAI.h"
 
 #include <string>
 using namespace std;
 
+Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL);
+Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL);
+Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL);
 
-TextLCD lcd(PB_7, PA_15, PA_14, PB_8, PB_1, PA_13, TextLCD::LCD20x4); //rs,e,d4-d7
-
+TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7
+encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING);
+encoderKRAI enc2 (PC_0, PC_1, 11, encoderKRAI::X4_ENCODING);
+encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING);
 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)
 
-/* motor
-    Motor motor3(PA_8, PB_0, PC_15, 1); //motor1
-    Motor motor1(PA_11, PA_6, PA_5, 1); //motor2
-    Motor motor4(PA_9, PC_2, PC_3, 1);   //motor_slider
-    Motor motor2(PA_10, PB_5, PB_4, 1);  //motor_griper
-*/
-
+    Motor motor3(PB_7, PA_14, PA_15); //motor4
+    Motor motor2(PA_11, PA_6, PA_5); //motor2
+    //Motor motor1(PA_10, PB_5, PB_4);  //motor_griper
+    Motor motor1(PB_6, PA_7, PB_12); //motor 3
+string a;
+    
+void dataJoystick();
+void lcdPrint();
+void motorP();
 
 Serial pc(USBTX,USBRX);
 
@@ -26,20 +34,21 @@
 {    
     pc.baud(115200);
     ps2.init();
-    
-    string a;
+    thread1.start(dataJoystick);
+    thread2.start(lcdPrint);
+    thread3.start(motorP);
     while (1)
     {      
         // baca input
+/*      
         ps2.poll();
-        
         if(ps2.read(PS_PAD::PAD_X)==1)                   a = "silang";
         else if(ps2.read(PS_PAD::PAD_CIRCLE)==1)         a = "lingkaran";
         else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1)       a = "segitiga";
         else if(ps2.read(PS_PAD::PAD_SQUARE)==1)         a = "kotak";
         else                                             a = "NULL";
-    
-        
+ */   
+/*        
         //tampilkan LCD
         lcd.locate(0,0);
         lcd.printf("input joystik :");
@@ -48,6 +57,48 @@
     
         wait_ms(10);
         lcd.cls();
+*/
+    }
+}
+
+void dataJoystick(){
+    while(true){
+    ps2.poll();
+    if(ps2.read(PS_PAD::PAD_X)==1)                   a =  "silang";
+    else if(ps2.read(PS_PAD::PAD_CIRCLE)==1)         a = "lingkaran";
+    else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1)       a = "segitiga";
+    else if(ps2.read(PS_PAD::PAD_SQUARE)==1)         a = "kotak";
+    else                                             a = "NULL";    
     }
 }
-        
\ No newline at end of file
+
+void lcdPrint(){
+    while (true){
+    lcd.cls(); 
+    int pulse1 = enc1.getPulses();
+    int pulse2 = enc2.getPulses();
+    int pulse3 = enc3.getPulses();
+    lcd.locate(0,0);
+    lcd.printf("input : %s", a);
+    lcd.locate(0,1);
+    lcd.printf("enc1 = %d", pulse1);
+    lcd.locate(0,2);
+    lcd.printf("enc2 = %d", pulse2);
+    lcd.locate(0,3);
+    lcd.printf("enc3 = %d", pulse3);
+    Thread::wait(20);
+    }
+}
+
+void motorP() {
+        while(true){
+            motor1.speed(0.5);
+            motor2.speed(0.5);
+            motor3.speed(0.5);
+            Thread::wait(2000);
+            motor1.speed(-0.5);
+            motor2.speed(-0.5);
+            motor3.speed(-0.5);
+            Thread::wait(2000);
+        }  
+}      
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Tue Dec 12 07:48:20 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/EL4121-Embedded-System/code/mbed-os/#b74591d5ab33

--- a/mbed.bld	Sat Dec 09 10:20:20 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/9296ab0bfc11
\ No newline at end of file