File content as of revision 0:a6918b57d3fa:

/**
 *          THROWER ROBOT
 * Bismillahirahmanirrahim
 *
 * Updated :
 *  -Perhitungan PID masih error
 *  -encoder kanan sama encoder kiri belum kebaca. ntah kenapa
 *
 * To do List :
 *  -Coba ODOMETRY !!
 *
 * Last Edited by : Calmantara
 **/
 
//LIBRARY
#include "mbed.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "millis.h"

/***************************
 *  Konstanta dan Variabel *
 ***************************/

//Deklarasi Motor
Motor motor_depan(PB_6, PB_13, PA_10);  //pwm, fwd, rev
Motor motor_kanan(PB_8, PA_9, PA_5);    //pwm, fwd, rev
Motor motor_kiri(PB_9, PA_12, PA_11);   //pwm, fwd, rev

//Variabel Motor
float pwm_kanan = 0.3;
float pwm_kiri = -0.3;
float limit_speed = 0.7;

//Variabel Serial
Serial pc(USBTX, USBRX);    //Deklarasi serial pc TX RX

//Deklarasi Rotary Encoder
encoderKRAI encMotor_depan(PC_0, PC_1, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
encoderKRAI encMotor_kanan(PC_5, PC_4, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
encoderKRAI encMotor_kiri(PC_7, PC_6, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType

//Variabel Encoder
float pulses_depan;    //variabel untuk membaca nilai encoder motor depan
float pulses_kanan;    //variabel untuk membaca nilai encoder motor kanan
float pulses_kiri;     //variabel untuk membaca nilai encoder motor kiri

//Target nilai encoder
float target_depan = 0;

//Variabel PID_1
unsigned long int current_millis;    //variabel untuk mendapatkan millis yang berjalan
unsigned long int prev_millis1;      //variabel untuk mendapatkan millis sebelumnya
unsigned short time_sampling = 12;   //variabel untuk time sampling
float max_speed = 0.4;
float min_speed = -0.4;
float kp_1 = 0.5;                    //variabel konstanta PID
float current_error1, prev_error1;
float speed, prev_speed;

//Variabel Motor Berhenti
unsigned long int prev_millis2;      //variabel untuk mendapatkan millis sebelumnya

/***************************
 *  Main Function          *
 ***************************/

int main(){

//Inisiasi Serial
    pc.baud(115200);

//Start Millis
    //wait_ms(5000);
    startMillis();

//Looping Program
    while(1){
        current_millis = millis();
        /* Speed Motor */
        if(pwm_kanan > limit_speed){
            pwm_kanan = limit_speed;
            }
        else if(pwm_kanan < -limit_speed){
            pwm_kanan = -limit_speed;
            } 
        if(pwm_kiri > limit_speed){
            pwm_kiri = limit_speed;
            }
        else if(pwm_kiri < -limit_speed){
            pwm_kiri = -limit_speed;
            } 
        
        motor_kanan.speed(pwm_kanan);
        motor_kiri.speed(pwm_kiri);
        
        //pc.printf("%f\t%f\t%f\n", pulses_depan, pulses_kanan, pulses_kiri);
        
        if(current_millis - prev_millis1 >= time_sampling){
        /* masuk ke perhitungan ketika sudah pada time sampling */
            pulses_depan = (float) encMotor_depan.getRevolutions();  //Mengambil pulses encoder
            current_error1 = target_depan - pulses_depan;
            speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1;
            /* Kondisi untuk menambahkan speed */
            if(speed > max_speed){
                pwm_kiri = pwm_kiri+max_speed;
                pwm_kanan = pwm_kanan - max_speed;
            }
            else if ( speed < min_speed){
                pwm_kiri = pwm_kiri-min_speed;
                pwm_kanan = pwm_kanan + min_speed;
            }
            else {
                pwm_kiri = pwm_kiri + speed;
                pwm_kanan = pwm_kanan - speed;
            }
            prev_speed = speed;
            prev_error1 = current_error1;
            prev_millis1 = current_millis;
            }
    }
} /* end of main fuction */