File content as of revision 2:dea57b3f84cd:

/**
 *          THROWER ROBOT
 * Bismillahirahmanirrahim
 *
 * Updated :
 *  -
 *
 * To do List :
 *  -Coba ODOMETRY !!
 *  -Coba baca encoder kanan dan kiri dulu
 *
 * UPDATED : 18/10
 * - Dapet nilai mundur Kp = 0.00083, pwm kanan kiri = 0.4
 * - Dapet nilai maju Kp = 0.00035
 * Last Edited by : Calmantara & Robertsen
 **/

 
//LIBRARY
#include "mbed.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "millis.h"

/***************************
 *  Konstanta dan Variabel *
 ***************************/
#define PI 3.141592
#define DRODA 100

//Variabel Robot
float kll_roda = (float) PI*DRODA;  //variabel keliling robot

//Primitive Function
void limitMotor();  //procedure untuk mengecek limit pwm motor
float getY();         //fungsi untuk menghitung jarak Y

//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 Kecepatan
float pwm_kanan = 0.4;
float pwm_kiri = -0.4;
float limit_pwm = 0.8;

//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_10, PC_11, 540, encoderKRAI::X4_ENCODING); //chA, chB, revPulse, interruptType
encoderKRAI encMotor_kiri(PC_2, PC_3, 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
float delta_kaki;      //variabel untuk membaca nilai delta kanan kiri
float rev_depan;       //variabel untuk membaca nilai putaran roda depan
float rev_kanan;       //variabel untuk membaca nilai putaran roda kanan
float rev_kiri;       //variabel untuk membaca nilai putaran roda kiri

//Target nilai encoder
float target_depan = 0; //Target untuk jumlah rotary encoder gerakan

//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 kp_1 = 0.00035;                //variabel konstanta PID
float current_error1, prev_error1;   //variabel untuk millis PID
float speed, prev_speed;             //variabel untuk penambahan pwm PID

//Variabel cari jarak
float target_jarak = 2.0;             //variabel yang digunakan untuk jarak odometry
float jarak_y;                        //variabel untuk increment jarak y

//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(3000);
    startMillis();

//Looping Program
    while(1){
        current_millis = millis();
        
        /* Speed Motor */
        motor_kanan.speed(pwm_kanan);
        motor_kiri.speed(pwm_kiri);
        
        pc.printf("%f\t%f\t%f\n", speed, pulses_kanan, pulses_kiri);
        
        if(current_millis - prev_millis1 >= time_sampling){
        /* masuk ke perhitungan ketika sudah pada time sampling */
            pulses_kanan = (float) encMotor_kanan.getPulses();
            pulses_kiri = (float) encMotor_kiri.getPulses();
            rev_kanan = (float) encMotor_kanan.getRevolutions(); 
            rev_kiri = (float) encMotor_kiri.getRevolutions();
            delta_kaki = pulses_kanan + pulses_kiri;
            current_error1 = target_depan - delta_kaki;
            speed = prev_speed + kp_1*current_error1 + (-kp_1)*prev_error1;
            /* Kondisi untuk menambahkan speed */
            pwm_kiri = pwm_kiri + speed;
            pwm_kanan = pwm_kanan + speed;
            prev_speed = speed;
            prev_error1 = current_error1;
            prev_millis1 = current_millis;
            encMotor_kanan.reset();
            encMotor_kiri.reset();
            }
            
        if(current_millis - prev_millis2 >= (int)5000){
            pwm_kiri = 0;
            pwm_kanan = 0;
            }
    }
} /* end of main fuction */

/************************************
 * Deklarasi Fungsi dan Prosedur    *
 ************************************/
 void limitMotor(){
 /* Prosedur yang digunakan untuk mengecek batas limit pwm motor */    
     if(pwm_kanan > limit_pwm){
            pwm_kanan = limit_pwm;
            }
        else if(pwm_kanan < (-limit_pwm)){
            pwm_kanan = -limit_pwm;
            } 
        if(pwm_kiri > limit_pwm){
            pwm_kiri = limit_pwm;
            }
        else if(pwm_kiri < (-limit_pwm)){
            pwm_kiri = -limit_pwm;
            } 
     }/* end of limitMotor */