KRAI 2017
/
Riset-Odometry
hadah
Fork of Riset-Odometry by
main.cpp@0:b455cd43929c, 2016-11-07 (annotated)
- Committer:
- gustavaditya
- Date:
- Mon Nov 07 12:24:01 2016 +0000
- Revision:
- 0:b455cd43929c
Coba-riset-PID
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:b455cd43929c | 39 | omniPos omni1(Dkiri); |
gustavaditya | 0:b455cd43929c | 40 | omniPos omni2(Dkanan); |
gustavaditya | 0:b455cd43929c | 41 | omniPos omni3(Bkiri); |
gustavaditya | 0:b455cd43929c | 42 | omniPos omni4(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 | 0:b455cd43929c | 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 | } |