![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
dear josh
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of DagonFly__RoadToJapan_15Mei by
main.cpp
- Committer:
- gustavaditya
- Date:
- 2017-02-14
- Revision:
- 33:69d266bc3fe9
- Parent:
- 32:581d4a2373f0
- Child:
- 34:1cfd9b1c7d27
File content as of revision 33:69d266bc3fe9:
/****************************************************************************/ /* PROGRAM UNTUK PID CLOSED LOOP */ /* */ /* Last Update : 14 Februari 2017 */ /* */ /* - 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 PERPINDAHAN 1 // Perpindahan ke kanan dan kiri // Variable Atas double speed, speed2, maxSpeed = 0.95, 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 = 8.0; float rpm2, target_rpm2 = 8.0; float pwmPowerUp = 0.70; float pwmPowerDown = -0.80; bool henti=false, hentis=false; // Variable Bawah float Vt; float keliling_enc = PI*D_ENCODER; float keliling_robot = PI*D_ROBOT; float speedT = 0.2; float vpid = 0; float PIVOT = 0.27; // PWM Pivot Kanan, Pivot Kiri float tuneDpn = 0.35; // Tunning PWM motor Depan float tuneBlk = 0.3; // Tunning PWM motor belakang /* Variabel Encoder Bawah */ float errTetha, Tetha; // Variabel yang didapatkan encoder /* Deklarasi Variable Millis */ unsigned long int previousMillis = 0; // PID launcher unsigned long int currentMillis; unsigned long int previousMillis2 = 0; // PID launcher unsigned long int currentMillis2; unsigned long int previousMillis3 = 0; // Pneumatik /* Variabel Stick */ //Logic untuk masuk aktuator int case_joy; bool isLauncher = false; bool isReload = false; int flagLauncher = 1; bool flag_Pneu = false; /*****************************************************/ /* Definisi Prosedur, Fungsi dan Setting Pinout */ /*****************************************************/ /* Fungsi dan Procedur Encoder */ void init_speed(); // void aktuator(); // Pergerakan aktuator berdasarkan case joystick int case_joystick(); // Mendapatkan case dari joystick //void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick void setCenter(); // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0) float getTetha(); // Fungsi mendapatkan error Tetha /* Inisialisasi Pin TX-RX Joystik dan PC */ joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); /* Deklarasi Encoder Base */ encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan /* Deklarasi Encoder Launcher */ encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING); encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING); /* Deklarasi Motor Base */ Motor motorDpn(PB_9, PA_12, PC_5); // pwm, fwd, rev Motor motorBlk(PB_6, PB_1, PB_12); // pwm, fwd, rev /* Deklarasi Motor Launcher */ Motor launcherDpn(PA_8,PC_2,PC_1); // pwm ,fwd, rev Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev /* Deklarasi Penumatik Launcher */ DigitalOut pneumatik(PB_3, PullUp); /*Dekalrasi Limit Switch */ DigitalIn limitAtas(PC_9, PullUp); DigitalIn limitTengah(PB_10, PullUp); DigitalIn limitBawah(PC_8, PullUp); /****************************************************/ /* Deklarasi Fungsi dan Procedure */ /****************************************************/ int case_joystick() { //---------------------------------------------------// // Gerak Motor Base // // Case 1 : Pivot kanan // // Case 2 : Pivot Kiri // // Case 3 : Kanan // // Case 4 : Kiri // // Case 5 : Break // //---------------------------------------------------// int caseJoystick; if (!joystick.L1 && joystick.R1) { // Pivot Kanan caseJoystick = 1; } else if (!joystick.R1 && joystick.L1) { // Pivot Kiri caseJoystick = 2; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { // Kanan caseJoystick = 3; //pc.printf("kanan"); } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { // Kiri caseJoystick = 4; //pc.printf("kiri"); } else if (joystick.segitiga_click && flagLauncher){ // Motor Launcher caseJoystick = 5; } else if (joystick.R2_click && (target_rpm2 < 20)){ // Target Pulse PID ++ Motor Depan caseJoystick = 6; } else if (joystick.L2_click && (target_rpm2 > 1)){ // Target Pulse PID -- Motor Depan caseJoystick = 7; } else if (joystick.R3_click && (target_rpm < 20 )){ // Target Pulse PID ++ Motor Belakang caseJoystick = 8; } else if (joystick.L3_click && (target_rpm > 1)){ // Target Pulse PID -- Motor Belakang caseJoystick = 9; } else if (joystick.silang_click){ // Pnemuatik ON caseJoystick = 10; } else if ((joystick.atas)&&(!joystick.bawah)) { // Power Screw Up caseJoystick = 11; } else if ((!joystick.atas)&&(joystick.bawah)) { // Power Screw Down caseJoystick = 12; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { // Break caseJoystick = 13; } return(caseJoystick); } float getTetha(){ // Fungsi untuk mendapatkan nilai tetha float busur, tetha; busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc); tetha = busur/keliling_robot*360; return -(tetha); } float pidBase(float Kp, float Ki, float Kd) { int errorP; errorP = getTetha(); if (errorP<3.5 && errorP>(-3.5)) errorP = 0; return (float)Kp*errorP; } void init_speed (){ if (speed>maxSpeed){ speed = maxSpeed; } if (speed<minSpeed){ speed = minSpeed; } if (speed2>maxSpeed){ speed2 = maxSpeed; } if (speed2<minSpeed){ speed2 = minSpeed; } } void setCenter(){ // Fungsi untuk menentukan center dari robot encoderBase.reset(); } void aktuator() { switch (case_joy) { case (1): { // Pivot Kanan motorDpn.speed(-PIVOT); motorBlk.speed(-PIVOT); setCenter(); break; } case (2): { // Pivot Kiri motorDpn.speed(PIVOT); motorBlk.speed(PIVOT); setCenter(); break; } case (3) : { // Kanan motorDpn.speed(-tuneDpn + pidBase(0.009,0,0)); motorBlk.speed(tuneBlk + pidBase(0.009,0,0)); //speedDpn = tuneDpn + pidBase(0.009,0,0) //speedBlk = tuneBlk + pidBase(0.009,0,0) //motorDpn.speed(-tuneDpn); //motorBlk.speed(tuneBlk); break; } case (4) : { // Kiri motorDpn.speed(tuneDpn + pidBase(0.009,0,0)); motorBlk.speed(-tuneBlk + pidBase(0.009,0,0)); //motorDpn.speed(tuneDpn); //motorBlk.speed(-tuneBlk); break; } case (5) : { // Aktifkan motor atas isLauncher = !isLauncher; break; } case (6) : { // Target Pulse PID ++ Motor Depan target_rpm2 = target_rpm2++; break; } case (7) : { // Target Pulse PID -- Motor Depan target_rpm2 = target_rpm2--; break; } case (8) : { // Target Pulse PID ++ Motor Belakang= target_rpm = target_rpm++; break; } case (9) : { // Target Pulse PID -- Motor Belakang target_rpm = target_rpm--; break; } case (10) : { // Pneumatik pneumatik = 0; previousMillis3 = millis(); flag_Pneu = true; break; } case (11) : { // Power Screw Up powerScrew.speed(pwmPowerUp); break; } case (12) : { // Power Screw Down powerScrew.speed(pwmPowerDown); break; } default : { motorDpn.brake(1); motorBlk.brake(1); if (!limitAtas) { powerScrew.brake(1); } if (!limitTengah) { case_joy = 12; aktuator(); } if (!limitBawah) { case_joy = 11; aktuator(); } } } // End Switch } void launcher() { if (isLauncher) { startMillis(); currentMillis = millis(); currentMillis2 = millis(); // PID Launcher Depan if (currentMillis-previousMillis>=10) { rpm = (float)encLauncherBlk.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(); launcherBlk.speed(speed); last_error = current_error; encLauncherBlk.reset(); //pc.printf("%.04lf\n",rpm); previousMillis = currentMillis; } if (currentMillis2-previousMillis2>=10) { rpm2 = (float)encLauncherDpn.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(); launcherDpn.speed(speed2); last_error2 = current_error2; encLauncherDpn.reset(); previousMillis2 = currentMillis2; } } else { launcherDpn.brake(1); launcherBlk.brake(1); } } /*********************************************************/ /* Main Function */ /*********************************************************/ int main (void) { // Set baud rate - 115200 joystick.setup(); pc.baud(115200); wait_ms(1000); setCenter(); wait_ms(500); pc.printf("ready...."); while(1) { // Interrupt Serial joystick.idle(); if(joystick.readable()) { // Panggil fungsi pembacaan joystik joystick.baca_data(); // Panggil fungsi pengolahan data joystik joystick.olah_data(); // Masuk ke case joystick case_joy = case_joystick(); aktuator(); launcher(); if ((millis()-previousMillis3 >= 700)&&(flag_Pneu)){ pneumatik = 1; flag_Pneu = false; } } else { joystick.idle(); } } }