File content as of revision 0:62ded4362bbc:

#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 ENCODER1_CHANNEL_A PC_14
#define ENCODER1_CHANNEL_B PC_2
#define PULSE_ENCODER 84
#define ENCODER2_CHANNEL_A PC_12
#define ENCODER2_CHANNEL_B PC_11

#define ENC_MOTOR_SAMP_US 2000
#define S_TO_US          1000000

#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);

Serial pcUwu (USBTX, USBRX, 115200);

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

uint32_t sampling_time = 0;
uint32_t sampling_debug = 0;

int main() {
    while(1) {
        if (us_ticker_read()-sampling_time >= 15000) {
            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("Pulse1: %d, Current_v1: %.3f\n", pulse1, v1_curr);
            pcUwu.printf("Pulse2: %d, Current_v2: %.3f\n", pulse2, v2_curr);
            
            sampling_debug = us_ticker_read();
        }
    }
}