TaPeng
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