Diff: main.cpp

Revision:

0:f439078cfdde

Child:

1:accc8b66773b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 12 10:56:42 2016 +0000
@@ -0,0 +1,94 @@
+#include "mbed.h"
+#include "millis.h"
+#include "Motor.h"
+
+
+int val; 
+
+// Variable Declaration
+    
+    //Variabel Encoder
+    int encoder0Pos = 0;
+    int encoder0PinALast = 0;
+    int n = 0;
+    float rpm;
+    int start = 1;
+    unsigned long int previousMillis = 0;
+    
+    //Variabel Motor
+    Motor motor1(PB_9, PC_5, PA_12); // pwm, fwd, rev
+    
+    // Variabel PID    
+    int errorP, last_errorD;
+    float pwm, pwm_last;
+    float P, Kp;
+    float D, Kd;
+
+    // Variabel Sabeb
+    int desired_rpm = 450;
+    
+    // Test Variabel
+    int selisiherror;
+    
+// Main Program
+ int main(void) { 
+ 
+   DigitalIn encoder0PinA(PA_5);
+   DigitalIn encoder0PinB(PA_6);
+   Serial pc(USBTX,USBRX);
+   pc.baud(9600);
+    startMillis();
+    
+    // Deklarasi awal PID
+    last_errorD = 0;
+    pwm_last = 0;
+    Kp = 0.000000007;
+    Kd = 0.00007;
+    
+    while(1){
+    
+    errorP = desired_rpm-rpm;
+    P = errorP*Kp;
+    D = Kd*(errorP-last_errorD);
+    selisiherror = (errorP-last_errorD);
+    
+    pwm = P + D + pwm_last;
+    
+    if (pwm >= 1) {
+        pwm = 1;
+        }
+    
+    motor1.speed(pwm);
+    pwm_last = pwm;
+    
+    unsigned long int currentMillis = millis();
+//    int counter;
+//    counter=currentMillis/100;
+    
+    if (currentMillis-previousMillis==500)
+    {
+       previousMillis = currentMillis;
+       rpm = encoder0Pos;
+       pc.printf ("pwm = %.5f | rpm = %.0f | P = %.10f | D = %.10f | errorP = %.d | last_errorD = %.d \n",pwm,rpm,P,D, errorP, last_errorD);
+       encoder0Pos = 0;
+    }
+
+    else
+    { 
+       n = encoder0PinA;
+       if ((!encoder0PinALast) && (n))
+       {
+         if (!encoder0PinB)
+         {
+           encoder0Pos--;
+         } else {
+           encoder0Pos++;
+         }
+         //pc.printf ("%d \n",encoder0Pos);
+        }
+        encoder0PinALast = n;
+    }
+    
+    last_errorD = errorP;
+ }
+ }
\ No newline at end of file