Revision 7:59a513681663, committed 2018-02-10

Comitter:

rizqicahyo

Date:

Sat Feb 10 04:24:23 2018 +0000

Parent:

6:7fce519e4976

Commit message:

publish;

Changed in this revision

--- a/Motor.lib	Thu Apr 28 14:59:21 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/aberk/code/Motor/#c75b234558af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motordriver.lib	Sat Feb 10 04:24:23 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/osmeest/code/Motordriver/#83245b45ea70

--- a/main.cpp	Thu Apr 28 14:59:21 2016 +0000
+++ b/main.cpp	Sat Feb 10 04:24:23 2018 +0000
@@ -2,7 +2,7 @@
 /**     FILE HEADER                                                                         **/
 /*********************************************************************************************/
 #include "mbed.h"
-#include "Motor.h"
+#include "motordriver.h"
 #include "PS_PAD.h"
 #include "esc.h"
 #include "Servo.h"
@@ -12,23 +12,24 @@
 /**     DEKLARASI INPUT OUTPUT                                                              **/
 /*********************************************************************************************/
 //serial
-Serial pc(USBTX,USBRX);     //debug
-Serial com(PA_0,PA_1);      //sensor
+Serial pc(USBTX,USBRX);      //debug
+Serial com1(PA_0,PA_1);      //sensor depan
+Serial com2(PC_6, PC_7);     //sensor belakang
 
 //joystick PS2
 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) 
 
 //motor (PWM, forward, reverse)
-Motor motor1(PA_8, PB_0, PC_15);
-Motor motor2(PA_11, PA_6, PA_5);
-Motor motor3(PA_9, PC_2, PC_3);
-Motor motor4(PA_10, PB_5, PB_4);
-Motor motorC2(PB_7, PA_14, PA_15);
-Motor motorC1(PB_9, PA_12, PC_5);
-Motor motorS(PB_8, PB_1, PA_13);
+Motor motor3(PA_8, PB_0, PC_15, 1); //motor1
+Motor motor1(PA_11, PA_6, PA_5, 1); //motor2
+Motor motor4(PA_9, PC_2, PC_3, 1);   //motor_slider
+Motor motor2(PA_10, PB_5, PB_4, 1);  //motor_griper
+Motor motorC1(PB_7, PA_14, PA_15 , 1); //motor4
+Motor motorC2(PB_9, PA_12, PC_5, 1);  //motor6
+Motor motorS(PB_8, PA_13, PB_1, 1);  //motor5
 //Motor motor4(PB_6, PA_7, PB_12);
 
-//pnuematik
+//pnuematik 
 DigitalOut pnuematik1(PC_11);
 DigitalOut pnuematik2(PC_10);
 DigitalOut pnuematik3(PD_2);
@@ -43,16 +44,17 @@
 DigitalIn field(PB_10 ,PullUp);
 
 //laser pointer
-DigitalOut laser(PB_3);
+DigitalIn IR(PB_3 ,PullUp);
 
 //servo(PWM)
 Servo servoEDF(PC_8);
 
 //EDF(PWM, timer)
-ESC edf(PC_6,20);
+ESC edf(PC_9,20);
 
 //Timer
 Ticker timer;
+Timer timer_linetracer;
 
 //Sensor Ping
 Ping ping(PB_2);
@@ -63,19 +65,19 @@
 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 ax = 0.0005;
+float vcurr_base = v0;
+float vcurr_slider = v0;
 
-const float tuning1 = 0.0;
-const float tuning2 = 0.04;
-const float tuning3 = 0.0;
-const float tuning4 = 0.04;
+const float tuning1 = 0.03;
+const float tuning2 = 0.06;
+const float tuning3 = 0.03;
+const float tuning4 = 0.0 ;
 
-const float driver0 = 1;
-const float driver1 = 0.82;
-const float driver2 = 0.6;
-const float driver3 = 0.15;
-
+const float driver1_0 = 1;
+const float driver1_1 = 0.91;
+const float driver1_2 = 0.78;
+const float driver1_3 = 0.55;
 
 // inisialisasi pwm awal servo
 double pwm = 0.00;
@@ -83,17 +85,20 @@
 // inisialisasi sudut awal
 double sudut = -85;
 
-// variabel kondisi pnuematik
-int g = 1;
+// variabel counting
+unsigned long int g = 0;
 
 //slider auto
 int c =0;  
-int batas_delay = 270;
+int batas_delay = 340;
+int flag_river;
 
 //data sensor garis dan line following
-int datasensor[6];
+int datasensor1[6];
+int datasensor2[6];
 int over;
-int g_flag;
+int g_flag1;
+int g_flag2;
 
 
 void aktuator(int f);
@@ -102,6 +107,7 @@
 void init_sensor();
 void linetracer(float speed);
 void flag_sensor();
+void putar(float speed);
 
 float read_jarak();
 
@@ -113,10 +119,14 @@
 int main() {
     //inisiasi serial
     pc.baud(115200);
-    com.baud(115200);
+    com1.baud(115200);
+    com2.baud(115200);
     
     if(field == 1)      sudut = -85;
-    else        sudut = 85;
+    else                sudut = 85;
+    
+    //inisisasi flag river
+    flag_river = 0;
     
     //inisiasi joystick
     ps2.init();
@@ -127,10 +137,7 @@
     //set edf pada posisi bukan kalibrasi, yaitu set edf 0
     edf.setThrottle(0);
     edf.pulse();
-    
-    //inisisasi laser
-    laser = 1;
-    
+     
     //inisiasi pnuematik
     pnuematik1 = 1;
     pnuematik2 = 1;
@@ -138,8 +145,9 @@
     pnuematik4 = 1;
     
     //inisisasi TIMER
-    timer.attach(&flag_sensor,0.0005);
-    timer.detach();
+    //timer.attach(&flag_sensor,0.0005);
+    //timer.detach();
+    timer_linetracer.reset();
     
     //kondisi robot
     bool manual=true; 
@@ -159,58 +167,85 @@
     
     //running otomatis
     timer.attach(&flag_sensor,0.0005);
+    timer_linetracer.start();
+    vcurr_base = v0;
     
     while(!pool){
         init_sensor();
         
-        if(vcurr > 0.4)  vcurr = (float) 0.4;
-        //else if (vcurr < 0.2)   vcurr = (float) 0.2;
-      
-        linetracer(vcurr);
-        //laser = 1;
-            
-        if((limit3==0) || (read_jarak() <= 9.0))   pool=true;
+        if(vcurr_base > (float)1)  vcurr_base = (float) 1;
+        //else if (vcurr_base < 0.2)   vcurr_base = (float) 0.2;
+        
+        linetracer(vcurr_base);            
         
-        //if(read_jarak() <= 45.0)        vcurr -= ax;
-        //else                        vcurr += ax;
-        vcurr += ax;
+
+        if(timer_linetracer.read_ms() >= 2500){
+            vcurr_base = 0.4;
+        }
+        else{
+            vcurr_base += 0.002;
+        }
         
         for(int i=0; i<6; i++){
-            printf("%d  ",datasensor[i]);
+            printf("%d  ",datasensor1[i]);
         }
-        printf("%d  \n",g_flag);
+        for(int i=5; i>=0; i--){
+            printf("%d  ",datasensor2[i]);
+        }
+        printf("%d  %d   %lf \n",g_flag1, g_flag2, read_jarak());
         
+        if((limit3==0) || (read_jarak() <= (float)5.0))   pool=true;   
     }
-    motor1.brake(0);
-    motor2.brake(0);
-    motor3.brake(0);
-    motor4.brake(0);
+    putar(0.2);    
     
     timer.detach();
-        
+    timer_linetracer.stop();
+    
+    motor1.stop(1);
+    motor2.stop(1);
+    motor3.stop(1);
+    motor4.stop(1); 
+    
+    wait_ms(200);
     pnuematik1=0;
-    wait_ms(1500);
+    
+    wait_ms(1000);
     
     while(limit4!=0){
-        motorC1.speed(0.95);
+        motorC1.speed(1);
         motorC2.speed(-1);
+        
+     /*   if(g < 600000){
+            motor1.speed(-(1-tuning1));
+            motor2.speed(-(1-tuning2));
+            motor3.speed(-(1-tuning3));
+            motor4.speed(-(1-tuning4)); 
+            
+            g++;
+        }
+        else{
+            motor1.stop(1);
+            motor2.stop(1);
+            motor3.stop(1);
+            motor4.stop(1);
+        }*/
     }
     
-    motorC1.brake(1);
-    motorC2.brake(1);
+    motorC1.stop(1);
+    motorC2.stop(1);
     
     if(field==1){
         pnuematik3 = 0;
+        wait_ms(1300);
+        pnuematik2 = 0;
         wait_ms(1500);
-        pnuematik2 = 1;
-        wait_ms(500);
         pnuematik3 = 1;
     }
     else{
         pnuematik4 = 0;
+        wait_ms(1300);
+        pnuematik2 = 0;
         wait_ms(1500);
-        pnuematik2 = 1;
-        wait_ms(500);
         pnuematik4 = 1;
     }
     
@@ -221,9 +256,10 @@
 /**     ALGORITMA PROSEDUR DAN FUNGSI                                                       **/
 /*********************************************************************************************/ 
 void aktuator(int f){
-    float speed = vcurr;
+    float speed = vcurr_base;
     
-    if(vcurr >= gMax_speed)    vcurr = gMax_speed;
+    if(vcurr_base >= gMax_speed)    vcurr_base = gMax_speed;
+    if(vcurr_slider >= 1)           vcurr_slider = 1; 
     
     if(f == 1){
         if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
@@ -234,7 +270,7 @@
             motor3.speed((float)-0.5*(speed-tuning3));
             pc.printf("pivot kiri \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
             //pivot kanan                      
@@ -244,47 +280,47 @@
             motor3.speed((float)0.5*(speed-tuning3));
             pc.printf("pivot kanan \n");
             
-            vcurr += ax;
+            vcurr_base += 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);
+            motor4.stop(1);
+            motor1.stop(1);
             motor3.speed(speed-tuning3);
             pc.printf("serong atas kanan \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
             //serong atas kiri                       
-            motor2.brake(1);
+            motor2.stop(1);
             motor4.speed(-(speed-tuning4));
             motor1.speed(-(speed-tuning1));
-            motor3.brake(1);
+            motor3.stop(1);
             pc.printf("serong atas kiri \n");
             
-            vcurr += ax;
+            vcurr_base += 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);
+            motor4.stop(1);
+            motor1.stop(1);
             motor3.speed(-(speed-tuning3));
             pc.printf("serong bawah kiri \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
             //serong bawah kanan                     
-            motor2.brake(1);
+            motor2.stop(1);
             motor4.speed(speed-tuning4);
             motor1.speed(speed-tuning1);
-            motor3.brake(1);
+            motor3.stop(1);
             pc.printf("serong bawah kanan \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
             //maju
@@ -294,7 +330,7 @@
             motor4.speed(-(speed-tuning4));
             pc.printf("maju \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
             //mundur     
@@ -304,7 +340,7 @@
             motor4.speed(speed-tuning4);
             pc.printf("mundur \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         } 
         else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
             //kanan                     
@@ -314,7 +350,7 @@
             motor3.speed(speed-tuning3);
             pc.printf("kanan \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
             //kiri                       
@@ -324,16 +360,16 @@
             motor3.speed(-(speed-tuning3));
             pc.printf("kiri \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else{
-            motor1.brake(1);
-            motor3.brake(1);
-            motor2.brake(1);
-            motor4.brake(1);
+            motor1.stop(1);
+            motor3.stop(1);
+            motor2.stop(1);
+            motor4.stop(1);
             pc.printf("diam \n");
             
-            vcurr = v0;
+            vcurr_base = v0;
         }
     }
     else{
@@ -345,7 +381,7 @@
             motor3.speed((float)-0.5*(speed-tuning3));
             pc.printf("pivot kiri \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
             //pivot kanan                      
@@ -355,47 +391,47 @@
             motor3.speed((float)0.5*(speed-tuning3));
             pc.printf("pivot kanan \n");
             
-            vcurr += ax;
+            vcurr_base += 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);
+            motor4.stop(1);
+            motor1.stop(1);
             motor3.speed(-(speed-tuning3));
             pc.printf("serong atas kanan \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
             //serong atas kiri                                   
-            motor2.brake(1);
+            motor2.stop(1);
             motor4.speed(speed-tuning4);
             motor1.speed(speed-tuning1);
-            motor3.brake(1);
+            motor3.stop(1);
             pc.printf("serong atas kiri \n");
             
-            vcurr += ax;
+            vcurr_base += 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);
+            motor4.stop(1);
+            motor1.stop(1);
             motor3.speed(speed-tuning3);                    
             pc.printf("serong bawah kiri \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
             //serong bawah kanan                     
-            motor2.brake(1);
+            motor2.stop(1);
             motor4.speed(-(speed-tuning4));
             motor1.speed(-(speed-tuning1));
-            motor3.brake(1);
+            motor3.stop(1);
             pc.printf("serong bawah kanan \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
             //maju
@@ -405,7 +441,7 @@
             motor4.speed(speed-tuning4);
             pc.printf("maju \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
             //mundur     
@@ -415,7 +451,7 @@
             motor4.speed(-(speed-tuning4));
             pc.printf("mundur \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         } 
         else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
             //kanan                     
@@ -425,7 +461,7 @@
             motor3.speed(-(speed-tuning3));
             pc.printf("kanan \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
             //kiri                                 
@@ -435,44 +471,49 @@
             motor3.speed(speed-tuning3);
             pc.printf("kiri \n");
             
-            vcurr += ax;
+            vcurr_base += ax;
         }
         else{
-            motor1.brake(1);
-            motor3.brake(1);
-            motor2.brake(1);
-            motor4.brake(1);
+            motor1.stop(1);
+            motor3.stop(1);
+            motor2.stop(1);
+            motor4.stop(1);
             pc.printf("diam \n");
             
-            vcurr = v0;
+            vcurr_base = v0;
         }
     
     }    
         
-    if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
+    if(((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1))|| (IR == 0)){
             //POWER WINDOW ATAS
-        motorS.speed(1);
+        motorS.speed(vcurr_slider);
         if (limit1 == 0){
-            motorS.brake(1);
+            motorS.stop(1);
         }
             
             
         pc.printf("up \n");
         c++;
+        
+        vcurr_slider += ax;
     }
     else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
             //POWER WINDOW BAWAH        
-        motorS.speed(-0.5);
+        motorS.speed(-vcurr_slider);
             
         if (limit2 ==0){
-            motorS.brake(1);
+            motorS.stop(1);
         }
             
         pc.printf("down \n");
-        c--;        
+        c--;     
+        
+        vcurr_slider += ax;   
     }
     else{
-        motorS.brake(1);
+        vcurr_slider = v0;
+        motorS.stop(1);
         if ((c <= batas_delay) && (c>=-batas_delay)){
             c=0;
         }
@@ -482,11 +523,11 @@
         
     if((c > batas_delay) && (limit1 == 0)){
         c = 0;
-        motorS.brake(1);
+        motorS.stop(1);
     }
     else if((c < -batas_delay) && (limit2 == 0)){
         c = 0;
-        motorS.brake(1);
+        motorS.stop(1);
     }
     else if( (c > batas_delay) && (limit1 != 0)){
         motorS.speed(1);
@@ -500,7 +541,7 @@
     {
             //mekanisme ambil gripper
             pc.printf("mekanisme gripper");
-            if (g==1){
+           /* if (g==1){
                 pc.printf("ambil 1");
                 pnuematik2 = 0;
                 g=2;
@@ -511,23 +552,35 @@
                 pnuematik2 = 1;
                 wait_ms(400);
                 g=1;
-            }
+            }*/
+            pc.printf("ambil 1");
+            pnuematik2 = !pnuematik2;
+                //g=2;
+            wait_ms(400);
+            
     }
 }
 
 void edf_servo(){
         if(ps2.read(PS_PAD::PAD_X)==1){
             //PWM ++
-            pwm += 0.0007;
+            
+            if( flag_river == 1){
+                pwm += 0.0007;
+            }
+            else{
+                pwm = 0.378;
+            }
+            
             if( pwm > 0.8)  pwm = 0.8;
-            pc.printf("gaspol \n");
+            pc.printf(" %f gaspol \n", pwm);
         }
         else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
             //PWM--
             pwm -= 0.0007;
             
             if(pwm < 0)    pwm = 0.0;
-            pc.printf("rem ndeng \n");
+            pc.printf("%f  rem ndeng \n", pwm);
         }
         
         if(ps2.read(PS_PAD::PAD_R2)==1){
@@ -547,8 +600,9 @@
         
         if(ps2.read(PS_PAD::PAD_START)==1){
             
+            flag_river = 1;
             sudut = 0;
-            pwm = 0.22;
+            pwm = 0.295;
         }
         
         
@@ -561,11 +615,19 @@
 
 void init_sensor(){
     char data;
-    if(com.readable()){  
-        data = com.getc();
+    if(com1.readable()){  
+        data = com1.getc();
         
         for(int i=0; i<6; i++){
-           datasensor[i] = (data >> i) & 1;
+           datasensor1[i] = (data >> i) & 1;
+       }
+    }
+    
+    if(com2.readable()){  
+        data = com2.getc();
+        
+        for(int i=0; i<6; i++){
+           datasensor2[i] = (data >> i) & 1;
        }
     }
 }
@@ -573,111 +635,48 @@
 void linetracer(float speed){    
     float speed1,speed2,speed3,speed4;
     
-        //////////////////logic dari PV (present Value)/////////////////////////
-        if(datasensor[2] && datasensor[3]){
-            speed1 = speed*driver0;
-            speed2 = speed*driver0;
-            speed3 = speed*driver0;
-            speed4 = speed*driver0;
-        }
-        else if(datasensor[2]){
-            speed1 = speed*driver1;
-            speed2 = speed*driver0;
-            speed3 = speed*driver0;
-            speed4 = speed*driver1;
-        }
-        else if(datasensor[3]){
-            speed1 = speed*driver0;
-            speed2 = speed*driver1;
-            speed3 = speed*driver1;
-            speed4 = speed*driver0;
-        }
-        else if(datasensor[1]){
-            speed1 = speed*driver2;
-            speed2 = speed*driver0;
-            speed3 = speed*driver0;
-            speed4 = speed*driver2;
-        }
-        else if(datasensor[4]){
-            speed1 = speed*driver0;
-            speed2 = speed*driver2;
-            speed3 = speed*driver2;
-            speed4 = speed*driver0;
-        }
-        else if(datasensor[0]){
-            speed1 = speed*driver3;
-            speed2 = speed*driver0;
-            speed3 = speed*driver0;
-            speed4 = speed*driver3;
-        }
-        else if(datasensor[5]){
-            speed1 = speed*driver0;
-            speed2 = speed*driver3;
-            speed3 = speed*driver3;
-            speed4 = speed*driver0;
-        }
-        
+        //////////////////logic dari PV (present Value)/////////////////////////    
+        if(datasensor1[2] && datasensor1[3])    {   speed1 = speed*driver1_0;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_0;   }
+        else if(datasensor1[2])                 {   speed1 = speed*driver1_1;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_1;   }
+        else if(datasensor1[3])                 {   speed1 = speed*driver1_0;   speed2 = speed*driver1_1;   speed3 = speed*driver1_1;   speed4 = speed*driver1_0;   }
+        else if(datasensor1[1])                 {   speed1 = speed*driver1_2;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_2;   }
+        else if(datasensor1[4])                 {   speed1 = speed*driver1_0;   speed2 = speed*driver1_2;   speed3 = speed*driver1_2;   speed4 = speed*driver1_0;   }
+        else if(datasensor1[0])                 {   speed1 = speed*driver1_3;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_3;   }
+        else if(datasensor1[5])                 {   speed1 = speed*driver1_0;   speed2 = speed*driver1_3;   speed3 = speed*driver1_3;   speed4 = speed*driver1_0;   }
         else
         {
-            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(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;    
-            }
+            if(g_flag1 == 0)        {   speed1 = speed*driver1_0;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_0;   }
+            else if(g_flag1 == 3)   {   speed1 = speed*driver1_1;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_1;   }
+            else if(g_flag1 == 4)   {   speed1 = speed*driver1_0;   speed2 = speed*driver1_1;   speed3 = speed*driver1_1;   speed4 = speed*driver1_0;   }
+            else if(g_flag1 == 2)   {   speed1 = speed*driver1_2;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_2;   }
+            else if(g_flag1 == 5)   {   speed1 = speed*driver1_0;   speed2 = speed*driver1_2;   speed3 = speed*driver1_2;   speed4 = speed*driver1_0;   }
+            else if(g_flag1 == 1)   {   speed1 = 0;                 speed2 = speed*driver1_2;   speed3 = speed*driver1_2;   speed4 = 0;                 }
+            else if(g_flag1 == 6)   {   speed1 = speed*driver1_2;   speed2 = 0;                 speed3 = 0;                 speed4 = speed*driver1_2;  }
         } 
-        
-        motor1.speed(-(float)(speed1-0.04));
-        motor2.speed(-(float)(speed2-0.04));
-        motor3.speed(-(float)(speed3-0.0));
-        motor4.speed(-(float)(speed4-0.0));
+
+        motor1.speed(-(float)(speed1-tuning1-0.04));
+        motor2.speed(-(float)(speed2-tuning2-0.04));
+        motor3.speed(-(float)(speed3-tuning3));
+        motor4.speed(-(float)(speed4-tuning4));
 
 }
 
 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;
+    if((datasensor1[2] == 1) && (datasensor1[3] == 1))   g_flag1 = 0;
+    else if(datasensor1[2] == 1)                         g_flag1 = 3;
+    else if(datasensor1[3] == 1)                         g_flag1 = 4;
+    else if(datasensor1[1] == 1)                         g_flag1 = 2;
+    else if(datasensor1[4] == 1)                         g_flag1 = 5;
+    else if(datasensor1[0] == 1)                         g_flag1 = 1;
+    else if(datasensor1[5] == 1)                         g_flag1 = 6;
+    
+    if((datasensor2[2] == 1) && (datasensor2[3] == 1))   g_flag2 = 0;
+    else if(datasensor2[2] == 1)                         g_flag2 = 3;
+    else if(datasensor2[3] == 1)                         g_flag2 = 4;
+    else if(datasensor2[1] == 1)                         g_flag2 = 2;
+    else if(datasensor2[4] == 1)                         g_flag2 = 5;
+    else if(datasensor2[0] == 1)                         g_flag2 = 1;
+    else if(datasensor2[5] == 1)                         g_flag2 = 6;
 }
 
 
@@ -689,4 +688,48 @@
     wait_ms(45);
     jarak = ping.Read_cm()/2;
     return jarak;
+}
+
+
+void putar(float speed){
+    float speed1, speed2, speed3, speed4;
+    
+    while(!((datasensor2[2] == 1) ||  (datasensor2[3] == 1))){
+        init_sensor();
+        
+        if(datasensor2[2] && datasensor2[3])    {   speed1 = 0;                  speed2 = 0;                  speed3 = 0;                  speed4 = 0;                   }
+        else if(datasensor2[3])                 {   speed1 = -speed*driver1_3;   speed2 = -speed*driver1_3;   speed3 = -speed*driver1_3;   speed4 = -speed*driver1_3;    }
+        else if(datasensor2[2])                 {   speed1 = speed*driver1_3;   speed2 = speed*driver1_3;   speed3 = speed*driver1_3;   speed4 = speed*driver1_3;    }
+        else if(datasensor2[4])                 {   speed1 = -speed*driver1_1;   speed2 = -speed*driver1_1;   speed3 = -speed*driver1_1;   speed4 = -speed*driver1_1;    }
+        else if(datasensor2[1])                 {   speed1 = speed*driver1_1;   speed2 = speed*driver1_1;   speed3 = speed*driver1_1;   speed4 = speed*driver1_1;    }
+        else if(datasensor2[5])                 {   speed1 = -speed*driver1_0;   speed2 = -speed*driver1_0;   speed3 = -speed*driver1_0;   speed4 = -speed*driver1_0;    }
+        else if(datasensor2[0])                 {   speed1 = speed*driver1_0;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_0;    }
+        else
+        {
+            if(g_flag2 == 0)        {   speed1 = speed*driver1_3;   speed2 = speed*driver1_3;   speed3 = speed*driver1_3;   speed4 = speed*driver1_3;    }
+            else if(g_flag2 == 3)   {   speed1 = -speed*driver1_2;   speed2 = -speed*driver1_2;   speed3 = -speed*driver1_2;   speed4 = -speed*driver1_2;    }
+            else if(g_flag2 == 4)   {   speed1 = speed*driver1_2;   speed2 = speed*driver1_2;   speed3 = speed*driver1_2;   speed4 = speed*driver1_2;    }
+            else if(g_flag2 == 2)   {   speed1 = -speed*driver1_1;   speed2 = -speed*driver1_1;   speed3 = -speed*driver1_1;   speed4 = -speed*driver1_1;    }
+            else if(g_flag2 == 5)   {   speed1 = speed*driver1_1;   speed2 = speed*driver1_1;   speed3 = speed*driver1_1;   speed4 = speed*driver1_1;    }
+            else if(g_flag2 == 1)   {   speed1 = -speed*driver1_0;   speed2 = -speed*driver1_0;   speed3 = -speed*driver1_0;   speed4 = -speed*driver1_0;    }
+            else if(g_flag2 == 6)   {   speed1 = speed*driver1_0;   speed2 = speed*driver1_0;   speed3 = speed*driver1_0;   speed4 = speed*driver1_0;    }
+        }
+        
+        motor1.speed((float)(speed1));
+        motor2.speed((float)(speed2));
+        motor3.speed(-(float)(speed3));
+        motor4.speed(-(float)(speed4));
+    }
+    
+        motor1.speed(-(0.35-tuning1));
+    motor2.speed(-(0.35-tuning2));
+    motor3.speed(-(0.35-tuning3));
+    motor4.speed(-(0.35-tuning4));
+        
+        wait_ms(300);
+        
+    motor1.stop(1);
+    motor2.stop(1);
+    motor3.stop(1);
+    motor4.stop(1);
 }
\ No newline at end of file

--- a/sensor_goertzel.txt	Thu Apr 28 14:59:21 2016 +0000
+++ b/sensor_goertzel.txt	Sat Feb 10 04:24:23 2018 +0000
@@ -22,11 +22,13 @@
 int sensor[6] = {A0,A5,A4,A1,A3,A2};
 
 // output pins
-//int output[6] = {0,1,2,3,4,5,13,6,12,7,11,8,10};
+int output[6] = {8,9,10,11,12,13};
 int calibrateButton = 2;
 
 //sensor data
 float THRESHOLD[6];
+float lref[6];
+float href[6];
 float magnitude[6];
 char data;
 
@@ -55,7 +57,7 @@
     tone(pwm, TARGET_FREQUENCY); 
     data = 0;
     
-    //Sensor logic
+    //Sensor logic cd
     for(int i=0; i<6; i++)
     {
       goertzel.sample(sensor[i]); //Will take n samples
@@ -65,40 +67,60 @@
       {
         data |= 1 << i;
         //Serial.print("1");
-        //digitalWrite(output[i],HIGH);
+        digitalWrite(output[i],HIGH);
       }
       else
       {
         data |= 0 << i;
-         //Serial.print("0");
-         //digitalWrite(output[i],LOW);
+        //Serial.print("0");
+        digitalWrite(output[i],LOW);
       }
   
       //Serial.print(THRESHOLD[i]);
       //Serial.print(magnitude[i]);
       //Serial.print("\t");
 
-      
     }
-
+ 
     //Serial.println();
     /* ============= KALIBRASI ============== */ 
 
     if(digitalRead(calibrateButton)== LOW)
     {
-       //Serial.write(0xFF);
-       for(int i=0; i<6; i++)
-       {
-          stat.clear();
-          for (int n=0; n<50; n++)
-          {
-            stat.add(magnitude[i]);
-          }
-          THRESHOLD[i] = stat.minimum() - 20;
+      for(int i=0; i<6; i++){
+        href[i] = THRESHOLD[i];
+        digitalWrite(output[i],HIGH);
+      }
+
+      do{
+        read_magnitude();
+        for(int i=0; i<6; i++){
+          if(magnitude[i] < href[i])        lref[i] = magnitude[i];
+          if(magnitude[i] > lref[i])        href[i] = magnitude[i];
+        }
+
+        //Serial.println("cek data");
+        delay(200);
+      }while( digitalRead(calibrateButton)!= LOW);
+
+      //Serial.println("simpan"); 
+      for(int i=0; i<6; i++){
+          THRESHOLD[i] = (href[i] + lref[i])/2;
        }
+      
        EEPROM.updateBlock(addr,THRESHOLD,6);
+
+       delay(200);
     }
     else{
-      Serial.write(data);
-    }
-}  
\ No newline at end of file
+       Serial.write(data);
+    }  
+}  
+
+void read_magnitude(){
+     for(int i=0; i<6; i++)
+     {
+        goertzel.sample(sensor[i]); //Will take n samples
+        magnitude[i] = goertzel.detect();  //check them for target_freq
+     }
+}