PIDremote.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "QEI.h"
00004 #include "PID.h"
00005 
00006 Serial pc(USBTX, USBRX);
00007 Serial bluetooth(PB_6, PA_10);
00008 
00009 QEI DepanKananQEI(PB_8, PC_9, NC, 986); //roda depan kanan PB_1, PB_15, NC, 624
00010 QEI BelakangKananQEI(PB_1, PB_15, NC, 986); //roda belakang kiri PB_14, PB_13, NC, 624
00011 QEI DepanKiriQEI(PC_10, PC_12, NC, 986); //roda depan kiri 
00012 QEI BelakangKiriQEI(PA_15, PB_7, NC, 986); //roda belakang kiri
00013 
00014 PID DepanKananPID(0.62, 0.1, 0.00001, 0.02);  //0000001 Kc, Ti, Td MOTOR baru nomor 1
00015 PID BelakangKananPID(0.6, 0.1, 0.000001, 0.02);  //Kc, Ti, Td MOTOR baru nomor 2
00016 PID BelakangKiriPID(0.89, 0.1, 0.000000000001, 0.02);  //Kc, Ti, Td MOTOR baru nomor 3
00017 PID DepanKiriPID(0.68, 0.1, 0.0000001, 0.02);  //Kc, Ti, Td MOTOR baru nomor 4
00018 
00019 Motor DepanKanan(PA_11, PC_8, PC_6);//PB_9, PA_12, PC_5
00020 Motor DepanKiri(PA_0, PB_0, PA_4);
00021 Motor BelakangKanan(PB_9, PC_5, PA_12);//PA_11, PC_6, PC_8
00022 Motor BelakangKiri(PA_1, PC_0, PC_1);
00023 
00024 Timer t_jalan;
00025 
00026 char nilai, huruf;
00027 int Jalan;
00028 int Jalan_Maju =1;
00029 int Berhenti = 0;
00030 int Jalan_Mundur = 2;
00031 int Jalan_Kanan = 3;
00032 int Jalan_Kiri = 4;
00033 int ke_setpoint = 5;
00034 int mode_inverse = 6;
00035 
00036 int DepanKananPulses      = 0; 
00037 int DepanKananPrevPulses  = 0; 
00038 float DepanKananVelocity  = 0.0; 
00039     
00040 int BelakangKananPulses     = 0; 
00041 int BelakangKananPrevPulses = 0; 
00042 float BelakangKananVelocity = 0.0;
00043 
00044 int BelakangKiriPulses     = 0;
00045 int BelakangKiriPrevPulses = 0; 
00046 float BelakangKiriVelocity = 0.0;
00047     
00048 int DepanKiriPulses     = 0;
00049 int DepanKiriPrevPulses = 0;
00050 float DepanKiriVelocity = 0.0;
00051 
00052 int Depan_Kanan_Count;
00053 int Depan_Kiri_Count;
00054 int Belakang_Kanan_Count;
00055 int Belakang_Kiri_Count;
00056 
00057 int d_pulse_DepanKanan;
00058 int d_pulse_DepanKiri;
00059 int d_pulse_BelakangKanan;
00060 int d_pulse_BelakangKiri;
00061 
00062 int koordinat_x_DepanKiri;
00063 int koordinat_x_DepanKanan;
00064 int koordinat_x_BelakangKiri;
00065 int koordinat_x_BelakangKanan;
00066 int koordinat_y_DepanKiri;
00067 int koordinat_y_DepanKanan;
00068 int koordinat_y_BelakangKiri;
00069 int koordinat_y_BelakangKanan;
00070 int koordinat_x;
00071 int koordinat_y;
00072 int Resolusi = 986;
00073 float pi = 3.141;
00074 float diameter = 5.8f;
00075 float rtd = pi/180.0f;
00076 int setpoint_x;
00077 int setpoint_y;
00078 float radius = 0.029;
00079 float orientasi;
00080 
00081 float target_depan_kiri;
00082 float target_depan_kanan;
00083 float target_belakang_kiri;
00084 float target_belakang_kanan;
00085 int arah_depan_kiri;
00086 int arah_depan_kanan;
00087 int arah_belakang_kiri;
00088 int arah_belakang_kanan;
00089 
00090 
00091 void fungsi_bluetooth(void)
00092 {
00093     if(bluetooth.readable())
00094     {
00095         setpoint_x = bluetooth.getc();
00096         setpoint_y = bluetooth.getc();
00097         nilai = bluetooth.getc();
00098         if (nilai == 'S')
00099         {
00100             huruf = 'S';
00101             Jalan = Berhenti;  
00102         }
00103         else if (nilai == 'F')
00104         {
00105             huruf = 'F';
00106             Jalan = Jalan_Maju;
00107         }
00108         else if(nilai =='B')
00109         {
00110             huruf = 'B';
00111             Jalan = Jalan_Mundur;
00112         }
00113         else if(nilai =='L')
00114         {
00115             huruf = 'L';
00116             Jalan = Jalan_Kiri;
00117         }
00118         else if(nilai =='R')
00119         {
00120             huruf = 'R';
00121             Jalan = Jalan_Kanan;
00122         }
00123         else if(nilai =='G')
00124         {
00125             huruf = 'G';
00126             Jalan = ke_setpoint;
00127         }
00128         else if(nilai == 'Y')
00129         {
00130             huruf = 'G';
00131             Jalan = mode_inverse;
00132         }
00133         else if(nilai == 'S')
00134         {
00135             Jalan = Berhenti;
00136         }         
00137     }
00138 }
00139 
00140 void kosongkan()
00141 {
00142     BelakangKananQEI.reset();
00143     DepanKananQEI.reset();
00144     DepanKiriQEI.reset();
00145     BelakangKiriQEI.reset();
00146                 
00147     DepanKananPulses      = 0;
00148     DepanKananPrevPulses  = 0; 
00149     DepanKananVelocity  = 0.0; 
00150                 
00151     BelakangKananPulses     = 0; 
00152     BelakangKananPrevPulses = 0; 
00153     BelakangKananVelocity = 0.0;
00154     
00155     BelakangKiriPulses     = 0;
00156     BelakangKiriPrevPulses = 0; 
00157     BelakangKiriVelocity = 0.0;
00158                 
00159     DepanKiriPulses     = 0;
00160     DepanKiriPrevPulses = 0;
00161     DepanKiriVelocity = 0.0; 
00162 }
00163 
00164 void proses_kecepatan()
00165 {
00166     DepanKananPulses = DepanKananQEI.getPulses();
00167     d_pulse_DepanKanan = DepanKananPulses - DepanKananPrevPulses;
00168     DepanKananVelocity = (DepanKananPulses - DepanKananPrevPulses) / 0.02;
00169     DepanKananPrevPulses = DepanKananPulses;             
00170     DepanKananPID.setProcessValue(fabs(DepanKananVelocity));
00171     
00172     DepanKiriPulses = DepanKiriQEI.getPulses();
00173     d_pulse_DepanKiri = DepanKiriPulses - DepanKiriPrevPulses;
00174     DepanKiriVelocity = (DepanKiriPulses - DepanKiriPrevPulses) / 0.02;
00175     DepanKiriPrevPulses = DepanKiriPulses;             
00176     DepanKiriPID.setProcessValue(fabs(DepanKiriVelocity));
00177     
00178     BelakangKananPulses = BelakangKananQEI.getPulses();
00179     d_pulse_BelakangKanan = BelakangKananPulses - BelakangKananPrevPulses;
00180     BelakangKananVelocity = (BelakangKananPulses - BelakangKananPrevPulses) / 0.02;
00181     BelakangKananPrevPulses = BelakangKananPulses;             
00182     BelakangKananPID.setProcessValue(fabs(BelakangKananVelocity));
00183     
00184     BelakangKiriPulses = BelakangKiriQEI.getPulses();
00185     d_pulse_BelakangKiri = BelakangKiriPulses - BelakangKiriPrevPulses;
00186     BelakangKiriVelocity = (BelakangKiriPulses - BelakangKiriPrevPulses) / 0.02;
00187     BelakangKiriPrevPulses = BelakangKiriPulses;             
00188     BelakangKiriPID.setProcessValue(fabs(BelakangKiriVelocity));
00189 }
00190 
00191 void Get_Count()
00192 {
00193     Depan_Kanan_Count = Depan_Kanan_Count + d_pulse_DepanKanan;
00194     Depan_Kiri_Count = Depan_Kiri_Count + d_pulse_DepanKiri;
00195     Belakang_Kanan_Count = Belakang_Kanan_Count + d_pulse_BelakangKanan;
00196     Belakang_Kiri_Count = Belakang_Kiri_Count + d_pulse_BelakangKiri;
00197 }
00198 
00199 void Cari_Koordinat()
00200 {
00201     koordinat_x_DepanKiri = ((double)Depan_Kiri_Count/(double)Resolusi)*pi*diameter*cos(45.0f*rtd);
00202     koordinat_x_DepanKanan = ((double)Depan_Kanan_Count/(double)Resolusi)*pi*diameter*cos(135.0f*rtd);
00203     koordinat_x_BelakangKanan = ((double)Belakang_Kanan_Count/(double)Resolusi)*pi*diameter*cos(225.0f*rtd);
00204     koordinat_x_BelakangKiri = ((double)Belakang_Kiri_Count/(double)Resolusi)*pi*diameter*cos(315.0f*rtd);
00205     
00206     koordinat_y_DepanKiri = ((double)Depan_Kiri_Count/(double)Resolusi)*pi*diameter*sin(45.0f*rtd);
00207     koordinat_y_DepanKanan = ((double)Depan_Kanan_Count/(double)Resolusi)*pi*diameter*sin(135.0f*rtd);
00208     koordinat_y_BelakangKanan = ((double)Belakang_Kanan_Count/(double)Resolusi)*pi*diameter*sin(225.0f*rtd);
00209     koordinat_y_BelakangKiri = ((double)Belakang_Kiri_Count/(double)Resolusi)*pi*diameter*sin(315.0f*rtd);
00210     
00211     orientasi = ((double)(koordinat_x_DepanKanan + koordinat_x_BelakangKanan) - (double)(koordinat_x_DepanKiri + koordinat_x_BelakangKiri))/(4.0f*13.0f);
00212     orientasi = orientasi*57.29;
00213     koordinat_y = (koordinat_x_DepanKiri + koordinat_x_DepanKanan + koordinat_x_BelakangKiri + koordinat_x_BelakangKanan)/2;
00214     koordinat_x = (koordinat_y_DepanKiri + koordinat_y_DepanKanan + koordinat_y_BelakangKiri + koordinat_y_BelakangKanan)/2;
00215 }
00216 
00217 int cari_abs(float a)
00218 {
00219     if(a<0)
00220     {
00221         return -1;
00222     }
00223     else if(a>0)
00224     {
00225         return 1;
00226     }
00227     else 
00228     {
00229         return 0;
00230     }
00231 }
00232 
00233 void cari_inverse(float setpoint_sudut, float target_x, float target_y)
00234 {
00235     target_x = target_x - koordinat_x;
00236     target_y = target_y - koordinat_y;
00237     float target_vx;
00238     float target_vy;
00239     target_x = (double)target_x/100.0f;
00240     target_y = (double)target_y/100.0f;
00241     if(fabs(target_x)>fabs(target_y))
00242     {
00243         if((target_x<0)&&(target_y<0))
00244         {
00245             target_vx = -0.6;
00246             target_vy = ((double)target_y/(double)target_x)*target_vx;
00247         }
00248         else if((target_x<0)&&(target_y>0))
00249         {
00250             target_vx = -0.6;
00251             target_vy = ((double)target_y/(double)target_x)*target_vx*-1;
00252         }
00253         else if((target_x>0)&&(target_y>0))
00254         {
00255             target_vx = 0.6;
00256             target_vy = ((double)target_y/(double)target_x)*target_vx;
00257         }
00258         else if((target_x>0)&&(target_y<0))
00259         {
00260             target_vx = 0.6;
00261             target_vy = ((double)target_y/(double)target_x)*target_vx*-1;
00262         }
00263         else if((target_y==0)&&(target_x>0))
00264         {
00265             target_vx = 0.6;
00266             target_vy = 0;
00267         }
00268         else if((target_y==0)&&(target_x<0))
00269         {
00270             target_vx = -0.6;
00271             target_vy = 0;
00272         }
00273     }
00274     else if(fabs(target_x)<fabs(target_y))
00275     {
00276         if((target_x<0)&&(target_y<0))
00277         {
00278             target_vy = -0.6;
00279             target_vx = ((double)target_x/(double)target_y)*target_vy;
00280         }
00281         else if((target_x<0)&&(target_y>0))
00282         {
00283             target_vy = -0.6;
00284             target_vx = ((double)target_x/(double)target_y)*target_vy*-1;
00285         }
00286         else if((target_x>0)&&(target_y>0))
00287         {
00288             target_vy = 0.6;
00289             target_vx = ((double)target_x/(double)target_y)*target_vy;
00290         }
00291         else if((target_x>0)&&(target_y<0))
00292         {
00293             target_vy = 0.6;
00294             target_vx = ((double)target_x/(double)target_y)*target_vy*-1;
00295         }
00296         else if((target_x==0)&&(target_y>0))
00297         {
00298             target_vy = 0.6;
00299             target_vx = 0;
00300         }
00301         else if((target_x==0)&&(target_y<0))
00302         {
00303             target_vy = -0.6;
00304             target_vx = 0;
00305         }
00306     }
00307     else if(target_x==target_y)
00308     {
00309         if((target_x<0)&&(target_y<0))
00310         {
00311             target_vy = -0.6;
00312             target_vx = target_vy;
00313         }
00314         else if((target_x<0)&&(target_y>0))
00315         {
00316             target_vy = -0.6;
00317             target_vx = -1*target_vy;
00318         }
00319         else if((target_x>0)&&(target_y<0))
00320         {
00321             target_vy = 0.6;
00322             target_vx = -1*target_vy;
00323         }
00324         else if((target_x>0)&&(target_y>0))
00325         {
00326             target_vy = 0.6;
00327             target_vx = target_vy;
00328         }
00329     }
00330     setpoint_sudut = setpoint_sudut*rtd;
00331     target_depan_kiri = (1/radius)*(sin(45.0f*rtd)*target_vx + cos(45.0f*rtd)*target_vy + radius*setpoint_sudut);
00332     target_depan_kanan = (1/radius)*(sin(135.0f*rtd)*target_vx + cos(135.0f*rtd)*target_vy + radius*setpoint_sudut);
00333     target_belakang_kanan = (1/radius)*(sin(225.0f*rtd)*target_vx + cos(225.0f*rtd)*target_vy + radius*setpoint_sudut);
00334     target_belakang_kiri = (1/radius)*(sin(315.0f*rtd)*target_vx + cos(315.0f*rtd)*target_vy + radius*setpoint_sudut);
00335     target_depan_kiri = (target_depan_kiri*624.0f)/(2.0f*3.14);
00336     target_depan_kanan = (target_depan_kanan*624.0f)/(2.0f*3.14);
00337     target_belakang_kiri = (target_belakang_kiri*624.0f)/(2.0f*3.14);
00338     target_belakang_kanan = (target_belakang_kanan*624.0f)/(2.0f*3.14);
00339     arah_depan_kiri = cari_abs(target_depan_kiri);
00340     arah_depan_kanan = cari_abs(target_depan_kanan);
00341     arah_belakang_kiri = cari_abs(target_belakang_kiri);
00342     arah_belakang_kanan = cari_abs(target_belakang_kanan);
00343 }
00344 
00345 void set_inverse()
00346 {
00347     DepanKananPID.setSetPoint(fabs(target_depan_kanan));
00348     BelakangKananPID.setSetPoint(fabs(target_belakang_kanan));
00349     BelakangKiriPID.setSetPoint(fabs(target_belakang_kiri));
00350     DepanKiriPID.setSetPoint(fabs(target_depan_kiri));
00351 }
00352 
00353 void jalan_maju()
00354 {
00355     t_jalan.reset();
00356     t_jalan.start();
00357     proses_kecepatan();
00358     Get_Count();
00359     Cari_Koordinat();
00360     pc.printf("Koordinat X : %d\n", koordinat_x);
00361     pc.printf("Koordinat Y : %d\n", koordinat_y);
00362     BelakangKanan.speed(BelakangKananPID.compute()*-1);
00363     BelakangKiri.speed(BelakangKiriPID.compute());
00364     DepanKanan.speed(DepanKananPID.compute()*-1);
00365     DepanKiri.speed(DepanKiriPID.compute());
00366     while(t_jalan.read_ms()<=20)
00367     {
00368     }
00369     t_jalan.reset();
00370 }
00371 
00372 void jalan_mundur()
00373 {
00374     t_jalan.reset();
00375     t_jalan.start();
00376     proses_kecepatan();
00377     Get_Count();
00378     Cari_Koordinat();
00379     pc.printf("Koordinat X : %d\n", koordinat_x);
00380     pc.printf("Koordinat Y : %d\n", koordinat_y);
00381     BelakangKanan.speed(BelakangKananPID.compute());
00382     BelakangKiri.speed(BelakangKiriPID.compute()*-1);
00383     DepanKanan.speed(DepanKananPID.compute());
00384     DepanKiri.speed(DepanKiriPID.compute()*-1);
00385     while(t_jalan.read_ms()<=20)
00386     {
00387     }
00388     t_jalan.reset();
00389 }
00390 
00391 void jalan_kanan()
00392 {
00393     t_jalan.reset();
00394     t_jalan.start();
00395     proses_kecepatan();
00396     Get_Count();
00397     Cari_Koordinat();
00398     pc.printf("Koordinat X : %d\n", koordinat_x);
00399     pc.printf("Koordinat Y : %d\n", koordinat_y);
00400     BelakangKanan.speed(BelakangKananPID.compute()*-1);
00401     BelakangKiri.speed(BelakangKiriPID.compute()*-1);
00402     DepanKanan.speed(DepanKananPID.compute());
00403     DepanKiri.speed(DepanKiriPID.compute());
00404     while(t_jalan.read_ms()<=20)
00405     {
00406     }
00407     t_jalan.reset();
00408 }
00409 
00410 void jalan_kiri()
00411 {
00412     t_jalan.reset();
00413     t_jalan.start();
00414     proses_kecepatan();
00415     Get_Count();
00416     Cari_Koordinat();
00417     pc.printf("Koordinat X : %d\n", koordinat_x);
00418     pc.printf("Koordinat Y : %d\n", koordinat_y);
00419     BelakangKanan.speed(BelakangKananPID.compute());
00420     BelakangKiri.speed(BelakangKiriPID.compute());
00421     DepanKanan.speed(DepanKananPID.compute()*-1);
00422     DepanKiri.speed(DepanKiriPID.compute()*-1);
00423     while(t_jalan.read_ms()<=20)
00424     {
00425     }
00426     t_jalan.reset();
00427 }
00428 
00429 void berhenti()
00430 {
00431     t_jalan.reset();
00432     t_jalan.start();
00433     pc.printf("Koordinat X : %d\n", koordinat_x);
00434     pc.printf("Koordinat Y : %d\n", koordinat_y);
00435     DepanKanan.brake(0);
00436     BelakangKanan.brake(0);
00437     BelakangKiri.brake(0);
00438     DepanKiri.brake(0);
00439     while(t_jalan.read_ms()<=20)
00440     {
00441     }
00442     t_jalan.reset();
00443 }
00444 
00445 void ke_setpoint_x()
00446 {
00447     if(koordinat_x<setpoint_x)
00448     {
00449         kosongkan();
00450         while(koordinat_x<setpoint_x)
00451         {
00452             jalan_kanan();
00453         }
00454         DepanKanan.brake();
00455         BelakangKanan.brake();
00456         BelakangKiri.brake();
00457         DepanKiri.brake();
00458     }
00459     else if(koordinat_x>setpoint_x)
00460     {
00461         kosongkan();
00462         while(koordinat_x>setpoint_x)
00463         {
00464             jalan_kiri();
00465         }
00466         DepanKanan.brake();
00467         BelakangKanan.brake();
00468         BelakangKiri.brake();
00469         DepanKiri.brake();
00470     }
00471     else if(koordinat_x==setpoint_x)
00472     {
00473         berhenti();
00474     }
00475 }
00476 
00477 void ke_setpoint_y()
00478 {
00479     if(koordinat_y<setpoint_y)
00480     {
00481         kosongkan();
00482         while(koordinat_y<setpoint_y)
00483         {
00484             jalan_maju();
00485         }
00486         DepanKanan.brake();
00487         BelakangKanan.brake();
00488         BelakangKiri.brake();
00489         DepanKiri.brake();
00490     }
00491     else if(koordinat_y>setpoint_y)
00492     {
00493         kosongkan();
00494         while(koordinat_y>setpoint_y)
00495         {
00496             jalan_mundur();
00497         }
00498         DepanKanan.brake();
00499         BelakangKanan.brake();
00500         BelakangKiri.brake();
00501         DepanKiri.brake();
00502     }
00503     else if(koordinat_y==setpoint_y)
00504     {
00505         berhenti();
00506     }
00507 }
00508 
00509 int main()
00510 {   
00511     pc.baud(38400);
00512     bluetooth.baud(9600);
00513     bluetooth.attach(&fungsi_bluetooth, Serial::RxIrq);
00514     
00515     DepanKanan.period(0.01f);    
00516     BelakangKanan.period(0.01f);
00517     BelakangKiri.period(0.01f);  //Set motor PWM periods to 20KHz.
00518     DepanKiri.period(0.01f);
00519 
00520     DepanKananPID.setInputLimits(0, 3000);//Input  units: counts per second.
00521     DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %.
00522     DepanKananPID.setMode(AUTO_MODE);
00523     
00524     BelakangKananPID.setInputLimits(0, 3000);
00525     BelakangKananPID.setOutputLimits(0.0, 0.9);
00526     BelakangKananPID.setMode(AUTO_MODE);
00527     
00528     BelakangKiriPID.setInputLimits(0, 3000);
00529     BelakangKiriPID.setOutputLimits(0.0, 0.9);
00530     BelakangKiriPID.setMode(AUTO_MODE);
00531     
00532     DepanKiriPID.setInputLimits(0, 3000);
00533     DepanKiriPID.setOutputLimits(0.0, 0.9);
00534     DepanKiriPID.setMode(AUTO_MODE);
00535     
00536     DepanKananPID.setSetPoint(1400);
00537     BelakangKananPID.setSetPoint(1400);
00538     BelakangKiriPID.setSetPoint(1400);
00539     DepanKiriPID.setSetPoint(1400); 
00540     while(1)
00541     {
00542         
00543         if(Jalan == Jalan_Maju)
00544         {
00545             kosongkan();
00546             while(Jalan== Jalan_Maju)
00547             {
00548                 t_jalan.reset();
00549                 t_jalan.start();
00550                 proses_kecepatan();
00551                 Get_Count();
00552                 Cari_Koordinat();
00553                 //pc.printf("Koordinat X : %d\n", koordinat_x);
00554                 //pc.printf("Koordinat Y : %d\n", koordinat_y);
00555                 pc.printf("Orientasi : %f\n", orientasi);
00556                 BelakangKanan.speed(BelakangKananPID.compute()*-1);
00557                 BelakangKiri.speed(BelakangKiriPID.compute());
00558                 DepanKanan.speed(DepanKananPID.compute()*-1);
00559                 DepanKiri.speed(DepanKiriPID.compute());
00560                 while(t_jalan.read_ms()<=20)
00561                 {
00562                 }
00563                 t_jalan.reset();
00564             }
00565             DepanKanan.brake();
00566             BelakangKanan.brake();
00567             BelakangKiri.brake();
00568             DepanKiri.brake();
00569         }
00570         
00571         else if(Jalan == Jalan_Mundur)
00572         {
00573             kosongkan();
00574             while(Jalan== Jalan_Mundur)
00575             {
00576                 t_jalan.reset();
00577                 t_jalan.start();
00578                 proses_kecepatan();
00579                 Get_Count();
00580                 Cari_Koordinat();
00581                 //pc.printf("Koordinat X : %d\n", koordinat_x);
00582                 //pc.printf("Koordinat Y : %d\n", koordinat_y);
00583                 pc.printf("Orientasi : %f\n", orientasi);
00584                 BelakangKanan.speed(BelakangKananPID.compute());
00585                 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
00586                 DepanKanan.speed(DepanKananPID.compute());
00587                 DepanKiri.speed(DepanKiriPID.compute()*-1);
00588                 while(t_jalan.read_ms()<=20)
00589                 {
00590                 }
00591                 t_jalan.reset();
00592             }
00593             DepanKanan.brake();
00594             BelakangKanan.brake();
00595             BelakangKiri.brake();
00596             DepanKiri.brake();
00597         }
00598         
00599         else if(Jalan == Jalan_Kanan)
00600         {
00601             kosongkan();
00602             while(Jalan== Jalan_Kanan)
00603             {
00604                 t_jalan.reset();
00605                 t_jalan.start();
00606                 proses_kecepatan();
00607                 Get_Count();
00608                 Cari_Koordinat();
00609                 //pc.printf("Koordinat X : %d\n", koordinat_x);
00610                 //pc.printf("Koordinat Y : %d\n", koordinat_y);
00611                 pc.printf("Orientasi : %f\n", orientasi);
00612                 BelakangKanan.speed(BelakangKananPID.compute());
00613                 BelakangKiri.speed(BelakangKiriPID.compute());
00614                 DepanKanan.speed(DepanKananPID.compute());
00615                 DepanKiri.speed(DepanKiriPID.compute());
00616                 while(t_jalan.read_ms()<=20)
00617                 {
00618                 }
00619                 t_jalan.reset();
00620             }
00621             DepanKanan.brake();
00622             BelakangKanan.brake();
00623             BelakangKiri.brake();
00624             DepanKiri.brake();
00625         }
00626         
00627         else if(Jalan == Jalan_Kiri)
00628         {
00629             kosongkan();
00630             while(Jalan== Jalan_Kiri)
00631             {
00632                 t_jalan.reset();
00633                 t_jalan.start();
00634                 proses_kecepatan();
00635                 Get_Count();
00636                 Cari_Koordinat();
00637                 //pc.printf("Koordinat X : %d\n", koordinat_x);
00638                 //pc.printf("Koordinat Y : %d\n", koordinat_y);
00639                 pc.printf("Orientasi : %f\n", orientasi);
00640                 BelakangKanan.speed(BelakangKananPID.compute()*-1);
00641                 BelakangKiri.speed(BelakangKiriPID.compute()*-1);
00642                 DepanKanan.speed(DepanKananPID.compute()*-1);
00643                 DepanKiri.speed(DepanKiriPID.compute()*-1);
00644                 while(t_jalan.read_ms()<=20)
00645                 {
00646                 }
00647                 t_jalan.reset();
00648             }
00649             DepanKanan.brake();
00650             BelakangKanan.brake();
00651             BelakangKiri.brake();
00652             DepanKiri.brake();
00653         }
00654         else if(Jalan == ke_setpoint)
00655         {
00656             DepanKananPID.setSetPoint(1400);
00657             BelakangKananPID.setSetPoint(1400);
00658             BelakangKiriPID.setSetPoint(1400);
00659             DepanKiriPID.setSetPoint(1400);
00660             while(Jalan == ke_setpoint)
00661             {
00662                 ke_setpoint_x();
00663                 ke_setpoint_y();
00664             }
00665         }
00666         else if(Jalan == mode_inverse)
00667         {
00668             kosongkan();
00669             cari_inverse(0, (float)setpoint_x, (float)setpoint_y);
00670             set_inverse();
00671             while((koordinat_x!=setpoint_x)&&(koordinat_y!=setpoint_y))
00672             {
00673                 t_jalan.reset();
00674                 t_jalan.start();
00675                 proses_kecepatan();
00676                 Get_Count();
00677                 Cari_Koordinat();
00678                 BelakangKanan.speed(BelakangKananPID.compute()*arah_belakang_kanan);
00679                 BelakangKiri.speed(BelakangKiriPID.compute()*arah_belakang_kiri);
00680                 DepanKanan.speed(DepanKananPID.compute()*arah_depan_kanan);
00681                 DepanKiri.speed(DepanKiriPID.compute()*arah_depan_kiri);
00682                 pc.printf("Koordinat X : %d\n", koordinat_x);
00683                 pc.printf("Koordinat Y : %d\n", koordinat_y);
00684                 while(t_jalan.read_ms()<=20)
00685                 {
00686                 }
00687                 t_jalan.reset();
00688             }
00689             DepanKanan.brake();
00690             BelakangKanan.brake();
00691             BelakangKiri.brake();
00692             DepanKiri.brake();
00693         }
00694         else if(Jalan == Berhenti)
00695         {
00696             while(Jalan==Berhenti)
00697             {
00698                 t_jalan.reset();
00699                 t_jalan.start();
00700                 pc.printf("Koordinat X : %d\n", koordinat_x);
00701                 pc.printf("Koordinat Y : %d\n", koordinat_y);
00702                 //pc.printf("Orientasi : %f\n", orientasi);
00703                 DepanKanan.brake(0);
00704                 BelakangKanan.brake(0);
00705                 BelakangKiri.brake(0);
00706                 DepanKiri.brake(0);
00707                 while(t_jalan.read_ms()<=20)
00708                 {
00709                 }
00710                 t_jalan.reset();    
00711             }
00712         }
00713     }                       
00714 }