![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
tugas akhir
Dependencies: Motor mbed millis
Fork of DagonFly__RoadToJapan_15Mei_Ultimate by
main.cpp
- Committer:
- Sufa
- Date:
- 2017-02-12
- Revision:
- 30:d69cc27ac644
- Parent:
- 29:7b372b0aaa61
- Child:
- 31:d5cbda07fd95
File content as of revision 30:d69cc27ac644:
/****************************************************************************/ /* PROGRAM UNTUK PID CLOSED LOOP */ /* */ /* - Digunakan encoder autonics */ /* - Konfigurasi Motor dan Encoder sbb : */ /* ______________________ */ /* / \ Rode Depan Belakang: */ /* / 2 (Belakang) \ Omniwheel */ /* | | */ /* | 3 (Encoder) 4 | Roda Kiri Kanan: */ /* | | Fixed Wheel */ /* \ 1 (Depan) / */ /* \______________________/ Putaran berlawanan arah */ /* jarum jam positif */ /* SETTINGS (WAJIB!) : */ /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ /* */ /****************************************************************************/ /* */ /* Joystick */ /* Kanan => */ /* Kiri => */ /* */ /* Tombol silang => Power Screw Aktif */ /* Tombol segitiga => Aktif motor Launcher */ /* Tombol lingkaran => Aktif Pneumatik Launcher */ /* Tombol L1 => Pivot Kiri */ /* Tombol R1 => Pivot Kanan */ /* Tombol L3 => PWM Motor Belakang Dikurangi */ /* Tombol R3 => PWM Motor Belakang Ditambah */ /* Tombol L2 => PWM Motor Depan Dikurangi */ /* Tombol R2 => PWM Motor Depan Ditambah */ /* */ /* Bismillahirahmanirrahim */ /* Jagalah Kebersihan Kodingan */ /****************************************************************************/ #include "mbed.h" #include "JoystickPS3.h" #include "Motor.h" #include "Servo.h" #include "encoderKRAI.h" #include "millis.h" /***********************************************/ /* Konstanta dan Variabel */ /***********************************************/ #define PI 3.14159265 #define D_ENCODER 10 // Diameter Roda Encoder #define D_ROBOT 80 // Diameter Roda Robot #define VMAX 0.3 // Kiri Kanan #define PIVOT 0.4 // Pivka, Pivki #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri // Variable Atas double speed,speed2, maxSpeed = 0.8, minSpeed = 0; double kpA=0.6757, kdA=0.6757, kiA=0.00006757; double p,i,d; double p2,i2,d2; double last_error = 0, current_error, sum_error = 0; double last_error2 = 0, current_error2, sum_error2 = 0; float rpm, target_rpm = 2.0; float rpm2, target_rpm2 = 4.0; bool henti=false, hentis=false; // Variable Bawah float Vt; float k_enc = PI*D_ENCODER; float k_robot = PI*D_ROBOT; float speedT = 0.2; float vpid = 0; float pwmP = 0.5; float pwmT = -0.95; /* Deklarasi Encoder Base */ encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan /* Deklarasi Encoder Launcher */ encoderKRAI encoderAtas2( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING); encoderKRAI encoderAtas( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING); /* Deklarasi Motor Base */ Motor motor1(PB_9, PA_12, PC_5); // pwm, fwd, rev, Motor Depan Motor motor2(PB_6, PB_1, PB_12); // pwm, fwd, rev, Motor Belakang /* Deklarasi Motor Launcher */ Motor motorL1(PA_8,PC_2,PC_1); // pwm ,fwd, rev, Motor Launcher1 Motor motorL3(PA_10, PC_3, PC_0); // pwm, fwd, rev, Motor Launcher2 Motor motorP(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Powerscrew /* Deklarasi Penumatik Launcher */ DigitalOut pneumatik(PB_3, PullUp); /*Dekalrasi Limit Switch */ DigitalIn limitA(PC_9, PullUp); DigitalIn limitT(PB_10, PullUp); DigitalIn limitB(PC_8, PullUp); /** * posX dan posY berdasarkan arah robot * encoder Depan & Belakang sejajar sumbu Y * encoder Kanan & Kirim sejajar sumbu X **/ /* Variabel Encoder */ float errT, Tetha; // Variabel yang didapatkan encoder /* Fungsi dan Procedur Encoder */ void setCenter(); // Fungsi reset agar robot di tengah float getTetha(); // Fungsi mendapatkan jarak Tetha /* Inisialisasi Pin TX-RX Joystik dan PC */ joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); /* Deklarasi Variable Milis */ unsigned long int previousMillis = 0; unsigned long int currentMillis; unsigned long int previousMillis2 = 0; unsigned long int currentMillis2; /* Variabel Stick */ char case_ger; bool launcher = false; bool reload = false; /****************************************************/ /* Deklarasi Fungsi dan Procedure */ /****************************************************/ void init_speed(); void speedKalibrasiMotor(); float pidBase(float Kp, float Ki, float Kd) { int errorP; errorP = getTetha(); return (float)Kp*errorP; } int case_gerak(){ /**************************************************** ** Gerak Motor Base ** Case 1 : Pivot kanan ** Case 2 : Pivot Kiri ** Case 3 : Kanan ** Case 4 : Kiri ** Case 5 : Break ****************************************************/ int casegerak; if (!joystick.L1 && joystick.R1) { // Pivot Kanan casegerak = 1; } else if (!joystick.R1 && joystick.L1) { // Pivot Kiri casegerak = 2; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { // Kanan casegerak = 3; // pc.printf("kanan"); } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { // Kiri casegerak = 4; // pc.printf("kiri"); } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { // Break casegerak = 5; } return(casegerak); } void aktuator(){ /* Fungsi untuk menggerakkan Motor PowerScrew */ // PowerScrew if (reload){ if (!limitA){ motorP.brake (1); if (hentis != henti){ wait_ms(1500); } hentis = henti; } else { henti = false; if (!limitT) { while (limitB) { motorP.speed(pwmT); } motorP.brake(1); reload = !reload; } else { motorP.speed(pwmP); } } } else { motorP.brake (1); } /*Motor Atas*/ if (launcher) { startMillis(); currentMillis = millis(); currentMillis2 = millis(); /*PID Launcher Depan*/ if (currentMillis-previousMillis>=10) { rpm = (float)encoderAtas.getPulses(); current_error = target_rpm - rpm; sum_error = sum_error + current_error; p = current_error*kpA; d = (current_error-last_error)*kdA/10.0; i = sum_error*kiA*10.0; speed = p + d + i; init_speed(); motorL1.speed(speed); last_error = current_error; encoderAtas.reset(); //pc.printf("%.04lf\n",rpm); previousMillis = currentMillis; } else { encoderAtas.getPulses(); } if (currentMillis2-previousMillis2>=10) { rpm2 = (float)encoderAtas2.getPulses(); current_error2 = target_rpm2 - rpm2; sum_error2 = sum_error2 + current_error2; p2 = current_error2*kpA; d2 = (current_error2-last_error2)*kdA/10.0; i2 = sum_error2*kiA*10.0; speed2 = p2 + d2 + i2; init_speed(); motorL3.speed(speed2); last_error2 = current_error2; encoderAtas2.reset(); //pc.printf("%.04lf\n",rpm2); previousMillis2 = currentMillis2; } else { encoderAtas2.getPulses(); } } else { motorL1.brake(1); motorL3.brake(1); } // MOTOR Bawah switch (case_ger) { case (1): { // Pivot Kanan motor1.speed(PIVOT); motor2.speed(PIVOT); setCenter(); break; } case (2): { // Pivot Kiri motor1.speed(-PIVOT); motor2.speed(-PIVOT); setCenter(); break; } case (3) : { // Kanan //motor1.speed(-VMAX-vpid); //motor2.speed(0.2+vpid); motor1.speed(-0.365+pidBase(0.09,0,0)); motor2.speed(0.46+pidBase(0.09,0,0)); break; } case (4) : { // Kiri //motor1.speed(VMAX-vpid); //motor2.speed(-0.2+vpid); motor1.speed(0.365+pidBase(0.09,0,0));//belakang motor2.speed(-0.46+pidBase(0.09,0,0));//depan break; } default : { motor1.brake(1); motor2.brake(1); } } // End Switch } void setCenter(){ /* Fungsi untuk menentukan center dari robot */ encoderKiri.reset(); } float getTetha(){ /* Fungsi untuk mendapatkan nilai tetha */ float busurKir, tetha; busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc); tetha = busurKir/k_robot*360; return -(tetha); } void speedKalibrasiMotor(){ /* Fungsi untuk speed launcher */ if (joystick.R3_click and target_rpm < 14){ target_rpm = target_rpm + 1; // RPM++ Motor Belakang } if (joystick.L3_click and target_rpm > 1){ target_rpm = target_rpm - 1; // RPM-- Motor Belakang } if (joystick.R2_click and target_rpm2 < 14){ target_rpm2 = target_rpm2 + 1; // RPM++ Motor Depan } if (joystick.L2_click and target_rpm2 > 1 ){ target_rpm2 = target_rpm2 - 1; // RPM-- Motor Depan } // pc.printf("Rpm depan = %.4f\t Rpm belakang = %.4f\n", target_rpm, target_rpm2); } void init_speed (){ if (speed>maxSpeed){ speed = maxSpeed; } if (speed<minSpeed){ speed = minSpeed; } if (speed2>maxSpeed){ speed2 = maxSpeed; } if (speed2<minSpeed){ speed2 = minSpeed; } } /*********************************************************/ /* Main Function */ /*********************************************************/ int main (void){ /* Set baud rate - 115200 */ joystick.setup(); pc.baud(115200); wait_ms(1000); setCenter(); wait_ms(500); pc.printf("ready...."); /* Untuk mendapatkan serial dari Arduino */ while(1) { // Interrupt Serial joystick.idle(); //pc.printf("enco : %d \n",encoderKiri.getPulses()); if(joystick.readable()) { // Panggil fungsi pembacaan joystik joystick.baca_data(); // Panggil fungsi pengolahan data joystik joystick.olah_data(); // Masuk ke case gerak case_ger = case_gerak(); aktuator(); if (joystick.segitiga_click){ launcher = !launcher; } if (joystick.lingkaran_click){ pneumatik = 0; henti = true; wait_ms(700); pneumatik = 1; } speedKalibrasiMotor(); if (joystick.silang_click){ reload = !reload; } } else { joystick.idle(); motor1.brake(1); motor2.brake(1); } } }