di BSCA

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of MainProgram_BaseBaru_fix_omni12Feb by KRAI 2017

Files at this revision

API Documentation at this revision

Comitter:
Najib_irvani
Date:
Sun Mar 26 03:32:56 2017 +0000
Parent:
43:3807a95aa284
Commit message:
okeeh;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 3807a95aa284 -r e23f6d8586c6 main.cpp
--- a/main.cpp	Sun Mar 12 06:56:53 2017 +0000
+++ b/main.cpp	Sun Mar 26 03:32:56 2017 +0000
@@ -25,15 +25,14 @@
 /*  Kanan =>                                                                */
 /*  Kiri  =>                                                                */
 /*                                                                          */
-/*  Tombol silang    => Power Screw Aktif                                   */
+/*  Tombol silang    => Pneumatik aktif                                     */
 /*  Tombol segitiga  => Aktif motor Launcher                                */
-/*  Tombol lingkaran => Aktif Pneumatik Launcher                            */
+/*  Tombol lingkaran => Reloader naik                                       */
+/*  Tombol kotak     => Reloader turun                                      */
 /*  Tombol L1        => Pivot Kiri                                          */
 /*  Tombol R1        => Pivot Kanan                                         */
-/*  Tombol L3        => PWM Motor Belakang Dikurangi                        */
-/*  Tombol R3        => PWM Motor Belakang Ditambah                         */
-/*  Tombol L2        => PWM Motor Depan Dikurangi                           */
-/*  Tombol R2        => PWM Motor Depan Ditambah                            */
+/*  Tombol L2        => Kurang PWM Motor Launcher                           */
+/*  Tombol R2        => Tambah PWM Motor Launcher                           */
 /*                                                                          */
 /*  Bismillahirahmanirrahim                                                 */
 /*  Jagalah Kebersihan Kodingan                                             */
@@ -53,21 +52,20 @@
 #define PI 3.14159265
 #define D_ENCODER 10        // Diameter Roda Encoder
 #define D_ROBOT 80          // Diameter Roda Robot
-#define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
 
 // Variable Atas
 double speed, speed2;
 const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.06757, kiA=0.00006757;
+const double kpA=0.6757, kdA=0.7757, kiA=0.00003757;
 double p,i,d;
 double p2,i2,d2;
 double last_error = 0, current_error, sum_error = 0;
 double last_error2 = 0, current_error2, sum_error2 = 0;
 float rpm, rpm2;
-float target_rpm = 8.0, target_rpm2 = 10.0; 
+float target_rpm = 16.0, target_rpm2 = 16.0; // selisih 4 bagus, sama bagus
 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
 
-const float pwmPowerUp        = 0.8;
+const float pwmPowerUp        = 0.9;
 const float pwmPowerDown      = -0.9;
 
 float jarak_ping=0;
@@ -77,13 +75,12 @@
 float keliling_enc      = PI*D_ENCODER;
 float keliling_robot    = PI*D_ROBOT;
 float speedT            = 0.2;
-float vpid              = 0;
-float PIVOT             = 0.2;      // PWM Pivot Kanan, Pivot Kiri
+float PIVOT             = 0.17;      // PWM Pivot Kanan, Pivot Kiri
 float tuneDpn           = 0.62;     // Tunning PWM motor Depan
 float tuneBlk           = 0.62;     // Tunning PWM motor belakang
-float tuneR             = 0.72;
+float tuneR             = 0.78;
 float tuneL             = 0.72;
-float serong            = 0.65;
+float serong            = 0.4;
 float rasio             = 1.4545;
 
 /* variable tunning */
@@ -111,6 +108,7 @@
 bool isReload = false;
 bool ReloadOn = false;
 bool flag_Pneu = false;
+bool ready = false;
 
 /*****************************************************/
 /*   Definisi Prosedur, Fungsi dan Setting Pinout    */
@@ -132,18 +130,18 @@
 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
 
 /* Deklarasi Encoder Launcher */
-encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
-encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherBlk( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
 
 /* Deklarasi Motor Base */
-Motor motorDpn(PB_9, PA_12, PC_5);  // pwm, fwd, rev
-Motor motorBlk(PB_6, PB_1, PB_12);    
-Motor motorL  (PA_11, PA_6, PA_7);
-Motor motorR  (PB_7, PA_14, PA_15);
+Motor motorDpn(PC_7, PC_13, PC_14); //(PB_9, PA_12, PC_5);
+Motor motorBlk(PB_9, PC_5, PA_12);  //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13); 
+Motor motorL  (PA_11, PA_6, PA_7);  
+Motor motorR  (PC_6, PB_5, PB_4);   //(PC_6, PB_4, PB_5);
 
 /* Deklarasi Motor Launcher */
 Motor launcherDpn(PA_8,PC_2,PC_1);    // pwm ,fwd, rev
-Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev 
+Motor launcherBlk(PB_7, PA_14, PA_15); // pwm, fwd, rev 
 Motor powerScrew(PA_9, PA_4, PC_15); // pwm, fwd, rev                
 
 /* Deklarasi Penumatik Launcher */
@@ -153,6 +151,7 @@
 //DigitalIn infraAtas(PC_9, PullUp);
 DigitalIn limitTengah(PB_10, PullUp);
 DigitalIn limitBawah(PC_8, PullUp);
+DigitalIn limitBawah1(PA_5, PullUp);
 
 /*deklarasi PING ultrasonic*/
 Ping pingAtas(PC_9);
@@ -241,43 +240,35 @@
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
         // Serong kanan maju
-        caseJoystick = 13;
-         //pc.printf("bawah");       
+        caseJoystick = 13;     
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Serong kiri maju
-        caseJoystick = 14;
-         //pc.printf("bawah");       
+        caseJoystick = 14;      
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
         // Serong kanan mundur
-        caseJoystick = 15;
-         //pc.printf("bawah");       
+        caseJoystick = 15;       
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Serong kiri mundur
-        caseJoystick = 16;
-         //pc.printf("bawah");       
+        caseJoystick = 16;       
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
         caseJoystick = 3;
-         //pc.printf("kanan");
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Kiri
         caseJoystick = 4;
-         //pc.printf("kiri");       
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
         // Atas -- Maju
-        caseJoystick = 8;
-         //pc.printf("atas");       
+        caseJoystick = 8;       
     }
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
         // Bawah -- Mundur
-        caseJoystick = 9;
-         //pc.printf("bawah");       
+        caseJoystick = 9;       
     } 
     else if (joystick.segitiga_click){
         // Motor Launcher
@@ -482,35 +473,36 @@
         {
             // Serong kanan maju
             motorDpn.speed(-serong);
-            motorL.brake(-rasio*serong);
+            motorL.speed(-serong);
             motorBlk.speed(serong);
-            motorR.brake(rasio*serong);
+            motorR.speed(serong);
             break;
         }
         case (14) : 
         {
             // Serong kiri maju
             motorDpn.speed(serong);
-            motorR.brake(rasio*serong);
+            motorR.speed(serong);
             motorBlk.speed(-serong);
-            motorL.brake(-rasio*serong);
+            motorL.speed(-serong);
             break;
         }
         case (15) : 
         {
             // Serong kanan mundur 
-            motorR.brake(-rasio*serong);
+            motorDpn.speed(-serong);
+            motorR.speed(-serong);
             motorBlk.speed(serong);
-            motorL.brake(rasio*serong);
+            motorL.speed(serong);
             break;
         }
         case (16) : 
         {
             // Serong kiri mundur
             motorDpn.speed(serong);
-            motorL.brake(rasio*serong);
+            motorL.speed(serong);
             motorBlk.speed(-serong);
-            motorR.brake(-rasio*serong);
+            motorR.speed(-serong);
             break;
         }
         case (3) : 
@@ -534,7 +526,6 @@
         case (8) : 
         {
             // Maju
-            //init_rpm();
             motorR.speed(tuneR);
             motorL.speed(-tuneL);
             motorDpn.brake(1);
@@ -544,7 +535,6 @@
         case (9) : 
         {
             // Mundur
-            //init_rpm();
             motorR.speed(-tuneR);
             motorL.speed(tuneL);
             motorDpn.brake(1);
@@ -576,9 +566,13 @@
         case (10) : 
         {
             // Pneumatik
-            pneumatik = 0;
-            previousMillis3 = millis();
-            flag_Pneu = true;
+            if (ready)
+            {
+                pneumatik = 0;
+                previousMillis3 = millis();
+                flag_Pneu = true;
+                ready = false;
+            }
             break;
         }
         case (11) : 
@@ -610,7 +604,7 @@
     if(ReloadOn){
         if(isReload){
             powerScrew.speed(pwmPowerDown);
-            if(!limitBawah){
+            if((!limitBawah)||(limitBawah1)){
                 isReload = false;
                 ReloadOn = false;
             }
@@ -620,12 +614,15 @@
         }
         else if((jarak_ping > 6.5) && !flag_Pneu){
             powerScrew.speed(pwmPowerUp);
+            ready = false;
         }
-        else if((jarak_ping < 6 ) && !flag_Pneu) {
-            powerScrew.speed(-0.1);
+        else if((jarak_ping < 6.0) && !flag_Pneu) {
+            powerScrew.speed(-0.4);
+            ready = false;
         }
         else{
             powerScrew.brake(1);
+            ready = true;
         }
     }
     else{
@@ -694,6 +691,12 @@
     {
         launcherDpn.brake(1);
         launcherBlk.brake(1);
+        sum_error = 0;
+        sum_error2 = 0;
+        current_error = 0;
+        current_error2 = 0;
+        last_error = 0;
+        last_error2 = 0;
     }     
 }
   
@@ -754,13 +757,8 @@
             joystick.idle();
         }
         
-        if(millis() - previousMillis5 >= 400){
-            //display.write(0,((int) target_rpm2-2) / 10);
-            //display.write(1,((int)target_rpm2-2) % 10);
-            //display.write(2, (int)target_rpm2 / 10);
-            //display.write(3, (int)target_rpm2 % 10);
-            //display.setColon(true);
-            
+        if(millis() - previousMillis5 >= 400)
+        {    
             display.write(0,((int) rpm2) / 10);
             display.write(1,((int)rpm2) % 10);
             display.write(2, (int)target_rpm2 / 10);