BacaMotor

Dependencies:   mbed Motornew millis

Files at this revision

API Documentation at this revision

Comitter:
tamamfirdaus
Date:
Mon Mar 18 15:15:43 2019 +0000
Commit message:
BacaMotor;

Changed in this revision

Motor.lib Show annotated file Show diff for this revision Revisions of this file
encoderKRAI.cpp Show annotated file Show diff for this revision Revisions of this file
encoderKRAI.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
millis.lib Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r b6f3a4e90aff Motor.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Mon Mar 18 15:15:43 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/KRAI-2018/code/Motornew/#1d0887244f8b
diff -r 000000000000 -r b6f3a4e90aff encoderKRAI.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.cpp	Mon Mar 18 15:15:43 2019 +0000
@@ -0,0 +1,121 @@
+/********************************************************/
+/*          Library untuk pembacaan Encoder             */
+/*                  Adopsi dari QEI                     */
+/*  Bismillahirahmanirrahim                             */
+/*  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::getPulses(void) {
+
+    return pulses_;
+
+}
+
+int encoderKRAI::getRevolutions(void) {
+
+    revolutions_ = pulses_ / pulsesPerRev_;
+    return revolutions_;
+
+}
+
+/***************************************
+ *  Perhitungan Pulse Encoder
+ ***************************************/
+
+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_;
+
+}
diff -r 000000000000 -r b6f3a4e90aff encoderKRAI.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.h	Mon Mar 18 15:15:43 2019 +0000
@@ -0,0 +1,87 @@
+/**
+ *  Header Encoder KRAI
+ *  untuk membaca nilai rotary encoder
+ **/
+#ifndef ENCODERKRAI_H
+#define ENCODERKRAI_H
+
+//Bismillahirahamnirahim
+
+//LIBRARY
+#include "mbed.h"
+
+/**************************
+ * Konstanta dan Variabel *
+ **************************/
+ 
+//KONSTANTA
+#define PREV_MASK 0x1 //Konstanta untuk mengetahui previous direction
+#define CURR_MASK 0x2 //Konstanta untuk mengetahui current direction
+#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;
+    
+    encoderKRAI(PinName channelA, PinName channelB, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+    /******************************************* 
+     * Membuat interface dari encoder    
+     * @param inA DigitalIn, out A dari encoder
+     * @param inB DigitalIn, out B dari encoder
+     *******************************************/
+     
+    void reset(void);
+    /*******************************************
+     * Reset encoder.
+     * Reset pembacaaan menjadi 0
+     *******************************************/
+     
+    int getPulses(void);
+    /*******************************************
+     * Membaca pulse yang didapat oleh encoder
+     * @return Nilai pulse yang telah dilalui.
+     *******************************************/
+    
+    int getRevolutions(void);
+    /*******************************************
+     * Membaca putaran yang didapat oleh encoder
+     * @return Nilai revolusi/putaran yang telah dilalui.
+     *******************************************/
+
+private:
+
+    void encode(void);
+    /*******************************************
+     * Menghitung pulse
+     * Digunakan setiap rising/falling edge baik channel A atau B
+     * Membaca putaran CW atau CCW => mengakibatkan pertambahan/pengurangan pulse
+     *******************************************/
+    
+//VARIABEL UNTUK PERHITUNGAN PULSE
+    Encoding encoding_;
+
+    InterruptIn channelA_;
+    InterruptIn channelB_;
+
+    int          pulsesPerRev_;
+    int          prevState_;
+    int          currState_;
+
+    volatile int pulses_;
+    volatile int revolutions_;
+
+
+};
+
+#endif /* ENCODERKRAI_H */
diff -r 000000000000 -r b6f3a4e90aff main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 18 15:15:43 2019 +0000
@@ -0,0 +1,71 @@
+//Library
+#include "mbed.h"
+#include "encoderKRAI.h"
+#include "millis.h"
+#include "Motor.h"
+
+//Konstanta
+#define PULSE_TO_JARAK 0.61313625
+#define Pressed 0
+#define NotPressed 1
+
+encoderKRAI encoderMotorA (PC_11, PC_12, 538, encoderKRAI::X4_ENCODING);
+encoderKRAI encoderMotorB (PC_2, PC_3, 538, encoderKRAI::X4_ENCODING);
+encoderKRAI encoderMotorC (PC_14, PC_15, 538, encoderKRAI::X4_ENCODING);
+encoderKRAI encoderMotorD (PC_10,PC_13, 538, encoderKRAI::X4_ENCODING);
+
+encoderKRAI encoder1 (PB_7,PB_6, 538, encoderKRAI::X4_ENCODING);
+encoderKRAI encoder2 (PB_4,PB_5, 538, encoderKRAI::X4_ENCODING);
+encoderKRAI encoder3 (PB_8,PB_9, 538, encoderKRAI::X4_ENCODING);
+
+/*Motor motorA(PA_7,  PA_5, PA_6);
+Motor motorB(PB_0, PC_1, PC_0);
+Motor motorC(PB_1, PB_15, PB_14);
+Motor motorD(PA_11, PA_12, PB_12);*/
+DigitalIn BlueButton(USER_BUTTON);
+
+
+Serial pc(USBTX, USBRX);
+
+//Variable Global
+float timeSampling;
+double pulseA;
+double pulseB;
+double pulseC;
+double pulseD;
+double pulse1;
+double pulse2;
+double pulse3;
+long lastMillis;
+
+//Baca Encoder
+int main(){
+   /* motorA.speed(0.4);
+    motorB.speed(0.4);
+    motorC.speed(0.4);
+    motorD.speed(0.4);
+    wait(5);
+    motorA.speed(0.0);
+    motorB.speed(0.0);
+    motorC.speed(0.0);
+    motorD.speed(0.0);*/
+    pc.baud(115200);
+    while(1){
+        /*if(BlueButton==Pressed){
+            encoderMotorA.reset();
+            }*/
+                 
+        pulseA = (double)encoderMotorA.getPulses()*PULSE_TO_JARAK;
+        pulseB = (double)encoderMotorB.getPulses()*PULSE_TO_JARAK;
+        pulseC = (double)encoderMotorC.getPulses()*PULSE_TO_JARAK;
+        pulseD = (double)encoderMotorD.getPulses()*PULSE_TO_JARAK;
+        
+        //pulse1 = (double)encoder1.getPulses()*PULSE_TO_JARAK;
+        //pulse2 = (double)encoder2.getPulses()*PULSE_TO_JARAK;
+        pulse3 = (double)encoder3.getPulses()*PULSE_TO_JARAK;
+        //pc.printf("A: %lf\t B: %lf\t C: %lf\t D: %lf\t \n", pulseA, pulseB, pulseC, pulseD);
+        pc.printf("Enc_1: %lf\n", pulse3);
+        
+        wait(0.2);    
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r b6f3a4e90aff mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Mar 18 15:15:43 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e95d10626187
\ No newline at end of file
diff -r 000000000000 -r b6f3a4e90aff millis.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/millis.lib	Mon Mar 18 15:15:43 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/DFRobot/code/millis/#736e6cc31bcd