main.cpp

00001 /*********************************************************************************************/
00002 /**     FILE HEADER                                                                         **/
00003 /*********************************************************************************************/
00004 #include "mbed.h"
00005 #include "Motor.h"
00006 #include "NewTextLCD.h"
00007 #include "PS_PAD.h"
00008 #include "PID.h"
00009 #include "millis.h"
00010 #include "esc.h"
00011 #include "Servo.h"
00012 #include "Ping.h"
00013 
00014 
00015 
00016 /*********************************************************************************************/
00017 /**     DEKLARASI INPUT OUTPUT                                                              **/
00018 /*********************************************************************************************/
00019 // serial pc
00020 Serial pc(USBTX,USBRX);
00021 Serial sensor(PA_0,PA_1);
00022 
00023 // joystick PS2
00024 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) default board lama pb_12
00025 
00026 // PID sensor garis 
00027 PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
00028 
00029 Motor gripper(PA_10, PB_3, PB_5); //PB_6, PB_8, PB_9
00030 //Motor slider(PC_6, PC_9, PC_8);
00031 Motor motor2(PA_11, PB_12, PA_7); //kanan
00032 Motor motor1(PA_8, PB_4, PB_1); //kiri
00033 
00034 DigitalOut limit0(PC_0,PullUp);
00035 DigitalOut limit1(PC_1,PullUp);
00036 
00037 DigitalOut pnuematik_lengan(PC_12);
00038 DigitalOut pnuematik_gripper(PC_11);
00039 
00040 ESC edf(PC_6,20); //pwm esc PB_8
00041 Servo myservo(PC_8); //pwm servo PB_9
00042 
00043 Timer timer;
00044 
00045 /*********************************************************************************************/
00046 /**     DEKLARASI VARIABEL GLOBAL                                                           **/
00047 /*********************************************************************************************/
00048 float gMax_speed=0.83; //nilai maksimum kecepatan motor
00049 float gMin_speed=-0.05;  //nilai minimum kecepatan motor
00050 float gTuning = 0.14;
00051 
00052 // tambahan power
00053 // inisialisasi pwm awal servo
00054 float pwm = 0.00;
00055  
00056 // inisialisasi sudut awal
00057 int sudut = 0;
00058 //membatasi nilai brushless pada edf
00059 float min=0;
00060 float max=0.70;
00061 
00062 unsigned char i; // variabel iterasi
00063 int over=0;
00064 int lapangan = 0;
00065 
00066 unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
00067 unsigned char gCase=0;  //variabel keadaan proses
00068 
00069 int c=0;    //motor gripper auto
00070 int g=2;    //pnuematik kondisi gripper
00071 int count=1;
00072 
00073 float k;
00074 float speed;
00075 
00076 int datasensor[6];
00077 
00078 /*********************************************************************************************/
00079 /**     DEKLARASI PROSEDUR DAN FUNGSI                                                        **/
00080 /*********************************************************************************************/     
00081 
00082 /*********************************************************************************************/
00083 void init_servo(int i){
00084     if (i){
00085         if (sudut>60){
00086             sudut=60;
00087             }
00088         if (sudut<-60){
00089             sudut=-60;
00090             }
00091     } else {
00092             
00093         if (sudut>60){
00094             sudut=60;
00095         }
00096         if (sudut<-60){
00097             sudut=-60;
00098          }    
00099     }
00100 }
00101 
00102 void init_pwm (){
00103     if (pwm>max){
00104         pwm = max;
00105     }
00106     
00107     if (pwm<min){
00108         pwm = min;
00109     }
00110 }
00111 
00112 
00113 void motor_base(void){
00114     if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
00115             speed = gMax_speed;                     
00116             motor1.brake(0.2);
00117             motor2.speed(speed-0.05);            
00118             pc.printf("maju serong kiri\n");
00119             
00120         }
00121         else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
00122             speed = gMax_speed;                     
00123             motor1.speed(speed-gTuning-0.05);
00124             motor2.brake(0.2);
00125             pc.printf("maju serong kanan\n");
00126             
00127         }
00128         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
00129             speed = gMax_speed;                     
00130             motor2.brake(1);
00131             motor1.speed(-(speed-gTuning-0.2));
00132             pc.printf("mundur serong kanan\n");
00133             
00134         }
00135         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
00136             speed = gMax_speed;                     
00137             motor2.speed(-(speed-0.2));
00138             motor1.brake(1);
00139             pc.printf("mundur serong kiri\n");
00140             
00141         }
00142        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
00143             //MOTOR DEPAN
00144             
00145             speed = k;
00146 
00147             if (k >= gMax_speed){
00148                 k = gMax_speed;
00149             }
00150             
00151             motor1.speed(speed-gTuning);
00152             motor2.speed(speed);
00153             pc.printf("maju \n");
00154             
00155             k += 0.1;
00156         }
00157         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
00158             //MOTOR BELAKANG
00159             speed = k;
00160 
00161             if (k >= gMax_speed){
00162                 k = gMax_speed;
00163             }
00164                                 
00165             motor1.speed(-speed);
00166             motor2.speed(-speed);
00167             pc.printf("mundur \n");
00168             
00169             k += 0.1;
00170         }
00171         else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
00172             //pivot kiri
00173             speed = gMax_speed;                        
00174             motor1.speed(-(speed*0.95-gTuning));
00175             motor2.speed(speed*0.95);
00176             pc.printf("pivot kiri \n");
00177         }
00178         else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
00179             //pivot kanan
00180             speed = gMax_speed;                        
00181             motor1.speed(speed*0.95-gTuning);
00182             motor2.speed(-speed*0.95    );
00183             pc.printf("pivot kanan \n");
00184           
00185         }
00186         else{
00187             motor1.brake(1);
00188             motor2.brake(1);
00189             
00190             k = 0.6;
00191         }
00192 }
00193 
00194 void motor_gripper(){
00195         if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
00196             //POWER WINDOW ATAS
00197         /*  do{
00198                 motor_base();
00199                 gripper.speed(1);
00200             }while((limit0 != 0) && (c > 7));
00201           */
00202             gripper.speed(0.9);
00203             
00204             if (limit0 == 0){
00205                 gripper.brake(1);
00206             }
00207             
00208             
00209             pc.printf("up \n");
00210             c++;
00211         }
00212         else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
00213             //POWER WINDOW BAWAH 
00214            /*do{
00215                 motor_base();
00216                 gripper.speed(-0.8);
00217             }while((limit1 != 0) && (c > 7));
00218             */
00219                        
00220             gripper.speed(-0.7);
00221             
00222             if (limit1 ==0){
00223                 gripper.brake(1);
00224             }
00225             
00226             pc.printf("down \n");
00227             c--;        
00228         }
00229         else{
00230             gripper.brake(1);
00231             if ((c <= 7) && (c>=-7)){
00232                 c=0;
00233             }
00234             
00235             pc.printf("diam \n");
00236         }           
00237         if((c > 7) && (limit0 == 0)){
00238             c = 0;
00239             gripper.brake(1);
00240         }
00241         else if((c < -7) && (limit1 == 0)){
00242             c = 0;
00243             gripper.brake(1);
00244         }
00245         else if( (c > 7) && (limit0 != 0)){
00246             gripper.speed(1);
00247         }
00248         else if ((c<-7) && (limit1 != 0)){
00249             gripper.speed(-0.9);
00250         }
00251 }
00252 
00253 void servo_edf(){
00254     if(ps2.read(PS_PAD::PAD_X)==1){
00255             //PWM ++
00256             pwm += 0.01;
00257             pc.printf("gaspol \n");
00258         }
00259         else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
00260             //PWM--
00261             pwm -= 0.01;
00262             pc.printf("rem ndeng \n");
00263         }
00264         
00265         if(ps2.read(PS_PAD::PAD_L2)==1){
00266             //SERVO --
00267             sudut += 3;
00268             pc.printf("servo min \n");
00269         }
00270         else if(ps2.read(PS_PAD::PAD_R2)==1){
00271             //SERVO ++
00272             sudut -= 3;
00273             pc.printf("servo max \n");
00274         }
00275         
00276         if(ps2.read(PS_PAD::PAD_START)==1){
00277             
00278             sudut = -60;
00279             pwm = 0.18;
00280         }
00281         
00282         init_servo(lapangan);
00283         init_pwm();
00284         edf.setThrottle(pwm);
00285         edf.pulse();
00286         wait_ms(25);
00287         myservo.position(sudut);
00288 }
00289 
00290 void pnuematik_running(){
00291     if ((ps2.read(PS_PAD::PAD_SELECT)==1))
00292         {
00293             //mekanisme ambil gripper
00294             pc.printf("mekanisme gripper");
00295             if (g==1){
00296                 pc.printf("ambil 1");
00297                 pnuematik_lengan = 1;
00298                 pnuematik_gripper = 1;
00299                 g=2;
00300                 wait_ms(400);
00301             }
00302             else
00303             {
00304                 pnuematik_gripper = 0;
00305                 wait_ms(400);
00306                 pnuematik_lengan = 0;
00307                 
00308                 g=1;
00309                 pc.printf("ambil 2");
00310                 wait_ms(400);
00311             }
00312         }
00313 }
00314 
00315 void line_follower(float speed){    
00316     int pv;  
00317     float speedR,speedL;
00318          
00319         //////////////////logic dari PV (present Value)/////////////////////////
00320         if(datasensor[0]){
00321             pv = -5;
00322             over=1;
00323         }
00324         else if(datasensor[5]){
00325             pv = 5;
00326             over=2;
00327         }
00328         else if(datasensor[0] && datasensor[1]){
00329             pv = -4;
00330         }
00331         else if(datasensor[4] && datasensor[5]){
00332             pv = 4;
00333         }
00334         else if(datasensor[1]){
00335             pv = -3;
00336         }
00337         else if(datasensor[4]){
00338             pv = 3;
00339         }
00340         else if(datasensor[1] && datasensor[2]){
00341             pv = -2;
00342         }
00343         else if(datasensor[3] && datasensor[4]){
00344             pv = 2;
00345         }
00346         else if(datasensor[2]){
00347             pv = -1;
00348         }
00349         else if(datasensor[3]){
00350             pv = 1;
00351         }
00352         else if(datasensor[2] && datasensor[3]){
00353             pv = 0;
00354         }
00355         else
00356         {
00357             ///////////////// robot bergerak keluar dari sensor/////////////////////
00358             if(over==1){
00359                 /*if(speed_ka > 0){
00360                     wait_ms(10);
00361                     motor2.speed(-speed_ka);
00362                     wait_ms(10);
00363                     }
00364                 else{
00365                     motor2.speed(speed_ka);
00366                     }
00367                 wait_ms(10);*/
00368                 
00369                 motor1.brake(1);
00370                 //wait_ms(100);
00371                 
00372             }
00373             else if(over==2){
00374                 /*if(speed_ki > 0){
00375                     wait_ms(10);
00376                     motor1.speed(-speed_ki);
00377                     wait_ms(10);
00378                     }
00379                 else{
00380                     wait_ms(10);
00381                     motor1.speed(speed_ki);
00382                     wait_ms(10);
00383                     }
00384                 wait_ms(10);*/
00385                 motor2.brake(1);
00386                 //wait_ms(100);
00387             }
00388         } 
00389         PID.setProcessValue(pv);
00390         PID.setSetPoint(0);
00391         
00392         // memulai perhitungan PID
00393     
00394         speedR = speed - PID.compute();
00395         if(speedR > speed){
00396             speedR = speed;
00397             }
00398         else if(speedR < gMin_speed)
00399             speedR = gMin_speed;
00400         motor2.speed(speedR);
00401     
00402         speedL = speed + PID.compute();
00403         if(speedL > speed)
00404             speedL = speed;
00405         else if(speedL < gMin_speed)
00406             speedL = gMin_speed;
00407         motor1.speed(speedL);
00408 }
00409 
00410 void init_sensor(){
00411     char data;
00412     if(sensor.readable()){  
00413         data = sensor.getc();
00414         
00415         for(int i=0; i<6; i++){
00416            datasensor[i] = (data >> i) & 1;
00417        }
00418     }
00419 }
00420 
00421 /*********************************************************************************************/
00422 /**     PROGRAM UTAMA                                                                       **/
00423 /*********************************************************************************************/
00424 /*********************************************************************************************/ 
00425 int main(void){
00426     //inisialisasi serial
00427     pc.baud(115200);
00428     sensor.baud(115200);
00429     
00430     //inisialisasi joystick
00431     ps2.init();   
00432 
00433     //set inisialisasi servo pada posisi 0 
00434     myservo.position(0);
00435 
00436     //set edf pada posisi bukan kalibrasi, yaitu set edf 0
00437     edf.setThrottle(pwm);
00438     edf.pulse();
00439     
00440     //inisialisasi gripper propeller
00441     pnuematik_lengan = 0;
00442     pnuematik_gripper = 0;
00443     
00444     //inisialisasi PID sensor    
00445     PID.setInputLimits(-15,15);
00446     PID.setOutputLimits(-1.0,1.0);
00447     PID.setMode(1.0);
00448     PID.setBias(0.0);
00449     PID.reset();
00450     
00451     bool manual=true;
00452    
00453    while(manual)
00454     {
00455         ps2.poll();          
00456         
00457         motor_base();
00458         
00459         motor_gripper();
00460              
00461         pnuematik_running();
00462                 
00463         servo_edf();
00464  
00465         if(ps2.read(PS_PAD::ANALOG_RIGHT)==1){
00466             manual = false;
00467         }
00468     }
00469     
00470     timer.start();
00471     while(1)
00472     {
00473         init_sensor();
00474     
00475         //line_follower(0.4);
00476             
00477         for(int i=0; i<6; i++){
00478             pc.printf("%d  ",datasensor[i]);
00479         }
00480         pc.printf("\n");
00481     }
00482         
00483 }