main.cpp

00001 /*********************************************************************************************/
00002 /** GARUDAGO-ITB (KRAI2016)                                                                 **/
00003 /** #ROADTOBANGKOK!                                                                         **/
00004 /**                                                                                         **/
00005 /** MAIN PROGRAM ROBOT HYBRID SEMI OTOMATIS                                                 **/
00006 /**                                                                                         **/
00007 /**                                                                                         **/
00008 /** Created by :                                                                            **/
00009 /** Rizqi Cahyo Yuwono                                                                      **/
00010 /** EL'14 - 13214090                                                                        **/
00011 /**                                                                                         **/
00012 /** Last Update : 19 Desember 2015, 06.10 WIB                                               **/
00013 /*********************************************************************************************/
00014 
00015 /*********************************************************************************************/
00016 /**     FILE HEADER                                                                         **/
00017 /*********************************************************************************************/
00018 #include "mbed.h"
00019 #include "Motor.h"
00020 #include "NewTextLCD.h"
00021 #include "PS3_USB.h"
00022 #include "PID.h"
00023 
00024 
00025 /*********************************************************************************************/
00026 /**     DEKLARASI INPUT OUTPUT                                                              **/
00027 /*********************************************************************************************/
00028 // serial pc
00029 Serial pc(USBTX,USBRX);
00030 
00031 // LCD 20x4
00032 TextLCD lcd(PB_13, PB_14, PB_15, PB_1, PB_2, PB_5, TextLCD::LCD20x4); //rs,e,d4-d7
00033 
00034 // joystick PS3
00035 PS3_USB PS3 (PA_11,PA_12); //(rx,tx)
00036 
00037 // PID sensor garis 
00038 PID PID(0.532,0.000,0.41,0.001); //(P,I,D, time sampling)
00039 
00040 // Motor
00041 Motor motor_kiri (PA_15, PA_14, PA_13); //motor1 (pwm, fwd, rev)
00042 Motor motor_kanan (PA_1, PC_14 ,PC_15); //motor2 (pwm, fwd, rev)
00043 
00044 // Selektor dan Sensor
00045 BusOut selektor(PB_0,PA_9,PC_2); //(selektor A, selektor B, selektor C)
00046 AnalogIn sensorR (PC_0); //ADC1    
00047 AnalogIn sensorL(PC_3); //ADC2
00048 
00049 // Multitasker
00050 Ticker timer;
00051 
00052 
00053 /*********************************************************************************************/
00054 /**     DEKLARASI VARIABEL GLOBAL                                                           **/
00055 /*********************************************************************************************/
00056 float gLimit=0.4; //nilai batas sensor agar dapat membedakan warna putih dengan background
00057 float gMax_speed=0.5; //nilai maksimum kecepatan motor
00058 float gMin_speed=-0.05;  //nilai minimum kecepatan motor
00059 
00060 unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
00061 unsigned char gCase=0;  //variabel keadaan proses
00062 
00063 unsigned char logic_sensor[16]; //variabel logic dari sensor garis
00064 unsigned char over=0;
00065 
00066 unsigned char i; // variabel iterasi
00067 
00068 
00069   
00070 /*********************************************************************************************/
00071 /**     DEKLARASI PROSEDUR DAN FUNGSI                                                        **/
00072 /*********************************************************************************************/      
00073 float adcSensor(int i)  //pembacaan nilai ADC dari sensor garis
00074 {
00075     if (i < 8)
00076     {
00077         selektor = 7-i;
00078         return sensorL.read();
00079     }
00080     else
00081     {
00082         selektor = 15-i;
00083         return sensorR.read();
00084     }
00085 }
00086 
00087 void PIDrunning()   //menjalankan perintah untuk line follower
00088 {  
00089     
00090     int pv;
00091     float speedR,speedL;
00092     
00093     //menentukan logic sensor
00094     for(i=1;i<16;i++){
00095         if(adcSensor(i) <= gLimit)
00096             logic_sensor[i]=1;
00097         else
00098             logic_sensor[i]=0;
00099     }
00100     
00101     //////////////////logic dari PV (present Value)/////////////////////////
00102     if(logic_sensor[0]==1){
00103         pv = -15;
00104         over=1;
00105     }
00106     else if(logic_sensor[15]==1){
00107         pv = 15;
00108         over=2;
00109     }
00110     else if(logic_sensor[1]==1 && logic_sensor[0]==1){
00111         pv = -14;
00112     }
00113     else if(logic_sensor[14]==1 && logic_sensor[15]==1){
00114         pv = 14;
00115     }
00116     else if(logic_sensor[1]==1 ){
00117         pv = -13;
00118     }
00119     else if(logic_sensor[14]==1 ){
00120         pv = 13;
00121     }
00122     else if(logic_sensor[2]==1 && logic_sensor[1]==1){
00123         pv = -12;
00124     }
00125     else if(logic_sensor[13]==1 && logic_sensor[14]==1){
00126         pv = 12;
00127     }
00128     else if(logic_sensor[2]==1 ){
00129         pv = -11;
00130     }
00131     else if(logic_sensor[13]==1 ){
00132         pv = 11;
00133     }
00134     else if(logic_sensor[3]==1 && logic_sensor[2]==1){
00135         pv = -10;
00136     }
00137     else if(logic_sensor[12]==1 && logic_sensor[13]==1){
00138         pv = 10;
00139     }
00140     else if(logic_sensor[3]==1 ){
00141         pv = -9;
00142     }
00143     else if(logic_sensor[12]==1 ){
00144         pv = 9;
00145     }
00146     else if(logic_sensor[4]==1 && logic_sensor[3]==1){
00147         pv = -8;
00148     }
00149     else if(logic_sensor[11]==1 && logic_sensor[12]==1){
00150         pv = 8;
00151     }
00152     else if(logic_sensor[4]==1 ){
00153         pv = -7;
00154     }
00155     else if(logic_sensor[11]==1 ){
00156         pv = 7;
00157     }
00158     else if(logic_sensor[5]==1 && logic_sensor[4]==1){
00159         pv = -6;
00160     }
00161     else if(logic_sensor[10]==1 && logic_sensor[11]==1){
00162         pv = 6;
00163     }
00164     else if(logic_sensor[5]==1 ){
00165         pv = -5;
00166     }
00167     else if(logic_sensor[10]==1 ){
00168         pv = 5;  
00169     }
00170     else if(logic_sensor[6]==1 && logic_sensor[5]==1){
00171         pv = -4;
00172     }
00173     else if(logic_sensor[9]==1 && logic_sensor[10]==1){
00174         pv = 4;
00175     }
00176     else if(logic_sensor[6]==1 ){
00177         pv = -3;
00178     }
00179     else if(logic_sensor[9]==1 ){
00180         pv = 3;
00181     }
00182     else if(logic_sensor[7]==1 && logic_sensor[6]==1){
00183         pv = -2;
00184     }  
00185      else if(logic_sensor[8]==1 && logic_sensor[9]==1){
00186         pv = 2;
00187     }
00188     else if (logic_sensor[7]==1 && logic_sensor[8]==1){
00189         pv = 0;
00190     }
00191     else if(logic_sensor[7]==1 ){
00192         pv = -1;    
00193     }
00194     else if(logic_sensor[8]==1 ){
00195         pv = 1;
00196     }  
00197     ///////////////// robot bergerak keluar dari sensor/////////////////////
00198         if(over=1){
00199             /*if(speed_ka > 0){
00200                 wait_ms(10);
00201                 motor_kanan.speed(-speed_ka);
00202                 wait_ms(10);
00203                 }
00204             else{
00205                 motor_kanan.speed(speed_ka);
00206                 }
00207             wait_ms(10);*/
00208             
00209             motor_kiri.brake(1);
00210             //wait_ms(100);
00211             
00212         }
00213         else if(over==2){
00214             /*if(speed_ki > 0){
00215                 wait_ms(10);
00216                 motor_kiri.speed(-speed_ki);
00217                 wait_ms(10);
00218                 }
00219             else{
00220                 wait_ms(10);
00221                 motor_kiri.speed(speed_ki);
00222                 wait_ms(10);
00223                 }
00224             wait_ms(10);*/
00225             motor_kanan.brake(1);
00226             //wait_ms(100);
00227         }
00228     
00229     PID.setProcessValue(pv);
00230     PID.setSetPoint(0);
00231     
00232     // memulai perhitungan PID
00233 
00234     speedR = gMax_speed + PID.compute();
00235     if(speedR > gMax_speed){
00236         speedR = gMax_speed;
00237         }
00238     else if(speedR < gMin_speed)
00239         speedR = gMin_speed;
00240     motor_kanan.speed(speedR);
00241 
00242     speedL = gMax_speed - PID.compute();
00243     if(speedL > gMax_speed)
00244         speedL = gMax_speed;
00245     else if(speedL < gMin_speed)
00246         speedL = gMin_speed;
00247     motor_kiri.speed(speedL);
00248     
00249     over=0;
00250 }
00251 
00252 void showLCD()  //menampilkan user interface pada LCD
00253 {   
00254     lcd.cls();
00255     lcd.locate(5,0);
00256     lcd.printf("GarudaGo !!");
00257     
00258     switch(gCase)
00259     {
00260         case 0 :
00261         {
00262             lcd.locate(1,1);
00263             lcd.printf("Kalibrasi = %.4f",gLimit);
00264             break;
00265         }
00266         case 1 :
00267         {   
00268             if (gMode == 1)
00269             {
00270                 lcd.locate(3,1);
00271                 lcd.printf("Mode = Otomatis");
00272             }
00273             else if (gMode==0)
00274             {
00275                 lcd.locate(3,1);
00276                 lcd.printf("Mode = Manual");
00277             }
00278             break;
00279         }
00280     }
00281     
00282     for(i=0;i<16;i++){
00283         if(adcSensor(i) <= gLimit)
00284             logic_sensor[i]=1;
00285         else
00286             logic_sensor[i]=0;    
00287         lcd.locate(i+2,3);
00288         lcd.printf("%d",logic_sensor[i]);
00289     }
00290 }
00291 
00292 void running()  //prosedur utama untuk kendali robot
00293 {
00294     float speed=gMax_speed;
00295     float k=1;
00296     
00297     switch(gCase)
00298     {
00299         case 0 :
00300         {
00301             motor_kiri.brake(0);
00302             motor_kanan.brake(0);
00303             
00304             if(PS3.L2 == 0 && PS3.R2==255 && !PS3.R1 && !PS3.L1)
00305                 gLimit += 0.00001;
00306             else if (PS3.L2 == 255 && PS3.R2==0 && !PS3.R1 && !PS3.L1)
00307                 gLimit -= 0.00001;
00308             else if(PS3.L2 == 0 && PS3.R2==0 && PS3.R1 && !PS3.L1)
00309                 gLimit += 0.0005;
00310             if(PS3.L2 == 0 && PS3.R2==0 && !PS3.R1 && PS3.L1)
00311                 gLimit -= 0.0005;
00312             
00313             if (gLimit > 1.0)
00314                 gLimit = 1.0;
00315             else if(gLimit < 0)
00316                 gLimit = 0.0;
00317             
00318             if(PS3.START && PS3.L2 == 0 && PS3.R2 == 0 && !PS3.R1 && !PS3.L1)
00319                 gCase++;
00320 
00321             break;   
00322         }
00323         case 1 : 
00324         {
00325             if (gMode == 1)
00326             {
00327                 if (PS3.silang)
00328                 {
00329                     PIDrunning();
00330                     pc.printf("PID \t %f \t ",PID.compute());
00331                     
00332                     for(i=0;i<16;i++)
00333                     {
00334                         pc.printf("%d ",logic_sensor[i]);
00335                     }
00336                     pc.printf("\n");
00337                 }
00338                 else if(!PS3.silang)
00339                 {
00340                     motor_kiri.brake(0);
00341                     motor_kanan.brake(0);
00342                     pc.printf("PID stop \n");
00343                 }
00344             }
00345             else if (gMode == 0)
00346             {
00347                 if (PS3.atas && !PS3.bawah && !PS3.L1 && !PS3.R1) //maju
00348                 {
00349                     if (speed < 0.1)
00350                         speed = 0.1;
00351                     else
00352                         speed += 0.005;
00353                     
00354                     if (speed >= gMax_speed)
00355                         speed = gMax_speed;  
00356                     
00357                     motor_kiri.speed(speed*k);
00358                     motor_kanan.speed(speed*k);
00359                     pc.printf("maju \n");
00360                 }
00361                 else if (!PS3.atas && PS3.bawah && !PS3.L1 && !PS3.R1) //mundur
00362                 {
00363                     if (speed < 0.1)
00364                         speed = 0.1;
00365                     else
00366                         speed += 0.005;
00367                     
00368                     if (speed >= gMax_speed)
00369                         speed = gMax_speed;
00370                         
00371                     motor_kiri.speed(-speed*k);
00372                     motor_kanan.speed(-speed*k);
00373                     pc.printf("mundur \n");
00374                 }
00375                 else if (!PS3.atas && !PS3.bawah && PS3.L1 && !PS3.R1) //pivot kiri
00376                 {
00377                     if (speed < 0.1)
00378                         speed = 0.1;
00379                     else
00380                         speed += 0.005;
00381                     
00382                     if (speed >= gMax_speed)
00383                         speed = gMax_speed;
00384                         
00385                     motor_kiri.speed(-speed*k*0.7);
00386                     motor_kanan.speed(speed*k*0.7);
00387                     pc.printf("piv kiri \n");
00388                 }
00389                 else if (!PS3.atas && !PS3.bawah && !PS3.L1 && PS3.R1) //pivot kanan
00390                 {
00391                     if (speed < 0.1)
00392                         speed = 0.1;
00393                     else
00394                         speed += 0.005;
00395                     
00396                     if (speed >= gMax_speed)
00397                         speed = gMax_speed;
00398                         
00399                     motor_kiri.speed(speed*k*0.7);
00400                     motor_kanan.speed(-speed*k*0.7);
00401                     pc.printf("piv kanan \n");
00402                 }
00403                 else if (!PS3.atas && !PS3.bawah && !PS3.L1 && !PS3.R1)
00404                 {
00405                     motor_kiri.brake(0);
00406                     motor_kanan.brake(0);
00407                     pc.printf("stop \n");
00408                 }
00409                 
00410                /*if(PS3.L2 == 255 && PS3.R2 == 0)
00411                     k=0.5;
00412                 else if (PS3.L2 == 0 && PS3.R2 == 255)
00413                     k=2;
00414                 else
00415                     k=1;*/
00416             }
00417             else
00418             {
00419                 motor_kiri.brake(1);
00420                 motor_kanan.brake(1);
00421                 pc.printf("no PID no MANUAL \n");
00422             }
00423             
00424             if(PS3.SELECT && !PS3.START)
00425             {
00426                 if (gMode==1)
00427                     gMode=0;
00428                 else
00429                     gMode=1;
00430             }
00431         
00432             if(PS3.START && !PS3.SELECT)
00433                 gCase--;
00434                 
00435             break;
00436         }   
00437     }
00438 }
00439 
00440 
00441 /*********************************************************************************************/
00442 /*********************************************************************************************/
00443 /**     PROGRAM UTAMA                                                                       **/
00444 /*********************************************************************************************/
00445 /*********************************************************************************************/
00446 int main(void){
00447     //inisialisasi joystick
00448     PS3.setup();
00449     
00450     //inisialisasi PID
00451     PID.setInputLimits(-15,15);
00452     PID.setOutputLimits(-1.0,1.0);
00453     PID.setMode(1.0);
00454     PID.setBias(0.0);
00455     PID.reset();
00456     
00457     // serial PC
00458     pc.baud(115200);
00459     
00460     //multitasking untuk menampilkan layar
00461     timer.attach(&showLCD,1);
00462     
00463     while(1){  
00464         PS3.idle();
00465         
00466         if(PS3.readable()) 
00467         {
00468            // Panggil fungsi pembacaan joystik
00469            PS3.baca_data();
00470            // Panggil fungsi pengolahan data joystik
00471            PS3.olah_data();
00472            //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",PS3.button, PS3.RL, PS3.button_click, PS3.RL_click, PS3.R2, PS3.L2, PS3.RX, PS3.RY, PS3.LX, PS3.LY);
00473            running();  
00474         }
00475         else 
00476         {
00477            PS3.idle();
00478         }
00479     }
00480 }