File content as of revision 4:8e6dfc416ecf:

#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(PA_8, PC_2, PC_1); // pwm, fwd, rev
    
    // Variabel PID    
    int errorP = 0, last_errorD = 0;
    float pwm = 0, pwm_last;
    float P, Kp;
    float D, Kd;

    // Variabel Sabeb
    int desired_rpm = 50;
    
    // Test Variabel
    int selisiherror;
    
// Main Program
 int main(void) { 
 
    DigitalIn encoder0PinA(PB_14);
    DigitalIn encoder0PinB(PB_13);
    Serial pc(USBTX,USBRX);
    pc.baud(9600);
    startMillis();
    
    // Deklarasi awal PID
    last_errorD = 0;
    pwm_last = 0;
    Kp = 0.000000033;               //0.000000037
    Kd = 0.000000297;               //0.000000270
    
    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;
    
    //motor1.speed(0.8);
    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;
    }
 }
 }