Diff: main.cpp

Revision:

26:256160a1a82d

Parent:

25:054d3048dd03

Child:

27:68efb1622985

--- a/main.cpp	Fri Feb 10 10:29:45 2017 +0000
+++ b/main.cpp	Fri Feb 10 13:22:02 2017 +0000
@@ -41,42 +41,54 @@
 #include "Motor.h"
 #include "Servo.h"
 #include "encoderKRAI.h"
+#include "millis.h"
 
 /***********************************************/
 /*          Konstanta dan Variabel             */
 /***********************************************/
 #define PI 3.14159265
-#define D_ENCODER 10     // Diameter Roda Encoder
-#define D_ROBOT 80             // Diameter Roda Robot
+#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, maxSpeed = 0.8, minSpeed = -0.8;
+double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
+double p,i,d;
+double last_error = 0, current_error, sum_error = 0;
+double rpm, target_rpm = 9.0;
+ 
+// Variable Bawah
 float Vt;
-
 float k_enc     = PI*D_ENCODER;
 float k_robot   = PI*D_ROBOT;
-
 float speedT    = 0.2;
 float speedB    = 0.43;
 float speedL    = 0.4;
-
 float vpid = 0;
 
-float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03;
 
-/* Deklarasi encoder */
+/* Deklarasi Encoder Base */
 encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
 
+/* Deklarasi Encoder Launcher */
+encoderKRAI encoderAtas( PB_13, PB_14, 14, encoderKRAI::X2_ENCODING);
+
 /* Deklarasi Motor Base */
-Motor motor1(PB_9, PC_5, PA_12);  // pwm, fwd, rev, Motor Depan
-Motor motor2(PB_6, PB_1, PB_12);   // pwm, fwd, rev, Motor Belakang
+Motor motor1(PB_9, PC_5, PA_12);    // pwm, fwd, rev, Motor Depan
+Motor motor2(PB_6, PB_1, PB_12);    // pwm, fwd, rev, Motor Belakang
 
 /* Deklarasi Motor Launcher */
+Motor motor3(PA_8,PC_1,PC_2);    // pwm ,fwd, rev, Motor Atas
 //Motor motorld(PA_8, PC_1, PC_2);   // pwm, fwd, rev
 //Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev
 
-/* Deklarasi Servo Launcher */
+/* Deklarasi Penumatik Launcher */
+DigitalOut pneumatik(PB_2, 0);
+
+/* Deklarasi Servo */
 //Servo servoS(PB_2);
 //Servo servoB(PA_5);
 
@@ -97,20 +109,24 @@
 joysticknucleo joystick(PA_0,PA_1);
 Serial pc(USBTX,USBRX);
 
+/* Deklarasi Variable Milis */
+unsigned long int previousMillis = 0;
+unsigned long int currentMillis;
+
 /* Variabel Stick */
 char case_ger;
-bool launcher = false, servoGo = false;
+bool launcher = false;
+//bool pneumatikGo = false;
 
 /****************************************************/
 /*         Deklarasi Fungsi dan Procedure           */
 /****************************************************/
-
-float pid(float Kp, float Ki, float Kd)
+void init_speed();
+void speedKalibrasiMotor();
+float pidBase(float Kp, float Ki, float Kd)
 {
     int errorP;
-    
     errorP = getTetha();
-    
     return (float)Kp*errorP;    
 }
 
@@ -151,9 +167,9 @@
 }
 
 void aktuator(){
-/* Fungsi untuk menggerakkan servo */
-    // Servo
-/*    if (servoGo){
+/* Fungsi untuk menggerakkan Penumatik */
+    // Pneumatik
+/*    if (pneumatikGo){
         servoS.position(20);
         wait_ms(500);
         servoS.position(-28);
@@ -172,16 +188,32 @@
         servoS.position(20);
         servoB.position(0);
     }
-    
-    // Motor Atas
+*/    
+    //Motor Atas
     if (launcher) {
-            motorld.speed(speedL);
-            motorlb.speed(speedB);
-    }else{
-            motorld.speed(0);
-            motorlb.speed(0);
+        startMillis();
+        currentMillis = millis();
+        if (currentMillis-previousMillis>=10)
+        {
+            rpm = (double)encoderAtas.getPulses();    
+            current_error = target_rpm - rpm;
+            sum_error = sum_error + current_error;
+            p = current_error*kpA;
+            d = (current_error-last_error)*kdA/10.0;
+            i = sum_error*kiA*10.0;
+            speed = p + d + i;
+            init_speed();
+            motor3.speed(speed);
+            last_error = current_error;
+            encoderAtas.reset();
+          //pc.printf("%.04lf\n",rpm);
+            previousMillis = currentMillis;
+        }
+        else
+        {
+            motor3.speed(0);
+        }
     }
-*/    
     // MOTOR Bawah
     switch (case_ger) {
         case (1): {       
@@ -202,21 +234,22 @@
             // Kanan
             //motor1.speed(-VMAX-vpid);
             //motor2.speed(0.2+vpid);
-            motor1.speed(-0.365+pid(0.09,0,0));
-            motor2.speed(0.46+pid(0.09,0,0));
+            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+pid(0.09,0,0));//belakang
-            motor2.speed(-0.46+pid(0.09,0,0));//depan
+            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);
+            break;
             }   
     } // End Switch 
 }
@@ -251,7 +284,16 @@
     }
     // pc.printf("Pwm depan = %.3f\t  Pwm belakang = %.3f\n", speedL, speedB);    
 }
-   
+
+void init_speed (){
+    if (speed>maxSpeed){
+        speed = maxSpeed;
+    }
+    
+    if (speed<minSpeed){
+        speed = minSpeed;
+    }
+}  
 /*********************************************************/
 /*                  Main Function                        */
 /*********************************************************/
@@ -271,25 +313,28 @@
         // 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();
             aktuator();
-
-            if (joystick.segitiga_click)    launcher    = !launcher;
-            if (joystick.lingkaran_click)   servoGo     = true;
-            speedKalibrasiMotor();
-        } 
+            if (joystick.segitiga_click){
+                launcher = !launcher;
+            }
+            if (joystick.lingkaran_click){
+                pneumatik = 1;
+                wait_ms(500);
+                pneumatik = 0;
+            }
+            speedKalibrasiMotor();  
+        }
         else {
             joystick.idle();    
-            //motor1.brake(1);
-            //motor2.brake(1);
+            motor1.brake(1);
+            motor2.brake(1);
         }
     }
 }