![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
utk magang
Dependencies: Motor Ultrasonic encoderKRAI mbed millis odometryKRAI pidKRAI
main.cpp@11:d4cf81f59601, 2017-10-25 (annotated)
- Committer:
- calmantara186
- Date:
- Wed Oct 25 15:16:27 2017 +0000
- Revision:
- 11:d4cf81f59601
- Parent:
- 10:e550e5daed74
- Child:
- 12:84cb23216f78
FAK. SAYA PERLU TAMASYA !!!!
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 | 7:cf822f5e7b12 | 6 | * - Cari konstanta Kanan Kiri |
calmantara186 | 10:e550e5daed74 | 7 | * - Cari Konstanta lagi :( |
calmantara186 | 9:e2c3175936fb | 8 | * |
calmantara186 | 7:cf822f5e7b12 | 9 | * - JAGA SEMANGAT DAN KESEHATAN KITA !!! :) |
calmantara186 | 0:a6918b57d3fa | 10 | * |
calmantara186 | 7:cf822f5e7b12 | 11 | * UPDATED : 22/10 |
calmantara186 | 7:cf822f5e7b12 | 12 | * - PID maju/mundur enak enak jos |
calmantara186 | 6:5632ff2142c0 | 13 | * |
calmantara186 | 6:5632ff2142c0 | 14 | * Last Edited by : Thrower KRAI |
calmantara186 | 0:a6918b57d3fa | 15 | **/ |
calmantara186 | 2:dea57b3f84cd | 16 | |
calmantara186 | 0:a6918b57d3fa | 17 | |
calmantara186 | 0:a6918b57d3fa | 18 | //LIBRARY |
calmantara186 | 0:a6918b57d3fa | 19 | #include "mbed.h" |
calmantara186 | 0:a6918b57d3fa | 20 | #include "Motor.h" |
calmantara186 | 0:a6918b57d3fa | 21 | #include "encoderKRAI.h" |
calmantara186 | 0:a6918b57d3fa | 22 | #include "millis.h" |
calmantara186 | 6:5632ff2142c0 | 23 | #include "pidKRAI.h" |
calmantara186 | 9:e2c3175936fb | 24 | #include "odometryKRAI.h" |
calmantara186 | 0:a6918b57d3fa | 25 | |
calmantara186 | 0:a6918b57d3fa | 26 | /*************************** |
calmantara186 | 0:a6918b57d3fa | 27 | * Konstanta dan Variabel * |
calmantara186 | 0:a6918b57d3fa | 28 | ***************************/ |
calmantara186 | 9:e2c3175936fb | 29 | #define DRODA 100.0 |
calmantara186 | 2:dea57b3f84cd | 30 | |
calmantara186 | 2:dea57b3f84cd | 31 | //Variabel Robot |
calmantara186 | 9:e2c3175936fb | 32 | double kll_roda = PI*DRODA; //variabel keliling robot |
calmantara186 | 2:dea57b3f84cd | 33 | |
calmantara186 | 6:5632ff2142c0 | 34 | //Variabel Program dan Planning |
calmantara186 | 11:d4cf81f59601 | 35 | unsigned short in_program = 0; //variabel untuk masuk ke looping program |
calmantara186 | 11:d4cf81f59601 | 36 | short state =-1; |
calmantara186 | 11:d4cf81f59601 | 37 | |
calmantara186 | 11:d4cf81f59601 | 38 | DigitalIn mybutton(USER_BUTTON); |
calmantara186 | 6:5632ff2142c0 | 39 | |
calmantara186 | 2:dea57b3f84cd | 40 | //Primitive Function |
calmantara186 | 9:e2c3175936fb | 41 | void limitMotor(); //procedure untuk mengecek limit pwm motor |
calmantara186 | 9:e2c3175936fb | 42 | void pidMotor(int x); //procedure untuk arah gerak motor |
calmantara186 | 9:e2c3175936fb | 43 | void getValue(); //procedure untuk menghitung pulses dan rev |
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 | 10:e550e5daed74 | 64 | //Deklarasi Odometry |
calmantara186 | 10:e550e5daed74 | 65 | odometryKRAI odometry(DRODA); //deklarasi odometry dengan input diameter roda |
calmantara186 | 10:e550e5daed74 | 66 | |
calmantara186 | 0:a6918b57d3fa | 67 | //Variabel Encoder |
calmantara186 | 0:a6918b57d3fa | 68 | float pulses_depan; //variabel untuk membaca nilai encoder motor depan |
calmantara186 | 0:a6918b57d3fa | 69 | float pulses_kanan; //variabel untuk membaca nilai encoder motor kanan |
calmantara186 | 0:a6918b57d3fa | 70 | float pulses_kiri; //variabel untuk membaca nilai encoder motor kiri |
calmantara186 | 6:5632ff2142c0 | 71 | |
calmantara186 | 10:e550e5daed74 | 72 | float d_tetha; //variabel untuk membaca nilai delta kanan kiri depan |
calmantara186 | 11:d4cf81f59601 | 73 | //float d_tetha2; //variabel untuk membaca nilai delta kanan kiri |
calmantara186 | 6:5632ff2142c0 | 74 | |
calmantara186 | 2:dea57b3f84cd | 75 | float rev_depan; //variabel untuk membaca nilai putaran roda depan |
calmantara186 | 2:dea57b3f84cd | 76 | float rev_kanan; //variabel untuk membaca nilai putaran roda kanan |
calmantara186 | 6:5632ff2142c0 | 77 | float rev_kiri; //variabel untuk membaca nilai putaran roda kiri |
calmantara186 | 0:a6918b57d3fa | 78 | |
calmantara186 | 0:a6918b57d3fa | 79 | //Target nilai encoder |
calmantara186 | 9:e2c3175936fb | 80 | float tn_tetha = 0; //target untuk nilai tetha robot |
calmantara186 | 0:a6918b57d3fa | 81 | |
calmantara186 | 6:5632ff2142c0 | 82 | //Variabel PID |
calmantara186 | 6:5632ff2142c0 | 83 | unsigned short ts_base = 12; //variabel untuk time sampling |
calmantara186 | 6:5632ff2142c0 | 84 | float speed_roty; //variabel untuk penambahan pwm PID rotasi y |
calmantara186 | 6:5632ff2142c0 | 85 | float speed_disy; //variabel untuk penambahan pwm PID distance y |
calmantara186 | 11:d4cf81f59601 | 86 | float limit_dis = 0.4; |
calmantara186 | 11:d4cf81f59601 | 87 | float limit_rot = 0.2; |
calmantara186 | 9:e2c3175936fb | 88 | |
calmantara186 | 6:5632ff2142c0 | 89 | float speed_rotx; //variabel untuk penambahan pwm PID rotasi x |
calmantara186 | 6:5632ff2142c0 | 90 | float speed_disx; //variabel untuk penambahan pwm PID distance x |
calmantara186 | 6:5632ff2142c0 | 91 | |
calmantara186 | 6:5632ff2142c0 | 92 | //Deklarasi PID |
calmantara186 | 11:d4cf81f59601 | 93 | pidKRAI pidRotY((float) 0.234, (float) 0.0098, (float) 7.9123, ts_base); //kp ki kd ts |
calmantara186 | 11:d4cf81f59601 | 94 | pidKRAI pidRotX((float) 0.234, (float) 0.0098, (float) 7.9123, ts_base); //kp ki kd ts |
calmantara186 | 6:5632ff2142c0 | 95 | |
calmantara186 | 11:d4cf81f59601 | 96 | pidKRAI pidDisY((float)0.2208,(float)0.0105,(float)4.26231,ts_base); //kp ki kd ts |
calmantara186 | 11:d4cf81f59601 | 97 | pidKRAI pidDisX((float)0.228,(float)0.0234,(float)9.6231,ts_base); //kp ki kd ts |
calmantara186 | 2:dea57b3f84cd | 98 | |
calmantara186 | 2:dea57b3f84cd | 99 | //Variabel cari jarak |
calmantara186 | 11:d4cf81f59601 | 100 | float target_y[50] = {2000.0, -2000.0}; //variabel yang digunakan untuk target jarak y |
calmantara186 | 9:e2c3175936fb | 101 | double jarak_y; //variabel untuk increment jarak y |
calmantara186 | 0:a6918b57d3fa | 102 | |
calmantara186 | 11:d4cf81f59601 | 103 | float target_x[50] = {2000.0, -2000.0}; //variabel yang digunakan untuk target jarak x |
calmantara186 | 9:e2c3175936fb | 104 | double jarak_x; //variabel untuk increment jarak x |
calmantara186 | 6:5632ff2142c0 | 105 | |
calmantara186 | 6:5632ff2142c0 | 106 | //Variabel Waktu |
calmantara186 | 6:5632ff2142c0 | 107 | unsigned long int current_millis; //variabel untuk mendapatkan millis yang berjalan |
calmantara186 | 6:5632ff2142c0 | 108 | unsigned long int prev_millis1; //variabel untuk masuk ke perhitungan PID |
calmantara186 | 10:e550e5daed74 | 109 | unsigned long int prev_millis2; |
calmantara186 | 0:a6918b57d3fa | 110 | |
calmantara186 | 0:a6918b57d3fa | 111 | /*************************** |
calmantara186 | 0:a6918b57d3fa | 112 | * Main Function * |
calmantara186 | 0:a6918b57d3fa | 113 | ***************************/ |
calmantara186 | 0:a6918b57d3fa | 114 | |
calmantara186 | 0:a6918b57d3fa | 115 | int main(){ |
calmantara186 | 0:a6918b57d3fa | 116 | |
calmantara186 | 0:a6918b57d3fa | 117 | //Inisiasi Serial |
calmantara186 | 0:a6918b57d3fa | 118 | pc.baud(115200); |
calmantara186 | 0:a6918b57d3fa | 119 | |
calmantara186 | 0:a6918b57d3fa | 120 | //Start Millis |
calmantara186 | 11:d4cf81f59601 | 121 | wait_ms(1000); |
calmantara186 | 0:a6918b57d3fa | 122 | startMillis(); |
calmantara186 | 6:5632ff2142c0 | 123 | //Looping Utama |
calmantara186 | 6:5632ff2142c0 | 124 | while(1){ |
calmantara186 | 6:5632ff2142c0 | 125 | //Looping Program Planning |
calmantara186 | 6:5632ff2142c0 | 126 | while(in_program == 0){ |
calmantara186 | 11:d4cf81f59601 | 127 | |
calmantara186 | 11:d4cf81f59601 | 128 | if (mybutton == 0){ |
calmantara186 | 11:d4cf81f59601 | 129 | pidRotY.reset(); |
calmantara186 | 11:d4cf81f59601 | 130 | pidDisY.reset(); |
calmantara186 | 11:d4cf81f59601 | 131 | pidRotX.reset(); |
calmantara186 | 11:d4cf81f59601 | 132 | pidDisX.reset(); |
calmantara186 | 11:d4cf81f59601 | 133 | jarak_y = 0; |
calmantara186 | 11:d4cf81f59601 | 134 | jarak_x = 0; |
calmantara186 | 11:d4cf81f59601 | 135 | state+=1; |
calmantara186 | 11:d4cf81f59601 | 136 | in_program = 1; |
calmantara186 | 11:d4cf81f59601 | 137 | } |
calmantara186 | 6:5632ff2142c0 | 138 | }/* end of planning program */ |
calmantara186 | 6:5632ff2142c0 | 139 | //Looping Program Utama |
calmantara186 | 6:5632ff2142c0 | 140 | while(in_program == 1){ |
calmantara186 | 10:e550e5daed74 | 141 | current_millis = millis(); |
calmantara186 | 11:d4cf81f59601 | 142 | if((jarak_y <= target_y[state]-(float)5.0)||(jarak_y >= target_y[state]+(float)5.0)){ |
calmantara186 | 9:e2c3175936fb | 143 | /* masuk ke case sumbu Y */ |
calmantara186 | 7:cf822f5e7b12 | 144 | if(current_millis - prev_millis1 >= 12){ |
calmantara186 | 7:cf822f5e7b12 | 145 | pidMotor(0); |
calmantara186 | 7:cf822f5e7b12 | 146 | } |
calmantara186 | 6:5632ff2142c0 | 147 | |
calmantara186 | 7:cf822f5e7b12 | 148 | /* Speed Motor */ |
calmantara186 | 7:cf822f5e7b12 | 149 | limitMotor(); |
calmantara186 | 10:e550e5daed74 | 150 | motor_depan.speed(pwm_depan); |
calmantara186 | 7:cf822f5e7b12 | 151 | motor_kanan.speed(pwm_kanan); |
calmantara186 | 7:cf822f5e7b12 | 152 | motor_kiri.speed(pwm_kiri); |
calmantara186 | 11:d4cf81f59601 | 153 | //prev_millis2 = current_millis; |
calmantara186 | 11:d4cf81f59601 | 154 | //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_depan, pulses_kanan, pulses_kiri); |
calmantara186 | 6:5632ff2142c0 | 155 | } |
calmantara186 | 11:d4cf81f59601 | 156 | if((jarak_x <= target_x[state]-(float)5.0)||(jarak_x >= target_x[state]+(float)5.0)){ |
calmantara186 | 10:e550e5daed74 | 157 | /* masuk ke case sumbu X */ |
calmantara186 | 10:e550e5daed74 | 158 | if(current_millis - prev_millis1 >= 12){ |
calmantara186 | 10:e550e5daed74 | 159 | pidMotor(1); |
calmantara186 | 10:e550e5daed74 | 160 | } |
calmantara186 | 10:e550e5daed74 | 161 | |
calmantara186 | 10:e550e5daed74 | 162 | /* Speed Motor */ |
calmantara186 | 10:e550e5daed74 | 163 | limitMotor(); |
calmantara186 | 10:e550e5daed74 | 164 | motor_depan.speed(pwm_depan); |
calmantara186 | 10:e550e5daed74 | 165 | motor_kanan.speed(pwm_kanan); |
calmantara186 | 11:d4cf81f59601 | 166 | motor_kiri.speed(pwm_kiri); |
calmantara186 | 11:d4cf81f59601 | 167 | //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_x, d_tetha, rev_depan, rev_kanan, rev_kiri); |
calmantara186 | 9:e2c3175936fb | 168 | } |
calmantara186 | 7:cf822f5e7b12 | 169 | else{ |
calmantara186 | 9:e2c3175936fb | 170 | pwm_depan = 0.0; |
calmantara186 | 7:cf822f5e7b12 | 171 | pwm_kanan = 0.0; |
calmantara186 | 7:cf822f5e7b12 | 172 | pwm_kiri = 0.0; |
calmantara186 | 9:e2c3175936fb | 173 | motor_depan.brake(1); |
calmantara186 | 7:cf822f5e7b12 | 174 | motor_kanan.brake(1); |
calmantara186 | 7:cf822f5e7b12 | 175 | motor_kiri.brake(1); |
calmantara186 | 11:d4cf81f59601 | 176 | in_program = 0; |
calmantara186 | 9:e2c3175936fb | 177 | } |
calmantara186 | 9:e2c3175936fb | 178 | |
calmantara186 | 9:e2c3175936fb | 179 | /* mengambil data encoder motor */ |
calmantara186 | 9:e2c3175936fb | 180 | getValue(); |
calmantara186 | 7:cf822f5e7b12 | 181 | |
calmantara186 | 7:cf822f5e7b12 | 182 | /* masuk ke perhitungan parameter */ |
calmantara186 | 10:e550e5daed74 | 183 | jarak_y += odometry.getY(rev_kanan, rev_kiri); |
calmantara186 | 10:e550e5daed74 | 184 | jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri); |
calmantara186 | 10:e550e5daed74 | 185 | d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri); |
calmantara186 | 9:e2c3175936fb | 186 | encMotor_depan.reset(); |
calmantara186 | 7:cf822f5e7b12 | 187 | encMotor_kanan.reset(); |
calmantara186 | 7:cf822f5e7b12 | 188 | encMotor_kiri.reset(); |
calmantara186 | 11:d4cf81f59601 | 189 | //pc.printf("%.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_kanan, pulses_kiri); |
calmantara186 | 2:dea57b3f84cd | 190 | } |
calmantara186 | 6:5632ff2142c0 | 191 | } /* end of main program */ |
calmantara186 | 2:dea57b3f84cd | 192 | } |
calmantara186 | 2:dea57b3f84cd | 193 | |
calmantara186 | 2:dea57b3f84cd | 194 | /************************************ |
calmantara186 | 2:dea57b3f84cd | 195 | * Deklarasi Fungsi dan Prosedur * |
calmantara186 | 2:dea57b3f84cd | 196 | ************************************/ |
calmantara186 | 2:dea57b3f84cd | 197 | void limitMotor(){ |
calmantara186 | 2:dea57b3f84cd | 198 | /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */ |
calmantara186 | 6:5632ff2142c0 | 199 | if(pwm_kanan > limit_pwm){ |
calmantara186 | 1:5e0cb74f950e | 200 | pwm_kanan = limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 201 | } |
calmantara186 | 6:5632ff2142c0 | 202 | else if(pwm_kanan < (-limit_pwm)){ |
calmantara186 | 6:5632ff2142c0 | 203 | pwm_kanan = -limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 204 | } |
calmantara186 | 6:5632ff2142c0 | 205 | if(pwm_kiri > limit_pwm){ |
calmantara186 | 6:5632ff2142c0 | 206 | pwm_kiri = limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 207 | } |
calmantara186 | 6:5632ff2142c0 | 208 | else if(pwm_kiri < (-limit_pwm)){ |
calmantara186 | 6:5632ff2142c0 | 209 | pwm_kiri = -limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 210 | } |
calmantara186 | 6:5632ff2142c0 | 211 | if(pwm_depan > limit_pwm){ |
calmantara186 | 6:5632ff2142c0 | 212 | pwm_depan = limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 213 | } |
calmantara186 | 6:5632ff2142c0 | 214 | else if(pwm_depan < (-limit_pwm)){ |
calmantara186 | 6:5632ff2142c0 | 215 | pwm_depan = -limit_pwm; |
calmantara186 | 6:5632ff2142c0 | 216 | } |
calmantara186 | 3:4801d83f397c | 217 | }/* end of limitMotor */ |
calmantara186 | 3:4801d83f397c | 218 | |
calmantara186 | 6:5632ff2142c0 | 219 | void pidMotor(int x){ |
calmantara186 | 6:5632ff2142c0 | 220 | /** |
calmantara186 | 6:5632ff2142c0 | 221 | * prosedur untuk gerak motor |
calmantara186 | 6:5632ff2142c0 | 222 | * 0 : maju dan mundur |
calmantara186 | 6:5632ff2142c0 | 223 | * 1 : kanan dan kiri |
calmantara186 | 6:5632ff2142c0 | 224 | **/ |
calmantara186 | 6:5632ff2142c0 | 225 | if(x==0){ |
calmantara186 | 6:5632ff2142c0 | 226 | /* masuk ke perhitungan ketika sudah pada time sampling */ |
calmantara186 | 9:e2c3175936fb | 227 | /* mengambil data encoder motor */ |
calmantara186 | 9:e2c3175936fb | 228 | getValue(); |
calmantara186 | 6:5632ff2142c0 | 229 | |
calmantara186 | 6:5632ff2142c0 | 230 | /* masuk ke perhitungan parameter */ |
calmantara186 | 10:e550e5daed74 | 231 | jarak_y += odometry.getY(rev_kanan, rev_kiri); |
calmantara186 | 10:e550e5daed74 | 232 | d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri); |
calmantara186 | 6:5632ff2142c0 | 233 | |
calmantara186 | 10:e550e5daed74 | 234 | speed_roty = pidRotY.pidCount(tn_tetha, d_tetha, limit_rot); |
calmantara186 | 10:e550e5daed74 | 235 | speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_dis); |
calmantara186 | 9:e2c3175936fb | 236 | |
calmantara186 | 6:5632ff2142c0 | 237 | /* Kondisi untuk menambahkan speed */ |
calmantara186 | 7:cf822f5e7b12 | 238 | pwm_kiri = -speed_disy + speed_roty; |
calmantara186 | 6:5632ff2142c0 | 239 | pwm_kanan = speed_disy + speed_roty; |
calmantara186 | 9:e2c3175936fb | 240 | pwm_depan = speed_roty; |
calmantara186 | 6:5632ff2142c0 | 241 | |
calmantara186 | 6:5632ff2142c0 | 242 | prev_millis1 = current_millis; |
calmantara186 | 9:e2c3175936fb | 243 | encMotor_depan.reset(); |
calmantara186 | 6:5632ff2142c0 | 244 | encMotor_kanan.reset(); |
calmantara186 | 6:5632ff2142c0 | 245 | encMotor_kiri.reset(); |
calmantara186 | 6:5632ff2142c0 | 246 | |
calmantara186 | 6:5632ff2142c0 | 247 | } |
calmantara186 | 7:cf822f5e7b12 | 248 | else if(x==1){ |
calmantara186 | 7:cf822f5e7b12 | 249 | /* masuk ke perhitungan ketika sudah pada time sampling */ |
calmantara186 | 9:e2c3175936fb | 250 | /* mengambil data encoder motor */ |
calmantara186 | 9:e2c3175936fb | 251 | getValue(); |
calmantara186 | 7:cf822f5e7b12 | 252 | |
calmantara186 | 7:cf822f5e7b12 | 253 | /* masuk ke perhitungan parameter */ |
calmantara186 | 10:e550e5daed74 | 254 | jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri); |
calmantara186 | 10:e550e5daed74 | 255 | d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri); |
calmantara186 | 7:cf822f5e7b12 | 256 | |
calmantara186 | 10:e550e5daed74 | 257 | speed_rotx = pidRotX.pidCount(tn_tetha, d_tetha, limit_rot); |
calmantara186 | 10:e550e5daed74 | 258 | speed_disx = pidDisX.pidCount(target_x[0], jarak_x, limit_dis); |
calmantara186 | 9:e2c3175936fb | 259 | |
calmantara186 | 7:cf822f5e7b12 | 260 | /* Kondisi untuk menambahkan speed */ |
calmantara186 | 11:d4cf81f59601 | 261 | pwm_kiri = ((float)0.63*speed_disx) + speed_rotx; |
calmantara186 | 11:d4cf81f59601 | 262 | pwm_kanan = ((float)0.63*speed_disx) + speed_rotx; |
calmantara186 | 10:e550e5daed74 | 263 | pwm_depan = -(speed_disx) + speed_rotx; |
calmantara186 | 7:cf822f5e7b12 | 264 | |
calmantara186 | 7:cf822f5e7b12 | 265 | prev_millis1 = current_millis; |
calmantara186 | 9:e2c3175936fb | 266 | encMotor_depan.reset(); |
calmantara186 | 7:cf822f5e7b12 | 267 | encMotor_kanan.reset(); |
calmantara186 | 7:cf822f5e7b12 | 268 | encMotor_kiri.reset(); |
calmantara186 | 7:cf822f5e7b12 | 269 | } |
calmantara186 | 6:5632ff2142c0 | 270 | }/* end of pidMotor */ |
calmantara186 | 6:5632ff2142c0 | 271 | |
calmantara186 | 9:e2c3175936fb | 272 | void getValue(){ |
calmantara186 | 9:e2c3175936fb | 273 | /* |
calmantara186 | 9:e2c3175936fb | 274 | * Procedure yang digunakan untuk menghitung value pulses encoder |
calmantara186 | 9:e2c3175936fb | 275 | * dan revolusi dari motor |
calmantara186 | 9:e2c3175936fb | 276 | */ |
calmantara186 | 9:e2c3175936fb | 277 | pulses_depan = (float)encMotor_depan.getPulses(); |
calmantara186 | 9:e2c3175936fb | 278 | pulses_kanan = (float) encMotor_kanan.getPulses(); |
calmantara186 | 9:e2c3175936fb | 279 | pulses_kiri = (float) encMotor_kiri.getPulses(); |
calmantara186 | 10:e550e5daed74 | 280 | rev_depan = (float) pulses_depan/540; |
calmantara186 | 9:e2c3175936fb | 281 | rev_kanan = (float) pulses_kanan/540; |
calmantara186 | 9:e2c3175936fb | 282 | rev_kiri = (float) pulses_kiri/540; |
calmantara186 | 9:e2c3175936fb | 283 | }/* end of getValue */ |