KRAI 2017 / Mbed 2 deprecated Riset-PID-Encoder

On Progress. Di baca pelan-pelan :'

Dependencies:   Motor PID QEI mbed

Fork of Riset-Odometry by Gustav Aditya Permana

main.cpp@2:2a1e06b525dc, 2016-11-07 (annotated)

Committer:
gustavaditya
Date:
Mon Nov 07 12:42:58 2016 +0000
Revision:
2:2a1e06b525dc
Parent:
0:b455cd43929c 
On Progress

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gustavaditya 0:b455cd43929c 1 /****************************************************************************/
gustavaditya 0:b455cd43929c 2 /* PROGRAM UNTUK PID CLOSED LOOP */
gustavaditya 0:b455cd43929c 3 /* */
gustavaditya 0:b455cd43929c 4 /* - Digunakan encoder autonics */
gustavaditya 0:b455cd43929c 5 /* - Konfigurasi Motor dan Encoder sbb : */
gustavaditya 0:b455cd43929c 6 /* _________________ */
gustavaditya 0:b455cd43929c 7 /* | DEPAN | */
gustavaditya 0:b455cd43929c 8 /* | 1. e .2 | Angka ==> Motor */
gustavaditya 0:b455cd43929c 9 /* | ` ` | e ==> Encoder */
gustavaditya 0:b455cd43929c 10 /* | e e | */
gustavaditya 0:b455cd43929c 11 /* | . . | */
gustavaditya 0:b455cd43929c 12 /* | 4` e `3 | */
gustavaditya 0:b455cd43929c 13 /* |________________| */
gustavaditya 0:b455cd43929c 14 /* */
gustavaditya 0:b455cd43929c 15 /* SETTINGS (WAJIB!) : */
gustavaditya 0:b455cd43929c 16 /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
gustavaditya 0:b455cd43929c 17 /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
gustavaditya 0:b455cd43929c 18 /* */
gustavaditya 0:b455cd43929c 19 /****************************************************************************/
gustavaditya 0:b455cd43929c 20
gustavaditya 0:b455cd43929c 21 #include "mbed.h"
gustavaditya 0:b455cd43929c 22 #include "Motor.h"
gustavaditya 0:b455cd43929c 23 #include "omniPos.h"
gustavaditya 0:b455cd43929c 24
gustavaditya 0:b455cd43929c 25 #define diameterRoda 10
gustavaditya 0:b455cd43929c 26 #define delta_t 0.001
gustavaditya 0:b455cd43929c 27
gustavaditya 0:b455cd43929c 28 // Deklarasi variabel motor
gustavaditya 0:b455cd43929c 29 Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev
gustavaditya 0:b455cd43929c 30 Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev
gustavaditya 0:b455cd43929c 31 Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev
gustavaditya 0:b455cd43929c 32 Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev
gustavaditya 0:b455cd43929c 33
gustavaditya 0:b455cd43929c 34 // Deklarasi variabel encoder
gustavaditya 0:b455cd43929c 35 //~ Dkiri untuk Motor 1
gustavaditya 0:b455cd43929c 36 //~ Dkanan untuk Motor 2
gustavaditya 0:b455cd43929c 37 //~ Bkanan untuk Motor 3
gustavaditya 0:b455cd43929c 38 //~ Bkiri untuk Motor 4
gustavaditya 2:2a1e06b525dc 39 omniPos omni1(omniPos::Dkiri);
gustavaditya 2:2a1e06b525dc 40 omniPos omni2(omniPos::Dkanan);
gustavaditya 2:2a1e06b525dc 41 omniPos omni3(omniPos::Bkiri);
gustavaditya 2:2a1e06b525dc 42 omniPos omni4(omniPos::Bkiri);
gustavaditya 0:b455cd43929c 43
gustavaditya 0:b455cd43929c 44 // Inisialisasi Pin TX-RX Joystik dan PC
gustavaditya 0:b455cd43929c 45 Serial pc(USBTX,USBRX);
gustavaditya 0:b455cd43929c 46
gustavaditya 0:b455cd43929c 47 // Deklarasi Variabel Global
gustavaditya 0:b455cd43929c 48 /*
gustavaditya 0:b455cd43929c 49 * posX dan posY berdasarkan arah robot
gustavaditya 0:b455cd43929c 50 * encoder Depan & Belakang sejajar sumbu Y
gustavaditya 0:b455cd43929c 51 * encoder Kanan & Kiri sejajar sumbu X
gustavaditya 0:b455cd43929c 52 */
gustavaditya 0:b455cd43929c 53 float jarak, posX, posY;
gustavaditya 2:2a1e06b525dc 54 //float keliling = pi*diameterRoda;
gustavaditya 0:b455cd43929c 55
gustavaditya 0:b455cd43929c 56 void detect_encoder()
gustavaditya 0:b455cd43929c 57 {
gustavaditya 0:b455cd43929c 58 int pv;
gustavaditya 0:b455cd43929c 59 // Motor1
gustavaditya 0:b455cd43929c 60
gustavaditya 0:b455cd43929c 61 PID.setProcessValue(omni1.getVel(delta_t));
gustavaditya 0:b455cd43929c 62 PID.setSetPoint();
gustavaditya 0:b455cd43929c 63 }
gustavaditya 0:b455cd43929c 64
gustavaditya 0:b455cd43929c 65 int main()
gustavaditya 0:b455cd43929c 66 {
gustavaditya 0:b455cd43929c 67 pc.baud(115200);
gustavaditya 0:b455cd43929c 68
gustavaditya 0:b455cd43929c 69 }
gustavaditya 0:b455cd43929c 70
gustavaditya 0:b455cd43929c 71 void gerakKanan()
gustavaditya 0:b455cd43929c 72 {
gustavaditya 0:b455cd43929c 73 if(vcurr<0.1) {
gustavaditya 0:b455cd43929c 74 vcurr=0.1;
gustavaditya 0:b455cd43929c 75 } else {
gustavaditya 0:b455cd43929c 76 vcurr+=ax;
gustavaditya 0:b455cd43929c 77 }
gustavaditya 0:b455cd43929c 78 //perlambatan=0;
gustavaditya 0:b455cd43929c 79 } else {
gustavaditya 0:b455cd43929c 80 //perlambatan=1;
gustavaditya 0:b455cd43929c 81 }
gustavaditya 0:b455cd43929c 82
gustavaditya 0:b455cd43929c 83 if (vcurr>=vmax) {
gustavaditya 0:b455cd43929c 84 vcurr=vmax;
gustavaditya 0:b455cd43929c 85 }
gustavaditya 0:b455cd43929c 86
gustavaditya 0:b455cd43929c 87 if(joystick.R2==255 && joystick.L2==0) {
gustavaditya 0:b455cd43929c 88 koef=2;
gustavaditya 0:b455cd43929c 89 } else if (joystick.L2==255 && joystick.R2==0) {
gustavaditya 0:b455cd43929c 90 koef=0.5;
gustavaditya 0:b455cd43929c 91 } else {
gustavaditya 0:b455cd43929c 92 koef=1;
gustavaditya 0:b455cd43929c 93 }
gustavaditya 0:b455cd43929c 94
gustavaditya 0:b455cd43929c 95 s1 =(float)(-1*koef*vcurr);
gustavaditya 0:b455cd43929c 96 s2 =(float)(-1.0*koef*vcurr);
gustavaditya 0:b455cd43929c 97 s3 =(float)(1*koef*vcurr);
gustavaditya 0:b455cd43929c 98 s4 =(float)(1.0*koef*vcurr);
gustavaditya 0:b455cd43929c 99
gustavaditya 0:b455cd43929c 100 kanan=true;
gustavaditya 0:b455cd43929c 101 maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
gustavaditya 0:b455cd43929c 102
gustavaditya 0:b455cd43929c 103 pc.printf("Kanan\n");
gustavaditya 0:b455cd43929c 104
gustavaditya 0:b455cd43929c 105 motor1.speed(s1);
gustavaditya 0:b455cd43929c 106 motor2.speed(s2);
gustavaditya 0:b455cd43929c 107 motor3.speed(s3);
gustavaditya 0:b455cd43929c 108 motor4.speed(s4);
gustavaditya 0:b455cd43929c 109 break;
gustavaditya 0:b455cd43929c 110 }
gustavaditya 0:b455cd43929c 111 }
gustavaditya 0:b455cd43929c 112
gustavaditya 0:b455cd43929c 113 void gerakKiri()
gustavaditya 0:b455cd43929c 114 {
gustavaditya 0:b455cd43929c 115 float revDepan, revBelakang, revKanan, revKiri;
gustavaditya 0:b455cd43929c 116 float tempX, tempY;
gustavaditya 0:b455cd43929c 117
gustavaditya 0:b455cd43929c 118 revDepan = encoderDepan.getRevolutions();
gustavaditya 0:b455cd43929c 119 revBelakang = encoderBelakang.getRevolutions();
gustavaditya 0:b455cd43929c 120 revKanan = encoderKanan.getRevolutions();
gustavaditya 0:b455cd43929c 121 revKiri = encoderKiri.getRevolutions();
gustavaditya 0:b455cd43929c 122 }