Yolanda Tania
udah bisa looo
Diff: main.cpp
- Revision:
- 0:aa8e05bc0533
- Child:
- 1:0122c72f6e1b
--- /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 */