File content as of revision 1:0b6d558a5431:

#include "mbed.h"
#include "encoderKRAI.h"
#include "Motor.h"
#include "pidLo.h"

#define PI 3.14159265358979f

#define MOTOR1_FORWARD  PC_1
#define MOTOR1_REVERSE  PC_0
#define MOTOR1_PWM      PB_0
#define MOTOR2_FORWARD  PC_15
#define MOTOR2_REVERSE  PC_14
#define MOTOR2_PWM      PB_1

#define PULSE_ENCODER 84

#define ENCODER1_CHANNEL_A PC_14
#define ENCODER1_CHANNEL_B PC_2
#define ENCODER2_CHANNEL_A PC_11
#define ENCODER2_CHANNEL_B PC_12

#define ENC_MOTOR_SAMP_US 20000
#define MOTOR_SAMP_US 5173
#define PID_SAMP_US 4173
#define S_TO_US       1000000

#define a_kp 0.0
#define a_ki 0.0
#define a_kd 0.0
#define a_TS 0.007

#define b_kp 0.0
#define b_ki 0.0
#define b_kd 0.0
#define b_TS 0.007

#define WHEEL_RAD 0.069        //meter

Motor motor1 (MOTOR1_PWM, MOTOR1_FORWARD, MOTOR1_REVERSE);
Motor motor2 (MOTOR2_PWM, MOTOR2_FORWARD, MOTOR2_REVERSE);

encoderKRAI encoder1 (ENCODER1_CHANNEL_A, ENCODER1_CHANNEL_B, PULSE_ENCODER, encoderKRAI::X4_ENCODING);
encoderKRAI encoder2 (ENCODER2_CHANNEL_A, ENCODER2_CHANNEL_B, PULSE_ENCODER, encoderKRAI::X4_ENCODING);

pidLo pid_motor1(a_kp, a_ki, 0, a_TS, 1, 0, 1000, 100);
pidLo pid_motor2(b_kp, b_ki, 0, b_TS, 1, 0, 1000, 100);

Serial pcUwu (USBTX, USBRX, 115200);

int pulse1 = 0;
int pulse2 = 0;
float v1_curr = 0;
float v2_curr = 0;
float a_pwm;
float b_pwm;

uint32_t sampling_time = 0;
uint32_t sampling_debug = 0;

int main() {
    while(1) {
        if (us_ticker_read()-sampling_time >= ENC_MOTOR_SAMP_US) {
            pulse1 = encoder1.getPulses();
            pulse2 = encoder2.getPulses();
            v1_curr = (float) pulse1*2*PI*WHEEL_RAD*S_TO_US/(PULSE_ENCODER*ENC_MOTOR_SAMP_US);
            v2_curr = (float) pulse2*2*PI*WHEEL_RAD*S_TO_US/(PULSE_ENCODER*ENC_MOTOR_SAMP_US);
            
            /* reset nilai encoder */
            encoder1.reset();
            encoder2.reset();
            sampling_time = us_ticker_read();
        }
        
        if (us_ticker_read()-sampling_debug >= 100000) {
            pcUwu.printf("%.3f, %.3f\n", v1_curr, v2_curr);
            sampling_debug = us_ticker_read();
        }
        
        if(us_ticker_read()-sampling_debug >= PID_SAMP_US){
            //a_pwm = pid_motor1.createpwm(v1_target, v1_curr, 1.0);
//            b_pwm = pid_motor1.createpwm(v1_target, v1_curr, 1.0);
        }
        
        if(us_ticker_read()-sampling_debug >= MOTOR_SAMP_US){
          //  motor.speeda_pwm = pid_motor1.createpwm(v1_target, v1_curr, 1.0);
//            b_pwm = pid_motor1.createpwm(v1_target, v1_curr, 1.0);
        }
    }
}