Diff: main.cpp

Revision:

3:8f57f69f6e64

Parent:

1:a2c7dd0a0f6e

--- a/main.cpp	Fri Mar 29 11:31:22 2019 +0000
+++ b/main.cpp	Wed Apr 10 01:44:10 2019 +0000
@@ -11,19 +11,23 @@
 #define TOLERANCET 1.0
 #define TS 7.0
 
+#define Kp_A 0.06//0.078
+#define Ki_A 8.554e-5
+#define Kd_A 0
+DigitalIn Ubutton(USER_BUTTON);
+Serial pc(USBTX, USBRX, 115200);
 CMPS12_KRAI cmps(PB_3, PB_10, 0xc0);
 encoderKRAI encSteer(PC_2,PC_3, 538, encoderKRAI::X4_ENCODING);  //encoder motor steering
 encoderKRAI encKanan(PB_9,PB_8, 538, encoderKRAI::X4_ENCODING);
 encoderKRAI encKiri(PC_11,PC_10, 538, encoderKRAI::X4_ENCODING);
 Motor motorSteer(PB_14, PC_4, PB_13);   //motor arah putaran CCW ++
-Motor motorKa(PA_11,  PA_7,PB_12);                  //motor kanan
-Motor motorKi(PB_15, PB_1, PB_2);                    //motor kiri
-DigitalIn Ubutton(USER_BUTTON);
-Serial pc(USBTX, USBRX, 115200);
+Motor motorKi(PA_11,  PB_12,PA_7);                  //motor kanan
+Motor motorKa(PA_5, PA_12, PA_6);                    //motor kiri
+
 DigitalIn switchKi(PA_9,PullUp);
 DigitalIn switchKa(PD_2,PullUp);
-DigitalOut pneu1[2] = {(PC_6),(PC_8)};          //kanan belakang , kiri depan
-DigitalOut pneu2[2] = {(PC_5),(PC_9)};           //kiri belakang, kanan depan
+DigitalOut pneu1[2] = {(PC_1),(PA_1)};          //kanan belakang , kiri depan
+DigitalOut pneu2[2] = {(PB_0),(PA_0)};           //kiri belakang, kanan depan
 int pulsetemp;
 float theta;
 float target;
@@ -41,19 +45,22 @@
 double KI;
 int angkat;
 int bouncing = 0;
+double pulseA, pulse_B;
 const double KP = 0.05,
              KI1 = 0.00000,
              KI2 = 0.00000;
 unsigned long lastParameter,    //millis utk perhintungan parameter
          last_mt_printPID,
-         last_mt_print,  //millis utk print
+         last_mt_print=0.0,  //millis utk print
          last_mt_button, //millis utk user button
          last_start, //millis start uji coba kuda
          lastFSM,    //finite state machine millis
          lastPWM,
          lastMotor,
          lastCompass,
-         lastThetaTarget;
+         lastThetaTarget,
+         lastSafety,
+         lastAngkat;
 int start;
 int state=0;
 float pwm;
@@ -66,32 +73,60 @@
 int thetaKa;
 int thetaKi;
 int pulseLangkah;
+double pulse_A, c_e_A, p_e_A,
+        p2_e_A, prev_speedA, speedA;
+double target_pulse=0;
+int bouncingKi, bouncingKa,  angkatKa, angkatKi;
+double pulsetempKa, pulsetempKi;
+int kiri, kanan;
+unsigned long lastStall;
+int stall,langkah;
+
+double pulseAngkatA=75.0,       //kanan belakang
+    pulseAngkatB=95.0,  //87    kiri depan
+    pulseAngkatC= 465.0,    //97    kiri belakang
+    pulseAngkatD=580.0, //565   //kanan depan
+    pulseTurunA=287.0,          //kanan belakang
+    pulseTurunB=277.0, //305    kiri depan
+    pulseTurunC=700.0,          //kiri belakang
+    pulseTurunD=760; //785      kanan depan
+int pulseKi, pulseKa;
+int angkatA=0, angkatB=0, angkatC=0,angkatD=0;
+int mulai = 0;
+
+void PIDMotor();
 void hitungParameter();
 void getHeading();
 void hitungPIDTheta(double theta_sp);
 void gerakMotor();
+
 void tambahPWM();
 void debug();
 void gerakDepan(int end);
 void hitungOdometry();
-void hitungParameterKa();
-void hitungParameterKi();
 void initPneu();
+void angkatKakiKi();
+void angkatKakiKa();
+void FSM();
 void angkatKaki();
 //int main(){
-//  startMillis();
-//  speed_maju=0.3;
-//  heading0=(float)cmps.getAngle()/10;
 //  while(1){
-//      debug();
-//      //pc.printf("TEST\n");
+//      if (!Ubutton){
+//          motorKa.speed(0.4);
+//          motorKi.speed(0.4);
+//      } else {
+//          motorKa.speed(0.0);
+//          motorKi.speed(0.0);
+//      }
 //  }
 //}
-
-
-int main()
-{
+int main() {
+    pulseKi=0;
+    pulseKa=0;
+    speedA =0;
     encSteer.reset();
+    encKanan.reset();
+    encKiri.reset();
     startMillis();
     theta = 0;
     cp = 1;
@@ -99,130 +134,312 @@
     theta_sp =0;
     ohstart=0;
     KI = KI1;
-    angkat = 0;
-    int kanan;
-    int kiri;
+
+    angkatKi = 0;
+    angkatKa = 0;
     initPneu();
+    kanan=0;
+    mulai=0;
     while(1) {
-        if (millis()- last_mt_print > 15) {
+//      pc.printf("TES");
+//      ohstart=1;
+//      if (millis() - last_mt_print>17) {
+//          pc.printf("TES %d - %.2f %.2f  %.2f\n",ohstart,pulse_A,pulse_B, speedA);//encKanan.getPulses(), encKiri.getPulses());
+//          last_mt_print=millis();
+//      }
+//      if (millis()>=10300)
+//          ohstart = 0;
+//      if(!Ubutton && millis() - last_mt_button>1000) {  //mulai gerak maju
+//          pulse_A +=0.3;
+//          last_mt_button=millis();
+//      }
+//        if(!Ubutton) {  //mulai gerak maju
+//            wait_ms(2000);  //diam 2 detik sebelum mulai
+//            ohstart = 1;
+//            state=1;
+//            last_start=millis();
+//        }
+//
+//        if (millis() - last_start>15000) { //maksimal waktu kuda gerak 17 detik
+//            ohstart=0;
+//        }
+//
+//        if (ohstart) {
+//
+//
+//        }
+        if (millis()- last_mt_print > 17){
             //pc.printf("%.2f\n",speed_maju);
-            if(!switchKa) {
+            if(!switchKa){
                 kanan = 1;
-                pc.printf("KENA     ");
-            } else {
+
+//              pc.printf("KENA Kanan\t");
+            }
+            else{
                 kanan = 0;
+                bouncingKa = 0;
             }
-            if(!switchKi) {
+            if(!switchKi){
                 kiri = 1;
-            } else {
+//              pc.printf("KENA Kiri\t");
+            }
+            else{
                 kiri = 0;
+                bouncingKi = 0;
             }
             last_mt_print = millis();
-            pc.printf("%d %d %.2f %d \n",kiri,kanan,speed_maju, pulsetemp);
+            pc.printf("%d %d s = %d\t%d\t %d%d%d%d \n",
+                    kanan, ohstart,
+                    pulseKa, pulseKi, angkatA,
+                    angkatB, angkatC, mulai);//kanan,speedA, pulseKi, pulseKa);//,pingKidep.distance(),pingKadep.distance());
         }
-        if(!Ubutton) {  //mulai gerak maju
+        if (millis() - lastAngkat>11){
+                    angkatKaki();
+                    lastAngkat=millis();
+                }
+        if (millis()>=28300)
+            ohstart = 0;
+        if(!Ubutton){   //mulai gerak maju
             wait_ms(2000);  //diam 2 detik sebelum mulai
             ohstart = 1;
             state=1;
-            pwm_maju = 0.8;
-            speed_maju=0.7;
+            stall=0;
+            lastStall=millis();
+            //pwm_maju = 0.8;
+            //speed_maju=0.7;
+            target_pulse = 5.0;
             last_start=millis();
         }
+//      urutan prioritas
+//      if (millis() - last_mt_printPID > 7){
+//          if (!(fabs(theta_sp- theta) < TOLERANCET)){
+//              hitungPIDTheta(theta_sp);
+//          }
+//          last_mt_printPID = millis();
+//      }
+//
+//      if (millis() - lastCompass>3) {
+//          getHeading();
+//          lastCompass = millis();
+//      }
 
-        if (millis() - last_start>15000) { //maksimal waktu kuda gerak 17 detik
-            ohstart=0;
+       //state machine
+//      if (millis() - last_start>7000) {
+//          ohstart = 0;    //kembali ke posisi awal (mati) setelah 15 detik
+//          target_pulse=0;
+//      }
+//      if (((kidep_dist < 150)||(kadep_dist < 150)) && (state == 1)) {
+//          //state = 2;      //kuda mendeteksi tembok di kanan atau kiri, siap untuk belok
+//      }
+//      if (((kidep_dist < 30) || (kadep_dist < 30)) && (state == 2)) {
+//          state = 3;      //kuda deteksi obstacle, siap untuk angkat kaki
+//      }
+//      /*if (kondisi cek sudah lewat obstacle dan udh mau belok lagi) {
+//          state = 4;      //kuda belok lagi, siap untuk deteksi tali dan angkat kaki
+//      }*/
+//      /*if (kondisi deteksi tali) {
+//          state 5;        //angkat kaki
+//      }*/
+//      /*if (switch nanjak dipencet) {
+//          state 6;        //jalan lurus nanjak
+//      }*/
+        if (millis() - lastParameter > 5){
+            hitungParameter();
+            PIDMotor();
+            lastParameter = millis();
         }
 
-
-        if (ohstart) {
-            angkatKaki();
-            if (millis() - lastFSM > 9) {
-                if (state==1) { //state maju lurus
-                    theta_sp=0;
-                    belok=0;
-                    pwm_maju=0.80;
-                }
+        if (ohstart){
+            if (millis() - lastFSM > 9){
+                FSM();
                 lastFSM=millis();
             }
-            //urutan prioritas
-            if (millis() - last_mt_printPID > 7) {
-                if (!(fabs(theta_sp- theta) < TOLERANCET)) {
-                    hitungPIDTheta(theta_sp);
-                }
-                last_mt_printPID = millis();
-            }
-            if (millis() - lastParameter > 5) {
-                hitungParameter();
-                hitungParameterKa();
-                hitungParameterKi();
-                lastParameter = millis();
-            }
-            if (millis() - lastMotor>4) {
+            if (millis() - lastMotor>2){
                 gerakMotor();
                 lastMotor=millis();
             }
+
+//          angkatKakiKi();
+//          angkatKakiKa();
+//
+//          if (millis() - last_ping > 9) {
+//              kidep_dist = pingKidep.distance();
+//              kadep_dist = pingKadep.distance();
+//              //belki_dist = pingKibel.distance();
+//              //belka_dist = pingKabel.distance();
+//              last_ping = millis();
+//          }
+        }
+        else {
+            target_pulse=0;
+            motorKa.speed(0);
+            motorKi.speed(0);
+//          motorKa.brake(1);
+//          motorKi.brake(1);
+        }
+        //fungsi untuk safety dari stall yang terlalu lama
+        if (millis() - lastSafety>19){
+            if (pulse_A<=0.01 && !stall){
+                lastStall=millis();
+                stall=1;
+            }
+            if (pulse_A>0.3 && stall){
+                lastStall=millis();
+                stall=0;
+            }
+            if (millis() - lastStall>3000 && stall){
+                ohstart=0;
+                target_pulse=0;
+            }
         }
 
+        //fungsi angkatKaki
+        //if (millis() - last_start>5000)
+        if (mulai>3){
+//          pneu1[1]=1;
+//          pneu2[1]=1;
+            angkatKaki();
+        }else{
+            pneu1[0]=0;
+            pneu1[1]=0;
+            pneu2[0]=0;
+            pneu2[1]=0;
+        }
     }
 
 }
 
-void hitungParameter()
-{
+void FSM(){
+    if (state==1){  //state maju lurus
+        theta_sp=0;
+        belok=0;
+        //pwm_maju=0.80;
+        target_pulse = 3;
+    }
+    else if (state == 2) {    //state belok, sudah deteksi tembok
+        //theta_target = 15.0;
+        //ubahTheta();
+        //belok = 1;
+        //KI = KI2;
+        //pwm_maju = 0.85;
+    }
+    else if (state == 3) { //state angkat kaki, udh deteksi obstacle
+        //kuda deteksi obstacle, angkat kaki
+    }
+    /*else if (state == 4) { //state belok lagi, udh lewat sand dune
+        //kuda belok lagi,
+    }*/
+    /*else if (state == 5) { //state angkat kaki, deteksi tali
+        //kuda deteksi tali, angkat kaki
+    }*/
+    /*else if (state == 6) { //state naik gunung
+        //jalan lurus nanjak
+    }*/
+}
+void hitungParameter(){
     pulse = (double)encSteer.getPulses()*PULSE_TO_DEG;
     encSteer.reset();
-    pulseLangkah = (int)encKiri.getPulses();
-    encKiri.reset();
-    pulsetemp += pulseLangkah;
-    if(bouncing == 1) {
-        pulsetemp =0;
-        bouncing = 2;//pc.printf("kena");
-    }
     theta += pulse;
 }
-void initPneu()
+void gerakMotor()
 {
+//  state=1;
+//  if (!Ubutton){
+//      speedA = 0.78;
+//  }
+//  else
+//      speedA = 0.0;
+//  speedA=1.0;
+    if (speedA > 0.15 && (ohstart)) {
+//  if (ohstart){
+        motorKa.speed(speedA);
+        motorKi.speed(speedA);
+    } else {
+        motorKa.speed(0);
+        motorKi.speed(0);
+//        motorKa.brake(1);
+//        motorKi.brake(1);
+    }
+}
+
+void PIDMotor(){
+        //target_pulse = 10.0;
+        pulse_A = (double)encKiri.getPulses();
+        //pulse_B = (double)encKanan.getPulses();
+        //encKanan.reset();
+        encKiri.reset();
+        pulseKi += (int)pulse_A;
+        pulseKa += (int)pulse_A;
+//        pulseKa = pulseKa%360;
+//        pulseKi = pulseKa%360;
+
+//        if (millis()-lastStall>2000){
+//          ohstart=0;
+//          speedA=0;
+//          target_pulse=0;
+//          state=0;
+//        }
+
+        c_e_A = (double) target_pulse - pulse_A;
+//         thetaKa += pulse_A;
+        speedA = (double) ( prev_speedA +
+                Kp_A*(c_e_A  - p_e_A) +
+                Ki_A*TS*(c_e_A + p_e_A)/2 +
+                Kd_A*(c_e_A - 2*p_e_A + p2_e_A) );
+        if (speedA>0.90)
+            speedA = 0.90;
+        else if (speed<-0.90)
+            speedA = -0.90;
+        if (!ohstart)
+            speedA=0.0;
+        prev_speedA = speedA;
+        p2_e_A = p_e_A;
+        p_e_A = c_e_A;
+}
+void initPneu(){
     pneu1[0]=0;
     pneu1[1]=0;
     pneu2[0]=0;
     pneu2[1]=0;
 }
-
-void angkatKaki()
-{
-    if(!switchKi && bouncing == 0) {
-        bouncing = 1;
-    }
-    if (pulsetemp > 0 && pulsetemp < 150 && !angkat) {
-        pneu2[0] = 1;
-        pneu2[1] = 1;
-        angkat = 1;
-    } else if (pulsetemp > 150 && angkat) {
-        pneu2[0] = 0;
-        pneu2[1] = 0;
-        angkat = 0;
-        bouncing = 0;
-    }
-}
-
-void hitungParameterKa()
-{
-    int pulseKa = (double)encKanan.getPulses()*PULSE_TO_DEG;
-    encKanan.reset();
-
-    thetaKa += pulseKa;
-}
-
-void hitungParameterKi()
-{
-    int pulseKi = (double)encKiri.getPulses()*PULSE_TO_DEG;
-    encKiri.reset();
-
-    thetaKi += pulseKi;
-}
-
-void getHeading()
-{
+//
+//void angkatKakiKi(){
+//    int batasKi = 15;
+//    if(!switchKi && bouncingKi == 0){
+//        bouncingKi = 1;
+//        pulsetempKi = 0;
+//    }
+//    if (pulsetempKi > batasKi && pulsetempKi < (batasKi + 95) && !angkatKi){
+//        pneu2[0] = 1;     //blkg
+//        pneu2[1] = 1;     //dpn
+//        angkatKi = 1;
+//    }
+//    else if (pulsetempKi > (batasKi+120) && angkatKi){
+//        pneu2[0] = 0;
+//        pneu2[1] = 0;
+//        angkatKi = 0;
+//        bouncingKi = 0;
+//    }
+//}
+//void angkatKakiKa(){
+//    int batasKa = 0;
+//    if(!switchKa && bouncingKa == 0){
+//        bouncingKa = 1;
+//        pulsetempKa = 0;
+//    }
+//    if (pulsetempKa > batasKa && pulsetempKa < (batasKa+130) && !angkatKa){
+//        pneu1[0] = 1;
+//        pneu1[1] = 1;
+//        angkatKa = 1;
+//    }
+//    else if (pulsetempKa > (batasKa+130) && angkatKa){
+//        pneu1[0] = 0;
+//        pneu1[1] = 0;
+//        angkatKa = 0;
+//        bouncingKa = 0;
+//    }
+//}
+void getHeading() {
     heading_temp = (cmps.getAngle()/10 - heading0);                //cari perubahan sudut badan
     if(heading_temp > 180.0 && heading_temp <= 360.0)             //kondisi agar pembacaan 0 - 180 derajat
         heading = (heading_temp - 360.0 );
@@ -232,14 +449,14 @@
         heading = heading_temp;
     heading = heading*(-1);                                   //output heading
 }
-void hitungPIDTheta(double theta_sp)
-{
+void hitungPIDTheta(double theta_sp){
     if (theta_sp>dThetaMax)
         theta_sp=dThetaMax;
     theta_error = theta_sp - theta;
-    if (theta_error > 180) {
+    if (theta_error > 180){
         theta_error = theta_error - 360;
-    } else if (theta_error < -179) {
+    }
+    else if (theta_error < -179){
         theta_error = theta_error + 360;
     }
 
@@ -248,47 +465,25 @@
     speed = w;
     theta_error_prev = theta_error;
 
-    if(speed >= PWM_MAX) {
+    if(speed >= PWM_MAX){
         pwm = PWM_MAX;
-    } else if (speed <= PWM_MIN) {
+    } else if (speed <= PWM_MIN){
         pwm = PWM_MIN;
     } else
         pwm = speed;
 }
-void gerakMotor()
-{
-    //if ((theta_error > 1.0 && (fabs(pwm) > 0.15)))// && (state!=0)))
-    motorSteer.speed(pwm);
-//  else {
-//        motorSteer.speed(0.0);
-//        motorSteer.brake(BRAKE_HIGH);
-//    }
-
-    tambahPWM();
-    if ( (state>0) && (fabs(speed_maju) > 0.15) && (ohstart)) {
-        motorKa.speed(speed_maju);
-        motorKi.speed(speed_maju);
-    } else {
-        motorKa.speed(0);
-        motorKi.speed(0);
-        motorKa.brake(1);
-        motorKi.brake(1);
-    }
-}
-
-void tambahPWM()
-{
-    if (millis() - lastPWM>50) {
-        if (speed_maju>pwm_maju) {
+void tambahPWM(){
+    if (millis() - lastPWM>50){
+        if (speed_maju>pwm_maju){
 //            speed_maju -=0.05;
-        } else if (speed_maju<pwm_maju) {
-            speed_maju+=0.05;
+        }
+        else if (speed_maju<pwm_maju){
+                speed_maju+=0.05;
         }
     }
     lastPWM = millis();
 }
-void debug()    //program utk cek konstanta derajat
-{
+void debug(){   //program utk cek konstanta derajat
     theta = encSteer.getPulses()*PULSE_TO_DEG;
 //    if (!Ubutton)
 //      motorKa.speed(0.8);
@@ -327,20 +522,133 @@
 //          theta, heading, theta_error, w,pwm);
 }
 
+void ubahTheta() {
+    if (millis() - lastThetaTarget >23) {
+        if (theta_sp < theta_target) {
+            theta_sp += 1.0;
+        }
+        else if (theta_sp > theta_target) {
+            theta_sp -= 1.0;
+        }
+        lastThetaTarget = millis();
+    }
+}
 
-void hitungOdometry()
-{
-    //Tolong isi program odometry utk pisisi robot terhadap lapangan
-    //berdasarkan geometry robot dan posisi ping (asumsi ping diletakan di tengah kaki bagian luar
-    //ukuran robot minta ama mekanik
-    /*  POSISI PING utk di kaki depan
-     PING            PING
-     ####            ####
-    #    #          #    #
-    #    #          #    #
-    PING#    #          #    # PING
-    #    #          #    #
-    #    #          #    #
-     ####            ####
-    */
+void angkatKaki(){
+    //1 putaran 650 pulse
+//  pulseKa=pulseKa%890;
+//  pulseKi=pulseKi%650;
+
+    if(kiri && bouncingKi == 0){
+        bouncingKi = 1;
+//      pulseKi = 0;
+//      pulseKa = 325;
+    }
+    if (pulseKa>900){
+            mulai++;
+            pulseKa=0;
+        }
+    else if(kanan && bouncingKa == 0){
+        bouncingKa = 1;
+        pulseKi = 325;
+        pulseKa = 0;
+        mulai++;
+    }
+//  //if (millis()>9000){
+////    if (pulseKi > pulseAngkatA && pulseKi < pulseTurunA && !angkatA){
+////        pneu1[0] = 1;       //blkg
+////        angkatA = 1;
+////        bouncingKi = 0;
+////    }
+////    else if (pulseKi>pulseTurunA && angkatA){
+////        pneu1[0] = 0;
+////        angkatA = 0;
+////    }
+//      if (pulseKa > pulseAngkatB && pulseKa < pulseTurunB ){
+//          pneu1[0] = 0;       //kiri dpn
+//          angkatB = 0;
+//      }
+//      else if (pulseKa < pulseAngkatB || pulseKa>pulseTurunB ){
+//          pneu1[0] = 1;       //kiri dpn
+//          angkatB = 1;
+//      }
+////    if (pulseKa > pulseAngkatC && pulseKa < pulseTurunC && !angkatC){
+////        pneu2[0] = 1;       //blkg
+////        angkatC = 1;
+////    }
+////    else if (pulseKa>pulseTurunC && angkatC){
+////        pneu2[0] = 0;
+////        angkatC = 0;
+////        bouncingKa = 0;
+////    }
+//      if (pulseKa > pulseAngkatD && pulseKa < pulseTurunD){
+//          pneu2[0] = 0;       //kanan dpn
+//          angkatD = 0;
+//      }
+//      else if (pulseKa < pulseAngkatD || pulseKa>pulseTurunD){
+//          pneu2[0] = 1;       //kanan dpn
+//          angkatD = 1;
+//
+//      }
+////    if (pulseKi > pulseAngkatA && pulseKi < pulseTurunA && !angkatA){
+////        pneu1[0] = 1;       //blkg
+////        angkatA = 1;
+////        bouncingKi = 0;
+////    }
+////    else if (pulseKi>pulseTurunA && angkatA){
+////        pneu1[0] = 0;
+////        angkatA = 0;
+////    }
+    if (mulai>9){
+
+    } else if (mulai>3 && mulai<7){
+        if (pulseKa > pulseAngkatB && pulseKa < pulseTurunB ){
+            pneu1[1] = 1;       //kiri dpn
+            angkatB = 1;
+        }
+        else if (pulseKa < pulseAngkatB || pulseKa>pulseTurunB ){
+            pneu1[1] = 0;       //kiri dpn
+            angkatB = 0;
+        }
+            if (pulseKa > pulseAngkatD && pulseKa < pulseTurunD){
+                pneu2[1] = 1;       //kanan dpn
+                angkatD = 1;
+            }
+            else if (pulseKa < pulseAngkatD || pulseKa>pulseTurunD){
+                pneu2[1] = 0;       //kanan dpn
+                angkatD = 0;
+            }
+    } else if (mulai>=7 && mulai<10){
+        if (mulai<9){
+            pneu1[1]=1;
+            pneu2[1]=1;
+        } else{
+            pneu1[1]=0;
+            pneu2[1]=0;
+        }
+        if (pulseKa > pulseAngkatA && pulseKa < pulseTurunA){
+            pneu1[0] = 1;       //blkg kanan
+            angkatA = 1;
+            //bouncingKi = 0;
+        }
+        else if (pulseKa < pulseAngkatA ||pulseKa>pulseTurunA){
+            pneu1[0] = 0;
+            angkatA = 0;
+        }
+
+        if (pulseKa > pulseAngkatC && pulseKa < pulseTurunC){
+            pneu2[0] = 1;       //blkg kiri
+            angkatC = 1;
+        }
+        else if (pulseKa < pulseAngkatC || pulseKa>pulseTurunC){
+            pneu2[0] = 0;
+            angkatC = 0;
+            //bouncingKa = 0;
+        }
+    } else if(mulai>10 && mulai<12){
+        pneu1[1]=0;
+        pneu2[1]=0;
+        pneu1[0]=1;
+        pneu2[0]=1;
+    }
 }