![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
main.cpp@6:5632ff2142c0, 2017-10-21 (annotated)
- Committer:
- calmantara186
- Date:
- Sat Oct 21 06:34:05 2017 +0000
- Revision:
- 6:5632ff2142c0
- Parent:
- 3:4801d83f397c
- Child:
- 7:cf822f5e7b12
thrower robot : to do list ada di program
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
calmantara186 | 0:a6918b57d3fa | 1 | /** |
calmantara186 | 0:a6918b57d3fa | 2 | * THROWER ROBOT |
calmantara186 | 0:a6918b57d3fa | 3 | * Bismillahirahmanirrahim |
calmantara186 | 0:a6918b57d3fa | 4 | * |
calmantara186 | 0:a6918b57d3fa | 5 | * To do List : |
calmantara186 | 6:5632ff2142c0 | 6 | * - Cari Fungsi Output Open Loop |
calmantara186 | 6:5632ff2142c0 | 7 | * - Buat library odometry |
calmantara186 | 0:a6918b57d3fa | 8 | * |
calmantara186 | 2:dea57b3f84cd | 9 | * UPDATED : 18/10 |
calmantara186 | 2:dea57b3f84cd | 10 | * - Dapet nilai mundur Kp = 0.00083, pwm kanan kiri = 0.4 |
calmantara186 | 6:5632ff2142c0 | 11 | * - Dapet nilai maju Kp = 0.00035, pwm kanan kiri = 0.4 |
calmantara186 | 6:5632ff2142c0 | 12 | * |
calmantara186 | 6:5632ff2142c0 | 13 | * Update : 19/10 |
calmantara186 | 6:5632ff2142c0 | 14 | * - Inlcude library PID |
calmantara186 | 6:5632ff2142c0 | 15 | * - Lebih rapi |
calmantara186 | 6:5632ff2142c0 | 16 | * |
calmantara186 | 6:5632ff2142c0 | 17 | * Last Edited by : Thrower KRAI |
calmantara186 | 0:a6918b57d3fa | 18 | **/ |
calmantara186 | 2:dea57b3f84cd | 19 | |
calmantara186 | 0:a6918b57d3fa | 20 | |
calmantara186 | 0:a6918b57d3fa | 21 | //LIBRARY |
calmantara186 | 0:a6918b57d3fa | 22 | #include "mbed.h" |
calmantara186 | 0:a6918b57d3fa | 23 | #include "Motor.h" |
calmantara186 | 0:a6918b57d3fa | 24 | #include "encoderKRAI.h" |
calmantara186 | 0:a6918b57d3fa | 25 | #include "millis.h" |
calmantara186 | 6:5632ff2142c0 | 26 | #include "pidKRAI.h" |
calmantara186 | 0:a6918b57d3fa | 27 | |
calmantara186 | 0:a6918b57d3fa | 28 | /*************************** |
calmantara186 | 0:a6918b57d3fa | 29 | * Konstanta dan Variabel * |
calmantara186 | 0:a6918b57d3fa | 30 | ***************************/ |
calmantara186 | 2:dea57b3f84cd | 31 | #define PI 3.141592 |
calmantara186 | 2:dea57b3f84cd | 32 | #define DRODA 100 |
calmantara186 | 2:dea57b3f84cd | 33 | |
calmantara186 | 2:dea57b3f84cd | 34 | //Variabel Robot |
calmantara186 | 2:dea57b3f84cd | 35 | float kll_roda = (float) PI*DRODA; //variabel keliling robot |
calmantara186 | 2:dea57b3f84cd | 36 | |
calmantara186 | 6:5632ff2142c0 | 37 | //Variabel Program dan Planning |
calmantara186 | 6:5632ff2142c0 | 38 | unsigned short in_program = 1; //variabel untuk masuk ke looping program |
calmantara186 | 6:5632ff2142c0 | 39 | |
calmantara186 | 2:dea57b3f84cd | 40 | //Primitive Function |
calmantara186 | 6:5632ff2142c0 | 41 | void limitMotor(); //procedure untuk mengecek limit pwm motor |
calmantara186 | 6:5632ff2142c0 | 42 | void pidMotor(int x); //procedure untuk arah gerak motor |
calmantara186 | 6:5632ff2142c0 | 43 | float getY(float y1, float y2); //fungsi untuk menghitung jarak Y |
calmantara186 | 2:dea57b3f84cd | 44 | |
calmantara186 | 0:a6918b57d3fa | 45 | //Deklarasi Motor |
calmantara186 | 0:a6918b57d3fa | 46 | Motor motor_depan(PB_6, PB_13, PA_10); //pwm, fwd, rev |
calmantara186 | 0:a6918b57d3fa | 47 | Motor motor_kanan(PB_8, PA_9, PA_5); //pwm, fwd, rev |
calmantara186 | 0:a6918b57d3fa | 48 | Motor motor_kiri(PB_9, PA_12, PA_11); //pwm, fwd, rev |
calmantara186 | 0:a6918b57d3fa | 49 | |
calmantara186 | 2:dea57b3f84cd | 50 | //Variabel Kecepatan |
calmantara186 | 6:5632ff2142c0 | 51 | float pwm_kanan; //pwm motor kanan |
calmantara186 | 6:5632ff2142c0 | 52 | float pwm_kiri; //pwm motor kiri |
calmantara186 | 6:5632ff2142c0 | 53 | float pwm_depan; //pwm motor depan |
calmantara186 | 6:5632ff2142c0 | 54 | float limit_pwm = 0.9; //limit pwm semua motor |
calmantara186 | 0:a6918b57d3fa | 55 | |
calmantara186 | 0:a6918b57d3fa | 56 | //Variabel Serial |
calmantara186 | 0:a6918b57d3fa | 57 | Serial pc(USBTX, USBRX); //Deklarasi serial pc TX RX |
calmantara186 | 0:a6918b57d3fa | 58 | |
calmantara186 | 0:a6918b57d3fa | 59 | //Deklarasi Rotary Encoder |
calmantara186 | 6:5632ff2142c0 | 60 | encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType |
calmantara186 | 6:5632ff2142c0 | 61 | encoderKRAI encMotor_kanan(PC_10, PC_11, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType |
calmantara186 | 6:5632ff2142c0 | 62 | encoderKRAI encMotor_kiri(PC_2, PC_3, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType |
calmantara186 | 0:a6918b57d3fa | 63 | |
calmantara186 | 0:a6918b57d3fa | 64 | //Variabel Encoder |
calmantara186 | 0:a6918b57d3fa | 65 | float pulses_depan; //variabel untuk membaca nilai encoder motor depan |
calmantara186 | 0:a6918b57d3fa | 66 | float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan |
calmantara186 | 0:a6918b57d3fa | 67 | float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri |
calmantara186 | 6:5632ff2142c0 | 68 | |
calmantara186 | 2:dea57b3f84cd | 69 | float delta_kaki; //variabel untuk membaca nilai delta kanan kiri |
calmantara186 | 6:5632ff2142c0 | 70 | float delta_semua; //variabel untuk membaca nilai delta semua encoder |
calmantara186 | 6:5632ff2142c0 | 71 | |
calmantara186 | 2:dea57b3f84cd | 72 | float rev_depan; //variabel untuk membaca nilai putaran roda depan |
calmantara186 | 2:dea57b3f84cd | 73 | float rev_kanan; //variabel untuk membaca nilai putaran roda kanan |
calmantara186 | 6:5632ff2142c0 | 74 | float rev_kiri; //variabel untuk membaca nilai putaran roda kiri |
calmantara186 | 0:a6918b57d3fa | 75 | |
calmantara186 | 0:a6918b57d3fa | 76 | //Target nilai encoder |
calmantara186 | 6:5632ff2142c0 | 77 | float tn_y = 0; //target untuk jumlah rotary encoder maju atau mundur |
calmantara186 | 6:5632ff2142c0 | 78 | float tn_x = 0; //target untuk jumlah rotary encoder kanan atau kiri |
calmantara186 | 0:a6918b57d3fa | 79 | |
calmantara186 | 6:5632ff2142c0 | 80 | //Variabel PID |
calmantara186 | 6:5632ff2142c0 | 81 | unsigned short ts_base = 12; //variabel untuk time sampling |
calmantara186 | 6:5632ff2142c0 | 82 | float speed_roty; //variabel untuk penambahan pwm PID rotasi y |
calmantara186 | 6:5632ff2142c0 | 83 | float speed_disy; //variabel untuk penambahan pwm PID distance y |
calmantara186 | 6:5632ff2142c0 | 84 | float speed_rotx; //variabel untuk penambahan pwm PID rotasi x |
calmantara186 | 6:5632ff2142c0 | 85 | float speed_disx; //variabel untuk penambahan pwm PID distance x |
calmantara186 | 6:5632ff2142c0 | 86 | |
calmantara186 | 6:5632ff2142c0 | 87 | //Deklarasi PID |
calmantara186 | 6:5632ff2142c0 | 88 | pidKRAI pidRotY((float) 0.0, (float) 0.0, (float) 0.0, ts_base); //kp ki kd ts |
calmantara186 | 6:5632ff2142c0 | 89 | pidKRAI pidRotX(); //kp ki kd ts |
calmantara186 | 6:5632ff2142c0 | 90 | |
calmantara186 | 6:5632ff2142c0 | 91 | pidKRAI pidDisY((float)0.0,(float)0.0,(float)0.0,ts_base); //kp ki kd ts |
calmantara186 | 6:5632ff2142c0 | 92 | pidKRAI pidDisX(); //kp ki kd ts |
calmantara186 | 2:dea57b3f84cd | 93 | |
calmantara186 | 2:dea57b3f84cd | 94 | //Variabel cari jarak |
calmantara186 | 6:5632ff2142c0 | 95 | float target_y[100] = {1100.0, 0.0}; //variabel yang digunakan untuk target jarak y |
calmantara186 | 6:5632ff2142c0 | 96 | float jarak_y; //variabel untuk increment jarak y |
calmantara186 | 0:a6918b57d3fa | 97 | |
calmantara186 | 6:5632ff2142c0 | 98 | float target_x[100]; //variabel yang digunakan untuk target jarak x |
calmantara186 | 6:5632ff2142c0 | 99 | float jarak_x; //variabel untuk increment jarak x |
calmantara186 | 6:5632ff2142c0 | 100 | |
calmantara186 | 6:5632ff2142c0 | 101 | //Variabel Waktu |
calmantara186 | 6:5632ff2142c0 | 102 | unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan |
calmantara186 | 6:5632ff2142c0 | 103 | unsigned long int prev_millis1; //variabel untuk masuk ke perhitungan PID |
calmantara186 | 6:5632ff2142c0 | 104 | unsigned long int prev_millis2; //variabel untuk matikan motor |
calmantara186 | 0:a6918b57d3fa | 105 | |
calmantara186 | 0:a6918b57d3fa | 106 | /*************************** |
calmantara186 | 0:a6918b57d3fa | 107 | * Main Function * |
calmantara186 | 0:a6918b57d3fa | 108 | ***************************/ |
calmantara186 | 0:a6918b57d3fa | 109 | |
calmantara186 | 0:a6918b57d3fa | 110 | int main(){ |
calmantara186 | 0:a6918b57d3fa | 111 | |
calmantara186 | 0:a6918b57d3fa | 112 | //Inisiasi Serial |
calmantara186 | 0:a6918b57d3fa | 113 | pc.baud(115200); |
calmantara186 | 0:a6918b57d3fa | 114 | |
calmantara186 | 0:a6918b57d3fa | 115 | //Start Millis |
calmantara186 | 2:dea57b3f84cd | 116 | wait_ms(3000); |
calmantara186 | 0:a6918b57d3fa | 117 | startMillis(); |
calmantara186 | 6:5632ff2142c0 | 118 | //Looping Utama |
calmantara186 | 6:5632ff2142c0 | 119 | while(1){ |
calmantara186 | 6:5632ff2142c0 | 120 | //Looping Program Planning |
calmantara186 | 6:5632ff2142c0 | 121 | while(in_program == 0){ |
calmantara186 | 6:5632ff2142c0 | 122 | }/* end of planning program */ |
calmantara186 | 6:5632ff2142c0 | 123 | //Looping Program Utama |
calmantara186 | 6:5632ff2142c0 | 124 | while(in_program == 1){ |
calmantara186 | 6:5632ff2142c0 | 125 | current_millis = millis(); |
calmantara186 | 6:5632ff2142c0 | 126 | |
calmantara186 | 6:5632ff2142c0 | 127 | /* Speed Motor */ |
calmantara186 | 6:5632ff2142c0 | 128 | motor_kanan.speed(pwm_kanan); |
calmantara186 | 6:5632ff2142c0 | 129 | motor_kiri.speed(pwm_kiri); |
calmantara186 | 6:5632ff2142c0 | 130 | |
calmantara186 | 6:5632ff2142c0 | 131 | pc.printf("%.2f \n", delta_kaki); |
calmantara186 | 6:5632ff2142c0 | 132 | |
calmantara186 | 6:5632ff2142c0 | 133 | if(current_millis - prev_millis1 >= ts_base){ |
calmantara186 | 6:5632ff2142c0 | 134 | //pidMotor(1); |
calmantara186 | 6:5632ff2142c0 | 135 | pulses_kanan = (float) encMotor_kanan.getPulses(); |
calmantara186 | 6:5632ff2142c0 | 136 | pulses_kiri = (float) encMotor_kiri.getPulses(); |
calmantara186 | 6:5632ff2142c0 | 137 | rev_kanan = (float) encMotor_kanan.getRevolutions(); |
calmantara186 | 6:5632ff2142c0 | 138 | rev_kiri = (float) encMotor_kiri.getRevolutions(); |
calmantara186 | 6:5632ff2142c0 | 139 | |
calmantara186 | 6:5632ff2142c0 | 140 | /* masuk ke perhitungan parameter */ |
calmantara186 | 6:5632ff2142c0 | 141 | jarak_y += getY(rev_kanan, rev_kiri); |
calmantara186 | 6:5632ff2142c0 | 142 | delta_kaki = pulses_kanan + pulses_kiri; |
calmantara186 | 6:5632ff2142c0 | 143 | } |
calmantara186 | 6:5632ff2142c0 | 144 | |
calmantara186 | 6:5632ff2142c0 | 145 | if(jarak_y > target_y[0]){ |
calmantara186 | 6:5632ff2142c0 | 146 | pwm_kiri = 0; |
calmantara186 | 6:5632ff2142c0 | 147 | pwm_kanan = 0; |
calmantara186 | 6:5632ff2142c0 | 148 | } |
calmantara186 | 2:dea57b3f84cd | 149 | } |
calmantara186 | 6:5632ff2142c0 | 150 | } /* end of main program */ |
calmantara186 | 2:dea57b3f84cd | 151 | } |
calmantara186 | 2:dea57b3f84cd | 152 | |
calmantara186 | 2:dea57b3f84cd | 153 | /************************************ |
calmantara186 | 2:dea57b3f84cd | 154 | * Deklarasi Fungsi dan Prosedur * |
calmantara186 | 2:dea57b3f84cd | 155 | ************************************/ |
calmantara186 | 2:dea57b3f84cd | 156 | void limitMotor(){ |
calmantara186 | 2:dea57b3f84cd | 157 | /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */ |
calmantara186 | 6:5632ff2142c0 | 158 | if(pwm_kanan > limit_pwm){ |
calmantara186 | 1:5e0cb74f950e | 159 | pwm_kanan = limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 160 | } |
calmantara186 | 6:5632ff2142c0 | 161 | else if(pwm_kanan < (-limit_pwm)){ |
calmantara186 | 6:5632ff2142c0 | 162 | pwm_kanan = -limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 163 | } |
calmantara186 | 6:5632ff2142c0 | 164 | if(pwm_kiri > limit_pwm){ |
calmantara186 | 6:5632ff2142c0 | 165 | pwm_kiri = limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 166 | } |
calmantara186 | 6:5632ff2142c0 | 167 | else if(pwm_kiri < (-limit_pwm)){ |
calmantara186 | 6:5632ff2142c0 | 168 | pwm_kiri = -limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 169 | } |
calmantara186 | 6:5632ff2142c0 | 170 | if(pwm_depan > limit_pwm){ |
calmantara186 | 6:5632ff2142c0 | 171 | pwm_depan = limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 172 | } |
calmantara186 | 6:5632ff2142c0 | 173 | else if(pwm_depan < (-limit_pwm)){ |
calmantara186 | 6:5632ff2142c0 | 174 | pwm_depan = -limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 175 | } |
calmantara186 | 3:4801d83f397c | 176 | }/* end of limitMotor */ |
calmantara186 | 3:4801d83f397c | 177 | |
calmantara186 | 6:5632ff2142c0 | 178 | void pidMotor(int x){ |
calmantara186 | 6:5632ff2142c0 | 179 | /** |
calmantara186 | 6:5632ff2142c0 | 180 | * prosedur untuk gerak motor |
calmantara186 | 6:5632ff2142c0 | 181 | * 0 : maju dan mundur |
calmantara186 | 6:5632ff2142c0 | 182 | * 1 : kanan dan kiri |
calmantara186 | 6:5632ff2142c0 | 183 | **/ |
calmantara186 | 6:5632ff2142c0 | 184 | if(x==0){ |
calmantara186 | 6:5632ff2142c0 | 185 | /* masuk ke perhitungan ketika sudah pada time sampling */ |
calmantara186 | 6:5632ff2142c0 | 186 | pulses_kanan = (float) encMotor_kanan.getPulses(); |
calmantara186 | 6:5632ff2142c0 | 187 | pulses_kiri = (float) encMotor_kiri.getPulses(); |
calmantara186 | 6:5632ff2142c0 | 188 | rev_kanan = (float) pulses_kanan/540; |
calmantara186 | 6:5632ff2142c0 | 189 | rev_kiri = (float) pulses_kiri/540; |
calmantara186 | 6:5632ff2142c0 | 190 | |
calmantara186 | 6:5632ff2142c0 | 191 | /* masuk ke perhitungan parameter */ |
calmantara186 | 6:5632ff2142c0 | 192 | jarak_y += getY(rev_kanan, rev_kiri); |
calmantara186 | 6:5632ff2142c0 | 193 | delta_kaki = pulses_kanan + pulses_kiri; |
calmantara186 | 6:5632ff2142c0 | 194 | |
calmantara186 | 6:5632ff2142c0 | 195 | speed_roty = pidRotY.pidCount(tn_y, delta_kaki); |
calmantara186 | 6:5632ff2142c0 | 196 | speed_disy = pidDisY.pidCount(target_y[0], jarak_y); |
calmantara186 | 6:5632ff2142c0 | 197 | /* Kondisi untuk menambahkan speed */ |
calmantara186 | 6:5632ff2142c0 | 198 | pwm_kiri = speed_disy + speed_roty; |
calmantara186 | 6:5632ff2142c0 | 199 | pwm_kanan = speed_disy + speed_roty; |
calmantara186 | 6:5632ff2142c0 | 200 | |
calmantara186 | 6:5632ff2142c0 | 201 | prev_millis1 = current_millis; |
calmantara186 | 6:5632ff2142c0 | 202 | encMotor_kanan.reset(); |
calmantara186 | 6:5632ff2142c0 | 203 | encMotor_kiri.reset(); |
calmantara186 | 6:5632ff2142c0 | 204 | |
calmantara186 | 6:5632ff2142c0 | 205 | } |
calmantara186 | 6:5632ff2142c0 | 206 | else if(x==1){} |
calmantara186 | 6:5632ff2142c0 | 207 | }/* end of pidMotor */ |
calmantara186 | 6:5632ff2142c0 | 208 | |
calmantara186 | 3:4801d83f397c | 209 | float getY(float y1, float y2){ |
calmantara186 | 3:4801d83f397c | 210 | /* fungsi untuk menghitung nilai Y dari roda kanan kiri */ |
calmantara186 | 3:4801d83f397c | 211 | float jarak_y1 = (kll_roda*y1)/4; |
calmantara186 | 3:4801d83f397c | 212 | float jarak_y2 = (kll_roda*y2)/4; |
calmantara186 | 3:4801d83f397c | 213 | |
calmantara186 | 3:4801d83f397c | 214 | return jarak_y1 - jarak_y2; |
calmantara186 | 6:5632ff2142c0 | 215 | }/* end of getY */ |