File content as of revision 2:89d69eae8917:

#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 = 125;
    
    // 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.00000007;
    Kd = 0.000000007;
    
    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==250)
    {
        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;
           
        last_errorD = errorP;
    }

    else
    { 
       n = encoder0PinA;
       if ((!encoder0PinALast) && (n))
       {
         if (!encoder0PinB)
         {
           encoder0Pos--;
         } else {
           encoder0Pos++;
         }
         //pc.printf ("%d \n",encoder0Pos);
        }
        encoder0PinALast = n;
    }
 }
 }