File content as of revision 1:0122c72f6e1b:

/***************************************************************************
 * Title      : Program Debug Motor dan Encoder
 * Name       : debug_motor.cpp
 * Version    : 1.0
 * Author     : Gian Arjuna EL 16
 * Date       : 12 Desember 2019
 * Description:
 *
 * Program ini dapat digunakan untuk mengambil data pulse encoder, menggerakan
 * motor, mengambil data kecepatan, dan mengambil data PID. Untuk menggunakan
 * program ini UBAH DEKLARASI PIN PADA BAGIAN PIN LIST DIBAWAH agar sesuai
 * dengan pin yang sedang digunakan. Kemudian pilih mode debug yang ingin 
 * dilakukan dengan comment/uncomment "#define" pada bagian mode debug
 *
 ***************************************************************************/

 
/* LIBRARY */
#include "mbed.h"
#include "encoderKRAI.h"
#include "Motor.h"
#include "pid_dagoz/PID.h"
 
/* MODE DEBUG YANG DIINGINKAN */ 
//#define AMBIL_ENCODER
//#define GERAK_MOTOR1
//#define GERAK_MOTOR2
//#define AMBIL_KECEPATAN
//#define TES_PID
#define TES_PID_TANGAN 
 
/***** PIN LIST *****/
/* pin assignment untuk encoder */
encoderKRAI enc_arm1(PB_7, PB_6, 538, encoderKRAI::X4_ENCODING);
//encoderKRAI enc_arm2(PB_2, PD_2, 538, encoderKRAI::X4_ENCODING);

/* pin assignment untuk motor */

Motor arm1(PA_15, PA_13, PA_14);
//Motor arm2(PC_8, PC_6, PC_5);

float kp1 = 0.0204622,kp2 = 0.0102622;
float kd1 = 0.06,kd2 = 0.065;
float ki1 = 0 ,ki2 = 0;
float ff1 = 0,ff2 = 0;
float n1 = 0,n2 = 0;
float ts1 = 0.009127;
float PI = 3.14159;
float wheel_radius = 0.075;

PID pid1(kp1, ki1, kd1, n1, ts1, ff1, PID::PI_MODE);
PID pid2(kp2, ki2, kd2, n2, ts1, ff2, PID::PI_MODE);

/* pin assignment lainnya */
DigitalIn mybutton(USER_BUTTON, PullUp); 

/* komunikasi serial dengan UART */
Serial pc(USBTX, USBRX, 115200); 

/* timer untuk mendapatkan waktu */
Timer timer1;


/* deklarasi variable global */
/* array untuk menyimpan data kecepatan */
float theta1[400];
float theta2[400];
float input[400];
float time_array[400];

/* variable kecepatan dan posisi*/
float curr_theta, prev_theta;
float en1,en2;

/* variable sampling time */
int samp, samp_pid, TS;
int i;
int counttt;


/* variable pid */
float curr_theta1,curr_theta2;
float prev_error, lowpass_error, prev_lowpass_error;
float pwm1,pwm2;

float teta_ref; 
float teta_act;
float kp_teta = 0.005;
float ki_teta = 0;
float kd_teta = 0;
float TS_pid;
float w_ref;
float prev_teta_ref;
float w_act;
float kp_w = 0.005;
float ki_w = 0;
float kd_w = 0.005;
float pwm;

/* variable penyimpan waktu */
uint32_t last_baca, last_pid, last_motor;

/* prototipe fungsi */
//void pid (float ref, float curr_feed, float prev_feed, float feedforward, float actual, float kp, float ki, float kd, float TS, float* output);
void pidyol(float target,float feedback);

int main() {
    arm1.period(0.02);
    /* ================================================================== */
    #ifdef AMBIL_ENCODER 
        while(1)
        {          
            en1= (float)enc_arm1.getPulses();
            //en2= (float)enc_arm2.getPulses();
            pc.printf("%f\t %f\n", en1, en2);
        }
    #endif

    /* ================================================================== */

    #ifdef GERAK_MOTOR1
        while(1)
        {
            float pwm = 0.6;
            arm1.speed(pwm);
            en1= (float)enc_arm1.getPulses();
            pc.printf("%f\t %f\n", en1, en2);
        }
    #endif    

    #ifdef GERAK_MOTOR2
        while(1)
        {
            float pwm = 0.6;
            arm2.speed(pwm);
            //en2= (float)enc_arm2.getPulses();
            pc.printf("%f\t %f\n", en1, en2);
        }
    #endif  
    /* ================================================================== */
    #ifdef AMBIL_KECEPATAN // (misal arm1(?))

        /* setup and initialization*/
        timer1.start();
    
        /* command move motor and sample data*/ 
        while(counttt <= 400)
        {           
            if (t.read_us()-samp >= 5000)
            {
                TS = t.read_us()-samp;
                curr_theta = (float)enc.getPulses()*360/538;
                theta1[counttt] = curr_theta1;
                enc_arm1.reset();
                arm1.speed(0.3);
                counttt ++;
                samp = t.read_us(); 
            }    
        }
        arm1.speed(0); /* turn off motor after sampling done */

        /* print data */
        for(i = 0; i < 400; i++)
        {
            pc.printf("%f\n", theta[i]);          
        }   
    #endif
    /* ================================================================== */

    #ifdef TES_PID
        /* setup and initialization*/
        timer1.start();
        float period = 0.0005;
        arm1.period(period);
        //arm2.period(period);
//        float pwmz = 0.7;
    
        /* command move motor and sample data*/ 
        while(counttt <= 400)
        {           
            if (timer1.read_us()-samp >= 7123)
            {
                TS = timer1.read_us()-samp;
                time_array[counttt] = (float)timer1.read_us()/1000000;
                input[counttt] = pwmz*24;
                curr_theta1 = (float)enc_arm1.getPulses()*360/538;
                //curr_theta2 = (float)enc_arm2.getPulses()*360/538;
                theta1[counttt] = curr_theta1;
                //theta2[counttt] = curr_theta2;
                enc_arm1.reset();
                //enc_arm2.reset();
                
                counttt ++;
                samp = timer1.read_us(); 
            } 
            if (timer1.read_us() - samp_pid > 9127)
            {
                float setpoint = 180;
                pwm1 = pid1.createpwm(setpoint,curr_theta1);
                //pwm2 = pid2.createpwm(setpoint,curr_theta2);

               
                arm1.speed(pwm1);                
                //arm2.speed(pwm2);
                if (curr_theta1>180){
                    arm1.speed(0);
                }
                samp_pid = timer1.read_us(); 
            }   
            
        }

        
        arm1.speed(0);
        arm2.speed(0);
         /* turn off motor after sampling done */

        /* print data */
        for(i = 0; i < 400; i++)
        {
            pc.printf("%f  %f  %f  %f \n", theta1[i], theta2[i], time_array[i], input[i]);          
        } 
    #endif
    
    #ifdef TES_PID_TANGAN
        /* setup and initialization*/
        timer1.start();
        float period = 0.0005;
        float setpoint = 150;
        arm1.period(period);
    
        /* command move motor and sample data*/ 
        while(1)
        {           
            if (timer1.read_us()-samp >= 7123)
            {
                curr_theta1 += (float)enc_arm1.getPulses()*360/538;
                //curr_theta2 += (float)enc_arm2.getPulses()*360/538;
                enc_arm1.reset();
                //enc_arm2.reset();
                
                samp = timer1.read_us(); 
            } 
            if(!mybutton){
                setpoint = 0;
            }
            if (timer1.read_us() - samp_pid > 9127)
            {
                pidyol(setpoint,curr_theta1);
                samp_pid = timer1.read_us(); 
                
                
            }   
            if(timer1.read_us() - last_baca > 100000){
                pc.printf("%.2f %.2f\n", curr_theta1, pwm1);
            }
            
        }
         /* turn off motor after sampling done */ 
    #endif
    /* ================================================================== */

}


/* definisi fungsi */
void pidyol(float target,float feedback){
    float error = target - feedback;

    lowpass_error = 0.1*error + 0.9*prev_error;
//                lowpass_error = 0.1*lowpass_error + 0.9*prev_lowpass_error;
//                prev_lowpass_error= lowpass_error;
    
    pwm1 = kp2*(lowpass_error) + kd2*(lowpass_error - prev_lowpass_error);
    pwm1 = fabs(pwm1) > 0.85 ? 0.85*fabs(pwm1)/pwm1 : pwm1;
    
    prev_lowpass_error = lowpass_error;
    
    prev_error = error;
                    
    //arm2.speed(pwm2);
//                if (curr_theta1>110 && curr_theta1 < 140 && setpoint >= 120){
    if (error>0){
        if (feedback>140 && feedback < 170 && target >= 145){
            arm1.speed(0);
            arm1.brake();
        }
        else if (feedback>130 && feedback <= 140){
            arm1.speed(0.075*pwm1);
        }
        else if (feedback>115 && feedback <= 130){
            arm1.speed(0.65*pwm1);
        }
        else {
            arm1.speed(pwm1); 
        }
    }
    else if (error<0){
        //kode turun
        if (feedback>20 && feedback< 40 && target <= 20){
            arm1.speed(0);
            arm1.brake();
        }
        else if (feedback>40 && feedback <= 60){
            arm1.speed(0.1*pwm1);
        }
        else if (feedback>60 && feedback <= 90){
            arm1.speed(0.65*pwm1);
        }
        else {
            arm1.speed(pwm1); 
        }
    }
}