File content as of revision 3:545fb5899097:

#include "mbed.h"
#include "millis.h"
#include "Motor.h"
#include "PID.h"

 int val; 

 int encoder0Pos = 0;
 int encoder0PinALast = 0;
 int n = 0;
 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;

// ----------------------------
// 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


    
    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
    
        unsigned long int currentMillis = millis();
    
       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;
      
       //pc.printf ("rpms = %.0f \n",rpms);
       rpms = 0;
       encoder0Pos = 0;
    }
 }
 
 }