![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
BacaMotor
Dependencies: mbed Motornew millis
Revision 0:b6f3a4e90aff, committed 2019-03-18
- Comitter:
- tamamfirdaus
- Date:
- Mon Mar 18 15:15:43 2019 +0000
- Commit message:
- BacaMotor;
Changed in this revision
--- /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
--- /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_; + +}
--- /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 */
--- /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
--- /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
--- /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