main.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 device(PB_6, PA_10);
00008 
00009 Motor DepanKanan(PB_9, PC_5, PA_12);
00010 QEI DepanKananQEI(PB_8, PC_9, NC, 624);
00011 PID DepanKananPID(1.2, 0.1, 0.00000001, 0.02);
00012 
00013 char nilai, huruf;
00014 int DepanKananPulses      = 0; //How far the left wheel has travelled.
00015 int DepanKananPrevPulses  = 0; //The previous reading of how far the left wheel
00016     //has travelled.
00017 int DepanKananCount, d_pulse_dk;
00018 float DepanKananVelocity  = 0.0; //The velocity of the left wheel in pulses per
00019 float Koordinat;
00020 int Resolusi = 624;
00021 float pi = 3.14;
00022 float diameter = 5.8;
00023 Timer t_jalan;
00024 
00025 void kosongkan()
00026 {
00027     DepanKananQEI.reset();
00028     DepanKananPulses      = 0;
00029     DepanKananPrevPulses  = 0;
00030     DepanKananVelocity  = 0.0;
00031 }
00032 
00033 void proses_kecepatan()
00034 {
00035     DepanKananPulses = DepanKananQEI.getPulses();
00036     d_pulse_dk = DepanKananPulses - DepanKananPrevPulses;
00037     DepanKananVelocity = (DepanKananPulses - DepanKananPrevPulses) / 0.02;
00038     DepanKananPrevPulses = DepanKananPulses;             
00039     DepanKananPID.setProcessValue(fabs(DepanKananVelocity));
00040 }
00041 
00042 void cari_koordinat()
00043 {
00044     Koordinat = (DepanKananCount/Resolusi)*pi*diameter;
00045 }
00046 
00047 void fungsiBluetooth(void)
00048 {
00049     if(device.readable())
00050     {
00051         nilai = device.getc();
00052         if (nilai == 'S')
00053         {
00054             huruf = 'S';    
00055         }
00056         else if (nilai == 'F')
00057         {
00058             huruf = 'F';
00059         }
00060         else if(nilai =='B')
00061         {
00062             huruf = 'B';
00063         }
00064         else if(nilai =='L')
00065         {
00066             huruf = 'L';
00067         }
00068         else if(nilai =='R')
00069         {
00070             huruf = 'R';
00071         }
00072         else
00073         {
00074             huruf = 'M';
00075         }
00076     }
00077         
00078 }
00079 
00080 void Get_Count()
00081 {
00082     DepanKananCount = DepanKananCount + DepanKananPulses;
00083 }
00084 
00085 int main()
00086 {
00087     pc.baud(38400);
00088     device.baud(9600);
00089     device.attach(&fungsiBluetooth, Serial::RxIrq);
00090     DepanKanan.period(0.01f);    
00091 
00092     DepanKananPID.setInputLimits(0, 2220);//Input  units: counts per second.
00093     DepanKananPID.setOutputLimits(0.0, 0.9);//Output units: PwmOut duty cycle as %.
00094     DepanKananPID.setMode(AUTO_MODE);
00095     
00096     DepanKananPID.setSetPoint(1000);
00097     wait(3);
00098     
00099     while(1)
00100     {
00101         if(huruf == 'F')
00102         {
00103             kosongkan();
00104             while(huruf == 'F')
00105             {
00106                 t_jalan.reset();
00107                 t_jalan.start();
00108                 proses_kecepatan();
00109                 DepanKanan.speed(DepanKananPID.compute());
00110                 while(t_jalan.read_ms()<=20)
00111                 {
00112                 }
00113                 t_jalan.reset();
00114                 DepanKananCount = DepanKananCount + d_pulse_dk;
00115                 cari_koordinat();
00116                 pc.printf("Koordinat = %f\n", Koordinat);
00117             }
00118             DepanKanan.brake();
00119         }
00120         
00121         else if(huruf == 'B')
00122         {
00123             kosongkan();
00124             while(huruf == 'B')
00125             {
00126                 t_jalan.reset();
00127                 t_jalan.start();
00128                 proses_kecepatan();
00129                 DepanKanan.speed(DepanKananPID.compute()*-1);
00130                 while(t_jalan.read_ms()<=20)
00131                 {
00132                 }
00133                 t_jalan.reset();
00134                 DepanKananCount = DepanKananCount + d_pulse_dk;
00135                 cari_koordinat();
00136                 pc.printf("\n\r Koordinat: %f", Koordinat);
00137             }
00138             DepanKanan.brake();
00139         }
00140         
00141         else
00142         {
00143             pc.printf("%c\n", huruf);
00144         }
00145     }
00146 }