Diff: main.cpp

Revision:

3:43d4cb3ece1b

Parent:

2:df6c49846367

Child:

4:7a7a8aa33fd5

diff -r df6c49846367 -r 43d4cb3ece1b main.cpp
--- a/main.cpp	Fri Apr 22 08:39:32 2016 +0000
+++ b/main.cpp	Tue Apr 26 14:57:35 2016 +0000
@@ -4,7 +4,6 @@
 #include "mbed.h"
 #include "Motor.h"
 #include "PS_PAD.h"
-#include "PID.h"
 #include "esc.h"
 #include "Servo.h"
 
@@ -21,24 +20,12 @@
 //motor (PWM, forward, reverse)
 Motor motor1(PA_8, PB_0, PC_15);
 Motor motor2(PA_11, PA_6, PA_5);
-//Motor gripper(PB_6, PA_7, PB_12);
 Motor motor3(PA_9, PC_2, PC_3);
-Motor motor4(PB_7, PA_14, PA_15);
+Motor motor4(PA_10, PB_5, PB_4);
+Motor motorS(PB_7, PA_14, PA_15);
 Motor motorC1(PB_9, PA_12, PC_5);
 Motor motorC2(PB_8, PB_1, PA_13);
-Motor motorS(PA_10, PB_5, PB_4);
-
-/* coba coba
-//motor (PWM, forward, reverse)
-Motor motor1(PA_8, PB_0, PC_15);
-Motor motorC1(PA_11, PA_6, PA_5);
-//Motor motor3(PB_6, PA_7, PB_12);
-Motor motorC2(PA_9, PC_2, PC_3);
-Motor motor4(PB_7, PA_14, PA_15);
-Motor motor2(PB_9, PA_12, PC_5);
-Motor motor3(PB_8, PB_1, PA_13);
-Motor motorS(PA_10, PB_5, PB_4);
-*/
+//Motor motor4(PB_6, PA_7, PB_12);
 
 //pnuematik
 DigitalOut pnuematik1(PC_11);
@@ -52,8 +39,10 @@
 DigitalIn limit3(PC_1 ,PullUp);
 DigitalIn limit4(PC_0 ,PullUp);
 
-//PID line follower(P, I, D, Time Sampling)
-PID PID(0.92,0.000,0.61,0.001);
+DigitalIn field(PB_10 ,PullUp);
+
+//laser pointer
+DigitalOut laser(PB_3);
 
 //servo(PWM)
 Servo servoEDF(PC_8);
@@ -61,30 +50,30 @@
 //EDF(PWM, timer)
 ESC edf(PC_6,20);
 
-//Timer Pnuematik
-//Timer timer;
+//Timer
+Ticker timer;
 
 
 
 /*********************************************************************************************/
 /**     DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI                                                       **/
 /*********************************************************************************************/ 
-const float gMax_speed = 0.8;
-const float gMin_speed = 0.1;
-/*const float tuning1 = 0.0;
-const float tuning2 = 0.2;
-const float tuning3 = 0.06;
-const float tuning4 = 0.24;*/
-const float v0 = 0.35;
-const float ax = 0.0006;
+const float gMax_speed = 0.85;
+//const float gMin_speed = 0.1;
+const float v0 = 0.4;
+const float ax = 0.0007;
+float vcurr = v0;
 
 const float tuning1 = 0.0;
-const float tuning2 = 0.16;
-const float tuning3 = 0.03;
-const float tuning4 = 0.22;
+const float tuning2 = 0.04;
+const float tuning3 = 0.0;
+const float tuning4 = 0.04;
 
+const float driver0 = 1;
+const float driver1 = 0.85;
+const float driver2 = 0.65;
+const float driver3 = 0.2;
 
-float vcurr = v0;
 
 // inisialisasi pwm awal servo
 double pwm = 0.00;
@@ -102,12 +91,14 @@
 //data sensor garis dan line following
 int datasensor[6];
 int over;
+int g_flag;
 
 ///////
-void aktuator();
+void aktuator(int f);
 void edf_servo();
 void init_sensor();
-void line(float speed);
+void linetracer(float speed);
+void flag_sensor();
 
 /*********************************************************************************************/
 /*********************************************************************************************/
@@ -119,6 +110,9 @@
     pc.baud(115200);
     com.baud(115200);
     
+    if(field == 1)      sudut = -85;
+    else        sudut = 85;
+    
     //inisiasi joystick
     ps2.init();
     
@@ -129,67 +123,59 @@
     edf.setThrottle(0);
     edf.pulse();
     
+    //inisisasi laser
+    laser = 1;
+    
     //inisiasi pnuematik
     pnuematik1 = 1;
     pnuematik2 = 1;
-    pnuematik3 = 1;
-    pnuematik4 = 1;
-    
-    //inisiasi PID sensor    
-    PID.setInputLimits(-15,15);
-    PID.setOutputLimits(-1.0,1.0);
-    PID.setMode(1.0);
-    PID.setBias(0.0);
-    PID.reset();
+    pnuematik3 = 0;
+    pnuematik4 = 0;
     
     //inisisasi TIMER
-    //timer.start();
+    timer.attach(&flag_sensor,0.0005);
+    timer.detach();
     
     //kondisi robot
     bool manual=true; 
-    /*
-    while(1)
-    {
-        ps2.poll();
-        init_sensor();
-        
-        if(ps2.read(PS_PAD::PAD_X)==1){    
-            line(0.3);
-        }
-        else{
-            motor1.brake(1);
-            motor2.brake(1);
-            motor3.brake(1);
-            motor4.brake(1);
-            
-            //v =0.1;
-        }
-            for(int i=0; i<6; i++){
-                pc.printf("%d  ",datasensor[i]);
-            }
-            pc.printf("\n");
-
-    }*/
-    
+    bool pool= false;
+ 
     while(manual){
         
         ps2.poll();
         
-        aktuator();
+        aktuator(field);
         edf_servo();
         
-        if(limit3==0)    manual = false;
+        if(ps2.read(PS_PAD::ANALOG_LEFT)==1)    manual = false;
         
     }
     
+    timer.attach(&flag_sensor,0.0005);
+    
+    while(!pool){
+        init_sensor();
+        
+        if(vcurr > 0.3)  vcurr = (float) 0.3;
+      
+        linetracer(vcurr);
+        //laser = 1;
+            
+        vcurr+=ax;
+        
+        if(limit3==0)   pool=true;
+        
+    }
     motor1.brake(1);
     motor2.brake(1);
     motor3.brake(1);
     motor4.brake(1);
     
+    timer.detach();
+        
     pnuematik1=0;
     wait_ms(1500);
-    
+
     while(limit4!=0){
         motorC1.speed(1);
         motorC2.speed(-1);
@@ -198,11 +184,20 @@
     motorC1.brake(1);
     motorC2.brake(1);
     
-    pnuematik3 = 0;
-    wait_ms(1500);
-    pnuematik2 = 1;
-    wait_ms(500);
-    pnuematik3 = 1;
+    if(field==1){
+        pnuematik3 = 0;
+        wait_ms(1500);
+        pnuematik2 = 1;
+        wait_ms(500);
+        pnuematik3 = 1;
+    }
+    else{
+        pnuematik4 = 0;
+        wait_ms(1500);
+        pnuematik2 = 1;
+        wait_ms(500);
+        pnuematik4 = 1;
+    }
     
     return 0;
 }
@@ -210,27 +205,28 @@
 /*********************************************************************************************/
 /**     ALGORITMA PROSEDUR DAN FUNGSI                                                       **/
 /*********************************************************************************************/ 
-void aktuator(){
+void aktuator(int f){
     float speed = vcurr;
     
     if(vcurr >= gMax_speed)    vcurr = gMax_speed;
     
+    if(f == 1){
         if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
             //pivot kiri                      
-            motor2.speed((float)0.2*(speed-tuning2));
-            motor4.speed((float)-0.2*(speed-tuning4));
-            motor1.speed((float)0.2*(speed-tuning1));
-            motor3.speed((float)-0.2*(speed-tuning3));
+            motor2.speed((float)0.5*(speed-tuning2));
+            motor4.speed((float)-0.5*(speed-tuning4));
+            motor1.speed((float)0.5*(speed-tuning1));
+            motor3.speed((float)-0.5*(speed-tuning3));
             pc.printf("pivot kiri \n");
             
             vcurr += ax;
         }
         else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
             //pivot kanan                      
-            motor2.speed((float)-0.2*(speed-tuning2));
-            motor4.speed((float)0.2*(speed-tuning4));
-            motor1.speed((float)-0.2*(speed-tuning1));
-            motor3.speed((float)0.2*(speed-tuning3));
+            motor2.speed((float)-0.5*(speed-tuning2));
+            motor4.speed((float)0.5*(speed-tuning4));
+            motor1.speed((float)-0.5*(speed-tuning1));
+            motor3.speed((float)0.5*(speed-tuning3));
             pc.printf("pivot kanan \n");
             
             vcurr += ax;
@@ -324,56 +320,169 @@
             
             vcurr = v0;
         }
-        
-        if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
-            //POWER WINDOW ATAS
-            motorS.speed(1);
+    }
+    else{
+        if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
+            //pivot kiri                      
+            motor2.speed((float)0.5*(speed-tuning2));
+            motor4.speed((float)-0.5*(speed-tuning4));
+            motor1.speed((float)0.5*(speed-tuning1));
+            motor3.speed((float)-0.5*(speed-tuning3));
+            pc.printf("pivot kiri \n");
+            
+            vcurr += ax;
+        }
+        else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
+            //pivot kanan                      
+            motor2.speed((float)-0.5*(speed-tuning2));
+            motor4.speed((float)0.5*(speed-tuning4));
+            motor1.speed((float)-0.5*(speed-tuning1));
+            motor3.speed((float)0.5*(speed-tuning3));
+            pc.printf("pivot kanan \n");
             
-            if (limit1 == 0){
-                motorS.brake(1);
-            }
+            vcurr += ax;
+        }
+        else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
+            //serong atas kanan                                  
+            motor2.speed(-(speed-tuning2));
+            motor4.brake(1);
+            motor1.brake(1);
+            motor3.speed(-(speed-tuning3));
+            pc.printf("serong atas kanan \n");
             
+            vcurr += ax;
+        }
+        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
+            //serong atas kiri                                   
+            motor2.brake(1);
+            motor4.speed(speed-tuning4);
+            motor1.speed(speed-tuning1);
+            motor3.brake(1);
+            pc.printf("serong atas kiri \n");
+            
+            vcurr += ax;
+        }
+        else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
+            //serong bawah kiri 
+            motor2.speed(speed-tuning2);
+            motor4.brake(1);
+            motor1.brake(1);
+            motor3.speed(speed-tuning3);                    
+            pc.printf("serong bawah kiri \n");
             
-            pc.printf("up \n");
-            c++;
+            vcurr += ax;
         }
-        else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
-            //POWER WINDOW BAWAH        
-            motorS.speed(-0.5);
+        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
+            //serong bawah kanan                     
+            motor2.brake(1);
+            motor4.speed(-(speed-tuning4));
+            motor1.speed(-(speed-tuning1));
+            motor3.brake(1);
+            pc.printf("serong bawah kanan \n");
+            
+            vcurr += ax;
+        }
+        else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
+            //maju
+            motor1.speed(speed-tuning1);
+            motor3.speed(-(speed-tuning3));
+            motor2.speed(-(speed-tuning2));
+            motor4.speed(speed-tuning4);
+            pc.printf("maju \n");
             
-            if (limit2 ==0){
-                motorS.brake(1);
-            }
+            vcurr += ax;
+        }
+        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
+            //mundur     
+            motor1.speed(-(speed-tuning1));
+            motor3.speed(speed-tuning3);
+            motor2.speed(speed-tuning2);
+            motor4.speed(-(speed-tuning4));
+            pc.printf("mundur \n");
             
-            pc.printf("down \n");
-            c--;        
+            vcurr += ax;
+        } 
+        else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
+            //kanan                     
+            motor2.speed(-(speed-tuning2));
+            motor4.speed(-(speed-tuning4));
+            motor1.speed(-(speed-tuning1));
+            motor3.speed(-(speed-tuning3));
+            pc.printf("kanan \n");
+            
+            vcurr += ax;
+        }
+        else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
+            //kiri                                 
+            motor2.speed(speed-tuning2);
+            motor4.speed(speed-tuning4);
+            motor1.speed(speed-tuning1);
+            motor3.speed(speed-tuning3);
+            pc.printf("kiri \n");
+            
+            vcurr += ax;
         }
         else{
-            motorS.brake(1);
-            if ((c <= batas_delay) && (c>=-batas_delay)){
-                c=0;
-            }
+            motor1.brake(1);
+            motor3.brake(1);
+            motor2.brake(1);
+            motor4.brake(1);
+            pc.printf("diam \n");
             
-            pc.printf("diam \n");
-        }           
-        if((c > batas_delay) && (limit1 == 0)){
-            c = 0;
+            vcurr = v0;
+        }
+    
+    }    
+        
+    if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
+            //POWER WINDOW ATAS
+        motorS.speed(1);
+        if (limit1 == 0){
+            motorS.brake(1);
+        }
+            
+            
+        pc.printf("up \n");
+        c++;
+    }
+    else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
+            //POWER WINDOW BAWAH        
+        motorS.speed(-0.5);
+            
+        if (limit2 ==0){
             motorS.brake(1);
         }
-        else if((c < -batas_delay) && (limit2 == 0)){
-            c = 0;
-            motorS.brake(1);
+            
+        pc.printf("down \n");
+        c--;        
+    }
+    else{
+        motorS.brake(1);
+        if ((c <= batas_delay) && (c>=-batas_delay)){
+            c=0;
         }
-        else if( (c > batas_delay) && (limit1 != 0)){
-            motorS.speed(1);
-        }
-        else if ((c<-batas_delay) && (limit2 != 0)){
-            motorS.speed(-0.7);
-        }
+            
+        pc.printf("diam \n");
+    }           
+        
+    if((c > batas_delay) && (limit1 == 0)){
+        c = 0;
+        motorS.brake(1);
+    }
+    else if((c < -batas_delay) && (limit2 == 0)){
+        c = 0;
+        motorS.brake(1);
+    }
+    else if( (c > batas_delay) && (limit1 != 0)){
+        motorS.speed(1);
+    }
+    else if ((c<-batas_delay) && (limit2 != 0)){
+        motorS.speed(-0.7);
+    }
         
         
-        if ((ps2.read(PS_PAD::PAD_SELECT)==1))
-        {
+    if ((ps2.read(PS_PAD::PAD_SELECT)==1))
+    {
             //mekanisme ambil gripper
             pc.printf("mekanisme gripper");
             if (g==1){
@@ -388,7 +497,7 @@
                 wait_ms(400);
                 g=1;
             }
-        }
+    }
 }
 
 void edf_servo(){
@@ -424,14 +533,20 @@
         if(ps2.read(PS_PAD::PAD_START)==1){
             
             sudut = 0;
-            pwm = 0.25;
+            pwm = 0.22;
+            
+            pnuematik3 = 1;
+            pnuematik4 = 1;
         }
-    
+        
+        
         servoEDF.position((float)sudut);
         edf.setThrottle((float)pwm);
         edf.pulse();
 }
 
+
+/////////////////LINE FOLLOWER/////////////////////////
 void init_sensor(){
     char data;
     if(com.readable()){  
@@ -443,108 +558,112 @@
     }
 }
 
-void line(float speed){    
-    int pv;  
+void linetracer(float speed){    
     float speed1,speed2,speed3,speed4;
-         
+    
         //////////////////logic dari PV (present Value)/////////////////////////
-        if(datasensor[0]){
-            pv = -5;
-            over=1;
+        if(datasensor[2] && datasensor[3]){
+            speed1 = speed*driver0;
+            speed2 = speed*driver0;
+            speed3 = speed*driver0;
+            speed4 = speed*driver0;
         }
-        else if(datasensor[5]){
-            pv = 5;
-            over=2;
+        else if(datasensor[2]){
+            speed1 = speed*driver1;
+            speed2 = speed*driver0;
+            speed3 = speed*driver0;
+            speed4 = speed*driver1;
         }
-        else if(datasensor[0] && datasensor[1]){
-            pv = -4;
-        }
-        else if(datasensor[4] && datasensor[5]){
-            pv = 4;
+        else if(datasensor[3]){
+            speed1 = speed*driver0;
+            speed2 = speed*driver1;
+            speed3 = speed*driver1;
+            speed4 = speed*driver0;
         }
         else if(datasensor[1]){
-            pv = -3;
+            speed1 = speed*driver2;
+            speed2 = speed*driver0;
+            speed3 = speed*driver0;
+            speed4 = speed*driver2;
         }
         else if(datasensor[4]){
-            pv = 3;
-        }
-        else if(datasensor[1] && datasensor[2]){
-            pv = -2;
-        }
-        else if(datasensor[3] && datasensor[4]){
-            pv = 2;
+            speed1 = speed*driver0;
+            speed2 = speed*driver2;
+            speed3 = speed*driver2;
+            speed4 = speed*driver0;
         }
-        else if(datasensor[2]){
-            pv = -1;
+        else if(datasensor[0]){
+            speed1 = speed*driver3;
+            speed2 = speed*driver0;
+            speed3 = speed*driver0;
+            speed4 = speed*driver3;
         }
-        else if(datasensor[3]){
-            pv = 1;
+        else if(datasensor[5]){
+            speed1 = speed*driver0;
+            speed2 = speed*driver3;
+            speed3 = speed*driver3;
+            speed4 = speed*driver0;
         }
-        else if(datasensor[2] && datasensor[3]){
-            pv = 0;
-        }
+        
         else
         {
-            ///////////////// robot bergerak keluar dari sensor/////////////////////
-            if(over==1){
-                /*if(speed_ka > 0){
-                    wait_ms(10);
-                    motor2.speed(-speed_ka);
-                    wait_ms(10);
-                    }
-                else{
-                    motor2.speed(speed_ka);
-                    }
-                wait_ms(10);*/
-                
-                motor1.brake(1);
-                motor4.brake(1);
-                //wait_ms(100);
-                
+            if(g_flag == 0){
+                speed1 = speed*driver0;
+                speed2 = speed*driver0;
+                speed3 = speed*driver0;
+                speed4 = speed*driver0;
+            }
+            else if(g_flag == 3){
+                speed1 = speed*driver1;
+                speed2 = speed*driver0;
+                speed3 = speed*driver0;
+                speed4 = speed*driver1;
+            }
+            else if(g_flag == 4){
+                speed1 = speed*driver0;
+                speed2 = speed*driver1;
+                speed3 = speed*driver1;
+                speed4 = speed*driver0;
             }
-            else if(over==2){
-                /*if(speed_ki > 0){
-                    wait_ms(10);
-                    motor1.speed(-speed_ki);
-                    wait_ms(10);
-                    }
-                else{
-                    wait_ms(10);
-                    motor1.speed(speed_ki);
-                    wait_ms(10);
-                    }
-                wait_ms(10);*/
-                motor3.brake(1);
-                motor2.brake(1);
-                //wait_ms(100);
+            else if(g_flag == 2){
+                speed1 = speed*driver2;
+                speed2 = speed*driver0;
+                speed3 = speed*driver0;
+                speed4 = speed*driver2;
+            }
+            else if(g_flag == 5){
+                speed1 = speed*driver0;
+                speed2 = speed*driver2;
+                speed3 = speed*driver2;
+                speed4 = speed*driver0;
+            }
+            else if(g_flag == 1){
+                speed1 = -speed*driver2;
+                speed2 = speed*driver2;
+                speed3 = speed*driver2;
+                speed4 = -speed*driver2;
+            }
+            else if(g_flag == 6){
+                speed1 = speed*driver2;
+                speed2 = -speed*driver2;
+                speed3 = -speed*driver2;
+                speed4 = speed*driver2;    
             }
         } 
-        PID.setProcessValue(pv);
-        PID.setSetPoint(0);
         
-        // memulai perhitungan PID
-        
-         
-        speed2 = speed - 0.0 + PID.compute();
-        speed1 = speed - 0.0 + PID.compute();
-        speed3 = speed - 0.2 - PID.compute();
-        speed4 = speed - 0.3 - PID.compute();
-        
-        if(speed1 > speed)      speed1 = speed;
-        else if(speed1 < gMin_speed)    speed1 = gMin_speed;
-        
-        if(speed2 > speed)     speed2 = speed;
-        else if(speed2 < gMin_speed)    speed2 = gMin_speed;
-        
-        if(speed3 > speed)      speed3 = speed;
-        else if(speed3 < gMin_speed)    speed3 = gMin_speed;
-        
-        if(speed4 > speed)      speed4 = speed;
-        else if(speed4 < gMin_speed)    speed4 = gMin_speed;
-        
-        motor3.speed(-(speed3));
-        motor2.speed(-(speed2));
-        
-        motor1.speed(-(speed1));
-        motor4.speed(-(speed4));
+        motor1.speed((float)-(speed1-0.0));
+        motor2.speed((float)-(speed2-0.04));
+        motor3.speed((float)-(speed3-0.0));
+        motor4.speed((float)-(speed4-0.0));
+
+}
+
+void flag_sensor(){
+    if((datasensor[2] == 1) && (datasensor[3] == 1))    g_flag = 0;
+    else if(datasensor[2] == 1)                         g_flag = 3;
+    else if(datasensor[3] == 1)                         g_flag = 4;
+    else if(datasensor[1] == 1)                         g_flag = 2;
+    else if(datasensor[4] == 1)                         g_flag = 5;
+    else if(datasensor[0] == 1)                         g_flag = 1;
+    else if(datasensor[5] == 1)                         g_flag = 6;
 }
\ No newline at end of file