hari ini

Dependencies:   DigitDisplay Motor PID Ping mbed millis

Fork of DagonFly__RoadToJapan_11Mei by KRAI 2017

Revision:
31:d5cbda07fd95
Parent:
30:d69cc27ac644
Child:
32:581d4a2373f0
--- a/main.cpp	Sun Feb 12 11:19:48 2017 +0000
+++ b/main.cpp	Mon Feb 13 12:03:37 2017 +0000
@@ -1,6 +1,8 @@
 /****************************************************************************/
 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
 /*                                                                          */
+/*                  Last Update : 13 Februari 2017                          */
+/*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
 /*                      ______________________                              */
@@ -49,88 +51,172 @@
 #define PI 3.14159265
 #define D_ENCODER 10        // Diameter Roda Encoder
 #define D_ROBOT 80          // Diameter Roda Robot
-#define VMAX 0.3            // Kiri Kanan
-#define PIVOT 0.4           // Pivka, Pivki
 #define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
 
 // Variable Atas
-double speed,speed2, maxSpeed = 0.8, minSpeed = 0;
+double speed, speed2, maxSpeed = 0.8, minSpeed = 0;
 double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
 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, target_rpm = 2.0;
-float rpm2, target_rpm2 = 4.0;
+float rpm, target_rpm = 8.0;
+float rpm2, target_rpm2 = 8.0;
+float pwmPowerUp        = 0.5;
+float pwmPowerDown      = -0.5;
 bool henti=false, hentis=false; 
  
 // Variable Bawah
 float Vt;
-float k_enc     = PI*D_ENCODER;
-float k_robot   = PI*D_ROBOT;
-float speedT    = 0.2;
-float vpid      = 0;
-float pwmP      = 0.5;
-float pwmT      = -0.95;
+float keliling_enc      = PI*D_ENCODER;
+float keliling_robot    = PI*D_ROBOT;
+float speedT            = 0.2;
+float vpid              = 0;
+float PIVOT             = 0.4;      // PWM Pivot Kanan, Pivot Kiri
+float tuneDpn           = 0.365;    // Tunning PWM motor Depan
+float tuneBlk           = 0.46;      // Tunning PWM motor belakang
+
+/* Variabel Encoder Bawah */
+float errTetha, Tetha;    // Variabel yang didapatkan encoder
+
+/* Deklarasi Variable Millis */
+unsigned long int previousMillis = 0;
+unsigned long int currentMillis;
+unsigned long int previousMillis2 = 0;
+unsigned long int currentMillis2;
+unsigned long int previousMillis3 = 0;
 
+/* Variabel Stick */
+//Logic untuk masuk aktuator
+int case_joy;
+bool isLauncher = false;
+bool isReload = false;
+int flagLauncher = 1;
+bool flag_Pneu = false;
+
+/*****************************************************/
+/*   Definisi Prosedur, Fungsi dan Setting Pinout    */
+/*****************************************************/
+
+/* Fungsi dan Procedur Encoder */
+void init_speed();                  // 
+void aktuator();                    // Pergerakan aktuator berdasarkan case joystick
+int case_joystick();                // Mendapatkan case dari joystick
+//void speedKalibrasiMotor();         // Pertambahan target RPM motor atas melalui joystick
+void setCenter();                   // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
+float getTetha();                   // Fungsi mendapatkan error Tetha
+
+/* Inisialisasi  Pin TX-RX Joystik dan PC */
+joysticknucleo joystick(PA_0,PA_1);
+Serial pc(USBTX,USBRX);
 
 /* Deklarasi Encoder Base */
-encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
 
 /* Deklarasi Encoder Launcher */
-encoderKRAI encoderAtas2( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
-encoderKRAI encoderAtas( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
+encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
+encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
 
 /* Deklarasi Motor Base */
-Motor motor1(PB_9, PA_12, PC_5);    // pwm, fwd, rev, Motor Depan
-Motor motor2(PB_6, PB_1, PB_12);    // pwm, fwd, rev, Motor Belakang
+Motor motorDpn(PB_9, PA_12, PC_5);    // pwm, fwd, rev
+Motor motorBlk(PB_6, PB_1, PB_12);    // pwm, fwd, rev
 
 /* Deklarasi Motor Launcher */
-Motor motorL1(PA_8,PC_2,PC_1);    // pwm ,fwd, rev, Motor Launcher1
-Motor motorL3(PA_10, PC_3, PC_0); // pwm, fwd, rev, Motor Launcher2 
-Motor motorP(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Powerscrew                
+Motor launcherDpn(PA_8,PC_2,PC_1);    // pwm ,fwd, rev
+Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev 
+Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev                
 
 /* Deklarasi Penumatik Launcher */
 DigitalOut pneumatik(PB_3, PullUp);
 
 /*Dekalrasi Limit Switch */
-DigitalIn limitA(PC_9, PullUp);
-DigitalIn limitT(PB_10, PullUp);
-DigitalIn limitB(PC_8, PullUp);
-
-/**
- *  posX dan posY berdasarkan arah robot
- *  encoder Depan & Belakang sejajar sumbu Y
- *  encoder Kanan & Kirim sejajar sumbu X 
- **/
-
-/* Variabel Encoder */
-float errT, Tetha;                  // Variabel yang didapatkan encoder
+DigitalIn limitAtas(PC_9, PullUp);
+DigitalIn limitTengah(PB_10, PullUp);
+DigitalIn limitBawah(PC_8, PullUp);
 
-/* Fungsi dan Procedur Encoder */
-void setCenter();                   // Fungsi reset agar robot di tengah
-float getTetha();                   // Fungsi mendapatkan jarak Tetha
-
-/* Inisialisasi  Pin TX-RX Joystik dan PC */
-joysticknucleo joystick(PA_0,PA_1);
-Serial pc(USBTX,USBRX);
-
-/* Deklarasi Variable Milis */
-unsigned long int previousMillis = 0;
-unsigned long int currentMillis;
-unsigned long int previousMillis2 = 0;
-unsigned long int currentMillis2;
-
-/* Variabel Stick */
-char case_ger;
-bool launcher = false;
-bool reload = false;
 
 /****************************************************/
 /*         Deklarasi Fungsi dan Procedure           */
 /****************************************************/
-void init_speed();
-void speedKalibrasiMotor();
+int case_joystick()
+{
+//---------------------------------------------------//
+//  Gerak Motor Base                                 //
+//   Case 1  : Pivot kanan                           //
+//   Case 2  : Pivot Kiri                            //
+//   Case 3  : Kanan                                 //
+//   Case 4  : Kiri                                  //
+//   Case 5  : Break                                 //
+//---------------------------------------------------//
+    
+    int caseJoystick;
+    if (!joystick.L1 && joystick.R1) {
+        // Pivot Kanan
+        caseJoystick = 1;
+    } 
+    else if (!joystick.R1 && joystick.L1) {
+        // Pivot Kiri
+        caseJoystick = 2;
+    } 
+    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.segitiga_click && flagLauncher){
+        // Motor Launcher
+        caseJoystick = 5;
+    }
+    else if (joystick.R2_click && (target_rpm2 < 14)){
+        // Target Pulse PID ++ Motor Depan
+         caseJoystick = 6;
+    }  
+    else if (joystick.L2_click && (target_rpm2 > 1)){
+        // Target Pulse PID -- Motor Depan
+         caseJoystick = 7;
+    }
+    else if (joystick.R3_click && (target_rpm < 14)){
+        // Target Pulse PID ++ Motor Belakang
+         caseJoystick = 8;
+    }
+    else if (joystick.L3_click && (target_rpm > 1)){
+        // Target Pulse PID -- Motor Belakang
+         caseJoystick = 9;
+    }
+    else if (joystick.silang_click){
+        // Pnemuatik ON
+        caseJoystick = 10;
+    }
+    else if ((joystick.atas)&&(!joystick.bawah))  {   
+        // Power Screw Up
+        caseJoystick = 11;
+    } 
+    else if ((!joystick.atas)&&(joystick.bawah)) {   
+        // Power Screw Down
+        caseJoystick = 12;       
+    } 
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
+        // Break
+        caseJoystick = 13; 
+    }
+    
+    return(caseJoystick);
+}
+
+float getTetha(){
+// Fungsi untuk mendapatkan nilai tetha
+    float busur, tetha;
+    busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
+    tetha = busur/keliling_robot*360;
+    
+    return -(tetha);    
+}
+
 float pidBase(float Kp, float Ki, float Kd)
 {
     int errorP;
@@ -138,86 +224,143 @@
     return (float)Kp*errorP;    
 }
 
-int case_gerak(){
-/****************************************************
- **  Gerak Motor Base
- **  Case 1  : Pivot kanan
- **  Case 2  : Pivot Kiri
- **  Case 3  : Kanan
- **  Case 4  : Kiri
- **  Case 5  : Break
- ****************************************************/ 
-    
-    int casegerak;
-    if (!joystick.L1 && joystick.R1) {
-        // Pivot Kanan
-        casegerak = 1;
-    } 
-    else if (!joystick.R1 && joystick.L1) {
-        // Pivot Kiri
-        casegerak = 2;
-    } 
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
-        // Kanan
-        casegerak = 3;
-//        pc.printf("kanan");
-    } 
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
-        // Kiri
-        casegerak = 4;
-//        pc.printf("kiri");       
-    } 
-    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
-        // Break
-        casegerak = 5; 
+void init_speed (){
+    if (speed>maxSpeed){
+        speed = maxSpeed;
+    }
+    if (speed<minSpeed){
+        speed = minSpeed;
     }
-    return(casegerak);
+    if (speed2>maxSpeed){
+        speed2 = maxSpeed;
+    }
+    if (speed2<minSpeed){
+        speed2 = minSpeed;
+    }
+}
+
+void setCenter(){
+// Fungsi untuk menentukan center dari robot
+    encoderBase.reset();
 }
 
-void aktuator(){
-/* Fungsi untuk menggerakkan Motor PowerScrew */
-    // PowerScrew
-    if (reload){
-        if (!limitA){
-            motorP.brake (1);
-            if (hentis != henti){
-                wait_ms(1500);           
-            }
-            hentis = henti;
+void aktuator()
+{
+    switch (case_joy) {
+        case (1): 
+        {       
+            // Pivot Kanan
+            motorDpn.speed(-PIVOT);
+            motorBlk.speed(-PIVOT);
+            setCenter();
+            break;
+        }
+        case (2): 
+        {
+            // Pivot Kiri
+            motorDpn.speed(PIVOT);
+            motorBlk.speed(PIVOT);
+            setCenter();
+            break;
+        }
+        case (3) : 
+        {
+            // Kanan
+            motorDpn.speed(-(tuneDpn) + pidBase(0.09,0,0));
+            motorBlk.speed((tuneBlk) + pidBase(0.09,0,0));
+            break;
+        }
+        case (4) : 
+        {
+            // Kiri
+            motorDpn.speed(tuneDpn + pidBase(0.09,0,0));
+            motorBlk.speed(-tuneBlk + pidBase(0.09,0,0));
+            break;
+        }
+        case (5) : 
+        {
+            // Aktifkan motor atas
+            isLauncher = !isLauncher;
+            break;
         }
-        else
+        case (6) : 
+        {
+            // Target Pulse PID ++ Motor Depan
+            target_rpm2 = target_rpm2++;
+            break;
+        }
+        case (7) : 
+        {
+            // Target Pulse PID -- Motor Depan
+            target_rpm2 = target_rpm2--;
+            break;
+        }
+        case (8) : 
+        {
+            // Target Pulse PID ++ Motor Belakang
+            target_rpm = target_rpm++;
+            break;
+        }
+        case (9) : 
         {
-            henti = false;
-            if (!limitT)
+            // Target Pulse PID -- Motor Belakang
+            target_rpm = target_rpm--;
+            break;
+        }
+        case (10) : 
+        {
+            // Pneumatik
+            pneumatik = 0;
+            previousMillis3 = millis();
+            flag_Pneu = true;
+            break;
+        }
+        case (11) : 
+        {
+            // Power Screw Up
+            powerScrew.speed(pwmPowerUp);
+            break;
+        }
+        case (12) : 
+        {
+            // Power Screw Down
+            powerScrew.speed(pwmPowerDown);
+            break;
+        }
+        default : 
+        {
+            motorDpn.brake(1);
+            motorBlk.brake(1);
+            if (!limitAtas)
             {
-                while (limitB) 
-                {
-                    motorP.speed(pwmT);
-                }
-                motorP.brake(1);
-                reload = !reload;
-            } 
-            else
+                powerScrew.brake(1);
+            }
+            if (!limitTengah)
+            {
+                case_joy = 12;
+                aktuator();   
+            }
+            if (!limitBawah)
             {
-                 motorP.speed(pwmP); 
+                case_joy = 11;
+                aktuator();   
             }
-        }
-    } 
-    else
+        }   
+    } // End Switch
+ }
+ 
+void launcher()
+{
+    if (isLauncher)
     {
-            motorP.brake (1);
-    }
-    
-/*Motor Atas*/
-    if (launcher) {
         startMillis();
         currentMillis  = millis();
         currentMillis2 = millis();
         
-        /*PID Launcher Depan*/
+        // PID Launcher Depan
         if (currentMillis-previousMillis>=10)
         {
-            rpm = (float)encoderAtas.getPulses();    
+            rpm = (float)encLauncherBlk.getPulses();    
             current_error = target_rpm - rpm;
             sum_error = sum_error + current_error;
             p = current_error*kpA;
@@ -225,19 +368,15 @@
             i = sum_error*kiA*10.0;
             speed = p + d + i;
             init_speed();
-            motorL1.speed(speed);
+            launcherBlk.speed(speed);
             last_error = current_error;
-            encoderAtas.reset();
+            encLauncherBlk.reset();
           //pc.printf("%.04lf\n",rpm);
             previousMillis = currentMillis;
         }
-        else
-        {
-            encoderAtas.getPulses();
-        }      
         if (currentMillis2-previousMillis2>=10)
         {
-            rpm2 = (float)encoderAtas2.getPulses();    
+            rpm2 = (float)encLauncherDpn.getPulses();    
             current_error2 = target_rpm2 - rpm2;
             sum_error2 = sum_error2 + current_error2;
             p2 = current_error2*kpA;
@@ -245,155 +384,54 @@
             i2 = sum_error2*kiA*10.0;
             speed2 = p2 + d2 + i2;
             init_speed();
-            motorL3.speed(speed2);
+            launcherDpn.speed(speed2);
             last_error2 = current_error2;
-            encoderAtas2.reset();
-          //pc.printf("%.04lf\n",rpm2);
+            encLauncherDpn.reset();
             previousMillis2 = currentMillis2;
         }
-        else
-        {
-            encoderAtas2.getPulses();
-        }
     }
     else
     {
-        motorL1.brake(1);
-        motorL3.brake(1);
-    }
-    
-    
-    // MOTOR Bawah
-    switch (case_ger) {
-        case (1): {       
-            // Pivot Kanan
-            motor1.speed(PIVOT);
-            motor2.speed(PIVOT);
-            setCenter();
-            break;
-            }
-        case (2): {
-            // Pivot Kiri
-            motor1.speed(-PIVOT);
-            motor2.speed(-PIVOT);
-            setCenter();
-            break;
-            }
-        case (3) : {
-            // Kanan
-            //motor1.speed(-VMAX-vpid);
-            //motor2.speed(0.2+vpid);
-            motor1.speed(-0.365+pidBase(0.09,0,0));
-            motor2.speed(0.46+pidBase(0.09,0,0));
-            break;
-            }
-        case (4) : {
-            // Kiri
-            //motor1.speed(VMAX-vpid);
-            //motor2.speed(-0.2+vpid);
-            motor1.speed(0.365+pidBase(0.09,0,0));//belakang
-            motor2.speed(-0.46+pidBase(0.09,0,0));//depan
-            break;
-            }
-        default : {
-            motor1.brake(1);
-            motor2.brake(1);
-            }   
-    } // End Switch 
+        launcherDpn.brake(1);
+        launcherBlk.brake(1);
+    }     
 }
-
-void setCenter(){
-/* Fungsi untuk menentukan center dari robot */
-    encoderKiri.reset();
-}
-
-float getTetha(){
-/* Fungsi untuk mendapatkan nilai tetha */
-    float busurKir, tetha;
-    busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
-    tetha = busurKir/k_robot*360;
-    
-    return -(tetha);    
-}
-
-void speedKalibrasiMotor(){
-/* Fungsi untuk speed launcher */
-    if (joystick.R3_click and target_rpm < 14){
-        target_rpm = target_rpm + 1; // RPM++ Motor Belakang
-    }
-    if (joystick.L3_click and target_rpm > 1){
-        target_rpm = target_rpm - 1; // RPM-- Motor Belakang
-    } 
-    if (joystick.R2_click and target_rpm2 < 14){
-        target_rpm2 = target_rpm2 + 1; // RPM++ Motor Depan
-    }  
-    if (joystick.L2_click and target_rpm2 > 1 ){
-        target_rpm2 = target_rpm2 - 1; // RPM-- Motor Depan
-    }
-    // pc.printf("Rpm depan = %.4f\t  Rpm belakang = %.4f\n", target_rpm, target_rpm2);    
-}
-
-void init_speed (){
-    if (speed>maxSpeed){
-        speed = maxSpeed;
-    }
-    
-    if (speed<minSpeed){
-        speed = minSpeed;
-    }
-    if (speed2>maxSpeed){
-        speed2 = maxSpeed;
-    }
-    
-    if (speed2<minSpeed){
-        speed2 = minSpeed;
-    }
-}  
+  
 /*********************************************************/
 /*                  Main Function                        */
 /*********************************************************/
 
-int main (void){
-    /* Set baud rate - 115200 */
+int main (void)
+{
+    // Set baud rate - 115200
     joystick.setup();
     pc.baud(115200);
     wait_ms(1000);
     setCenter();
     wait_ms(500);
     pc.printf("ready....");
-
-    /* Untuk mendapatkan serial dari Arduino */
     while(1)
     {
         // Interrupt Serial
         joystick.idle();        
-        //pc.printf("enco : %d \n",encoderKiri.getPulses());
-        if(joystick.readable()) {
+        if(joystick.readable()) 
+        {
             // Panggil fungsi pembacaan joystik
             joystick.baca_data();
             // Panggil fungsi pengolahan data joystik
             joystick.olah_data();
-            // Masuk ke case gerak
-            case_ger = case_gerak();
+            // Masuk ke case joystick
+            case_joy = case_joystick();
             aktuator();
-            if (joystick.segitiga_click){
-                launcher = !launcher;
-            }
-            if (joystick.lingkaran_click){
-                pneumatik = 0;
-                henti = true;
-                wait_ms(700);
+            launcher();
+            if ((millis()-previousMillis3 >= 700)&&(flag_Pneu)){
                 pneumatik = 1;
+                flag_Pneu = false;
             }
-            speedKalibrasiMotor();
-            if (joystick.silang_click){
-                reload = !reload;
-            }  
         }
-        else {
-            joystick.idle();    
-            motor1.brake(1);
-            motor2.brake(1);
+        else
+        {
+            joystick.idle();
         }
     }
 }
\ No newline at end of file