File content as of revision 12:84cb23216f78:

/*******************************************
 *          THROWER ROBOT                   *
 * Bismillahirahmanirrahim                  *
 *                                          *
 * To do List :
 * - Cari konstanta Kanan Kiri
 * - Cari Konstanta lagi :(
 *
 * - JAGA SEMANGAT DAN KESEHATAN KITA !!! :)
 *
 * UPDATED : 22/10
 * - PID maju/mundur enak enak jos
 *
 * Last Edited by : Thrower KRAI
 ********************************************/

 
//LIBRARY
#include "mbed.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "millis.h"
#include "pidKRAI.h"
#include "odometryKRAI.h"
#include "Ultrasonic.h"

/***************************
 *  Konstanta dan Variabel *
 ***************************/
#define DRODA 100.0

//ultrasonic 
Ultrasonic US(PA_7, PD_2);
//Variabel Robot
double kll_roda = PI*DRODA;  //variabel keliling robot

//Variabel Program dan Planning
unsigned short in_program = 0;        //variabel untuk masuk ke looping program
short state =-1;

DigitalIn mybutton(USER_BUTTON);

//Primitive Function
void limitMotor();                          //procedure untuk mengecek limit pwm motor
void pidMotor(int x);                       //procedure untuk arah gerak motor
void getValue();                            //procedure untuk menghitung pulses dan rev

//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;        //pwm motor kanan
float pwm_kiri;         //pwm motor kiri
float pwm_depan;        //pwm motor depan
float limit_pwm = 0.9;  //limit pwm semua motor

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

//Deklarasi Odometry
odometryKRAI odometry(DRODA);   //deklarasi odometry dengan input diameter roda

//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 d_tetha;         //variabel untuk membaca nilai delta kanan kiri depan
//float d_tetha2;        //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 tn_tetha = 0;    //target untuk nilai tetha robot

//Variabel PID
unsigned short ts_base = 12;          //variabel untuk time sampling
float speed_roty;                     //variabel untuk penambahan pwm PID rotasi y
float speed_disy;                     //variabel untuk penambahan pwm PID distance y
float limit_dis = 0.4;
float limit_rot = 0.2;

float speed_rotx;                     //variabel untuk penambahan pwm PID rotasi x
float speed_disx;                     //variabel untuk penambahan pwm PID distance x

//Deklarasi PID
pidKRAI pidRotY((float) 0.234, (float) 0.0098, (float) 7.9123, ts_base);   //kp ki kd ts
pidKRAI pidRotX((float) 0.234, (float) 0.0098, (float) 7.9123, ts_base);   //kp ki kd ts

pidKRAI pidDisY((float)0.2208,(float)0.0105,(float)4.26231,ts_base);     //kp ki kd ts
pidKRAI pidDisX((float)0.228,(float)0.0234,(float)9.6231,ts_base);     //kp ki kd ts

//Variabel cari jarak
float target_y[50] = {2000.0, -2000.0};      //variabel yang digunakan untuk target jarak y
double jarak_y;                               //variabel untuk increment jarak y

float target_x[50] = {2000.0, -2000.0};      //variabel yang digunakan untuk target jarak x
double jarak_x;                               //variabel untuk increment jarak x

//Variabel Waktu
unsigned long int current_millis;    //variabel untuk mendapatkan millis yang berjalan
unsigned long int prev_millis1;      //variabel untuk masuk ke perhitungan PID
unsigned long int prev_millis2;

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

int main(){

//Inisiasi Serial
    pc.baud(115200);

//Start Millis
    wait_ms(1000);
    startMillis();
//Looping Utama
while(1){
//Looping Program Planning
    while(in_program == 0){
        
        if (mybutton == 0){
            pidRotY.reset();
            pidDisY.reset();
            pidRotX.reset();
            pidDisX.reset();
            jarak_y = 0;
            jarak_x = 0;
            state+=1;
            in_program = 1;
            }
        }/* end of planning program */
//Looping Program Utama
    while(in_program == 1){
                current_millis = millis();
                if((jarak_y <= target_y[state]-(float)5.0)||(jarak_y >= target_y[state]+(float)5.0)){
                /* masuk ke case sumbu Y */
                    if(current_millis - prev_millis1 >= 12){
                        pidMotor(0);
                        }
                    
                    /* Speed Motor */
                    limitMotor();
                    motor_depan.speed(pwm_depan);
                    motor_kanan.speed(pwm_kanan);
                    motor_kiri.speed(pwm_kiri);
                    //prev_millis2 = current_millis;
                    pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_depan, pulses_kanan, pulses_kiri); 
                    }
                if((jarak_x <= target_x[state]-(float)5.0)||(jarak_x >= target_x[state]+(float)5.0)){
                /* masuk  ke case sumbu X */
                    if(current_millis - prev_millis1 >= 12){
                        pidMotor(1);
                        }
                    
                    /* Speed Motor */
                    limitMotor();
                    motor_depan.speed(pwm_depan);
                    motor_kanan.speed(pwm_kanan);
                    motor_kiri.speed(pwm_kiri);    
                    //pc.printf("%.2f %.2f %.2f %.2f %.2f\n", jarak_x, d_tetha, rev_depan, rev_kanan, rev_kiri);
                    }
                else{
                    pwm_depan = 0.0;
                    pwm_kanan = 0.0;
                    pwm_kiri = 0.0;
                    motor_depan.brake(1);
                    motor_kanan.brake(1);
                    motor_kiri.brake(1);
                    in_program = 0;
                    }
                    
            /* mengambil data encoder motor */
            getValue();
            
            /* masuk ke perhitungan parameter */
            jarak_y += odometry.getY(rev_kanan, rev_kiri);
            jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri);
            d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
            encMotor_depan.reset();
            encMotor_kanan.reset();
            encMotor_kiri.reset();
            //pc.printf("%.2f %.2f %.2f %.2f\n", jarak_y, d_tetha, pulses_kanan, pulses_kiri);    
            }
        } /* end of main program */
    }

/************************************
 * 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;
        }
    if(pwm_depan > limit_pwm){
            pwm_depan = limit_pwm;
        }
    else if(pwm_depan < (-limit_pwm)){
        pwm_depan = -limit_pwm;
        } 
     }/* end of limitMotor */

void pidMotor(int x){
/**
 * prosedur untuk gerak motor
 * 0 : maju dan mundur
 * 1 : kanan dan kiri
 **/
    if(x==0){
        /* masuk ke perhitungan ketika sudah pada time sampling */
            /* mengambil data encoder motor */
            getValue();
            
            /* masuk ke perhitungan parameter */
            jarak_y += odometry.getY(rev_kanan, rev_kiri);
            d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
        
            speed_roty = pidRotY.pidCount(tn_tetha, d_tetha, limit_rot);
            speed_disy = pidDisY.pidCount(target_y[0], jarak_y, limit_dis);
            
            /* Kondisi untuk menambahkan speed */
            pwm_kiri = -speed_disy + speed_roty;
            pwm_kanan = speed_disy + speed_roty;
            pwm_depan = speed_roty;
            
            prev_millis1 = current_millis;
            encMotor_depan.reset();
            encMotor_kanan.reset();
            encMotor_kiri.reset();
        
        }
    else if(x==1){
    /* masuk ke perhitungan ketika sudah pada time sampling */
            /* mengambil data encoder motor */
            getValue();
            
            /* masuk ke perhitungan parameter */
            jarak_x += odometry.getX(rev_depan, rev_kanan, rev_kiri);
            d_tetha = odometry.getTetha(pulses_depan, pulses_kanan, pulses_kiri);
        
            speed_rotx = pidRotX.pidCount(tn_tetha, d_tetha, limit_rot);
            speed_disx = pidDisX.pidCount(target_x[0], jarak_x, limit_dis);
            
            /* Kondisi untuk menambahkan speed */
            pwm_kiri = ((float)0.63*speed_disx) + speed_rotx;
            pwm_kanan = ((float)0.63*speed_disx) + speed_rotx;
            pwm_depan = -(speed_disx) + speed_rotx;
            
            prev_millis1 = current_millis;
            encMotor_depan.reset();
            encMotor_kanan.reset();
            encMotor_kiri.reset();
        }
    }/* end of pidMotor */

void getValue(){
/* 
 * Procedure yang digunakan untuk menghitung value pulses encoder 
 * dan revolusi dari motor
 */
    pulses_depan = (float)encMotor_depan.getPulses();    
    pulses_kanan = (float) encMotor_kanan.getPulses();
    pulses_kiri = (float) encMotor_kiri.getPulses();
    rev_depan = (float) pulses_depan/540;
    rev_kanan = (float) pulses_kanan/540; 
    rev_kiri = (float) pulses_kiri/540;
    }/* end of getValue */