Diff: main.cpp

Revision:

3:545fb5899097

Parent:

2:7565fb31357d

--- a/main.cpp	Fri Sep 23 12:56:22 2016 +0000
+++ b/main.cpp	Fri Oct 07 13:39:44 2016 +0000
@@ -1,61 +1,140 @@
 #include "mbed.h"
 #include "millis.h"
 #include "Motor.h"
+#include "PID.h"
 
-int val; 
+ int val; 
 
  int encoder0Pos = 0;
  int encoder0PinALast = 0;
  int n = 0;
- float rpm;
+ float rpm, rpms=0;
+ float speed;
+ float speeds;
+ float maxSpeed = 0.6;
+ float minSpeed = 0.1;
+ float speedR;
+ float pid;
+
  int start = 1;
+ 
+ float rpmStandard = 40; // tentuin rpm yang diinginkan
  unsigned long int previousMillis = 0;
- //float pwmRead=0.00;
- 
- Motor motor1(D11,D13,D12);
- AnalogIn pwmIn(A0);
+
+// ----------------------------
+// inisialisasi encoder 
+DigitalIn encoder0PinA(D7);
+DigitalIn encoder0PinB(D8);
+// ----------------------------
+
+// -----------------------------
+// Inisialisasi PID
+// PID(KP,TD,TI,INTERVAL PID)
+PID PID(0.985,0.0,0.0,0.001);
+// ----------------------------
+
+// ----------------------------
+// INISIALISASI SERIAL
+Serial pc(USBTX,USBRX);
+// ----------------------------
+
+// ----------------------------
+// INISIALISASI MORTOR
+Motor motor1(D3,D2,D4);
+// ----------------------------
+
+
+/*
+    init_speed() untuk mencegah pwm kelebihan batas
+    batas ditentukan oleh variabel global maxSpeed
+    ubah sesuai keinginan
+*/
+void init_speed (){
+    if (speed>maxSpeed){
+        speed = maxSpeed;
+    }
+    
+    if (speed<minSpeed){
+        speed = minSpeed;
+    }
+}
+
+/*
+    PID_ENCODER(float speed)
+    speed == pwm
+    pv merupakan process value dari keadaan nyata
+        --- 0 untuk keadaan rpm == rpmStandard
+        --- -1 untuk keadaan rpm < rpmStandard
+        --- 1 untuk keadaan rpm > rpmStandard
+*/
+void PID_ENCODER(float speed) {
+    int pv;
+     if(rpm == rpmStandard) {
+       pv = 0;
+     }
+     else if(rpm < rpmStandard) {
+      pv = -1;
+     } else {
+      pv = 1;    
+     }
+     
+    PID.setProcessValue(pv);
+    PID.setSetPoint(0);
+    pid = PID.compute();
+    speedR = speed + PID.compute(); // compute
+    if(speedR < minSpeed) {
+        speedR = minSpeed;
+    }
+    motor1.speed(speedR); // change speed of motor 1
+}
  
  int main(void) { 
+    pc.baud(9600); // set pc_baud
+    PID.setInputLimits(-1,1); // input limits pv dari -1 sampai 1 yaitu -1,0,1
+    PID.setOutputLimits(0,0.2); 
+    // output limits adalah batasan pwm max. karna pwm max diset oleh variabel maxSpeed, jadi ubah2 output limits sesuai batasan
+    // contoh jika input speed 0.2 dan batasan speed maksimum adalah 0.5, maka setoutputlimits menjadi (0,0.3); karna 0.2 + 0.3 = 0.5
+    PID.setMode(1.0); // set pid jadi full auto
+    PID.setBias(0.0); // default
+    PID.reset(); // default
 
-   DigitalIn encoder0PinA(D3);
-   DigitalIn encoder0PinB(D4);
-   Serial pc(USBTX,USBRX);
 
+    
     startMillis();
+    speed = 0.3; // tentuin speed sesuai keinginan
     
     while(1){
+        speeds = speed;
+        init_speed();
+        PID_ENCODER(speeds);
+   
+        pc.printf("pwm = %.02f\t pid = %.02f \t rp_loop = %.0f\n",speedR, pid, rpm); // output ke serial komputer
     
-//    pwmRead = pwmIn.read();
-    motor1.speed(pwmIn.read());
-    pc.printf("pwm = %.02f",pwmIn.read());
+        unsigned long int currentMillis = millis();
     
-    unsigned long int currentMillis = millis();
-//    int counter;
-//    counter=currentMillis/100;
-    
+       n = encoder0PinA;
+        if ((!encoder0PinALast) && (n))
+        {
+            if (!encoder0PinB)
+            {
+                encoder0Pos--;
+            } else {
+                encoder0Pos++;
+            }
+        } 
+        encoder0PinALast = n;
+        
+        rpm = 120.0/7.0*encoder0Pos; // hitung rpm
+        //rpms = rpms + rpm;
+             
     if (currentMillis-previousMillis>=500)
     {
        previousMillis = currentMillis;
-       rpm = 120.0/7.0*encoder0Pos;
-       pc.printf ("Dalam 500 ms = %d        ",encoder0Pos);
-       pc.printf ("rpm = %.0f \n",rpm);
+      
+       //pc.printf ("rpms = %.0f \n",rpms);
+       rpms = 0;
        encoder0Pos = 0;
     }
-
-    else
-    { 
-       n = encoder0PinA;
-       if ((!encoder0PinALast) && (n))
-       {
-         if (!encoder0PinB)
-         {
-           encoder0Pos--;
-         } else {
-           encoder0Pos++;
-         }
-         //pc.printf ("%d \n",encoder0Pos);
-        }
-        encoder0PinALast = n;
-    }
  }
+ 
  }
\ No newline at end of file