![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
udah bisa looo
Diff: main.cpp
- Revision:
- 0:aa8e05bc0533
- Child:
- 1:0122c72f6e1b
diff -r 000000000000 -r aa8e05bc0533 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Feb 27 12:40:03 2020 +0000 @@ -0,0 +1,290 @@ +/*************************************************************************** + * 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); + +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; + 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 (timer1.read_us() - samp_pid > 9127) + { +// float setpoint = 110; + float setpoint = 150; + float error = setpoint - curr_theta1; + + 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 (curr_theta1>140 && curr_theta1 < 170 && setpoint >= 145){ + arm1.speed(0); + arm1.brake(); + } + else if (curr_theta1>130 && curr_theta1 <= 140){ + arm1.speed(0.075*pwm1); + } + else if (curr_theta1>115 && curr_theta1 <= 130){ + arm1.speed(0.65*pwm1); + } + else { + arm1.speed(pwm1); + } + 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 */