![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
dear josh
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of DagonFly__RoadToJapan_15Mei by
Diff: main.cpp
- Revision:
- 47:6cac4f1a3c8e
- Parent:
- 46:85169ae8659b
- Child:
- 48:3006386dc8df
--- a/main.cpp Mon Apr 24 14:12:08 2017 +0000 +++ b/main.cpp Sat May 13 05:47:10 2017 +0000 @@ -1,8 +1,8 @@ - +/*tuning motor baru untuk konstanta pid baru */ /****************************************************************************/ /* PROGRAM UNTUK PID CLOSED LOOP */ /* */ -/* Last Update : 16 April 2017 */ +/* Last Update : 16 April 2017 */ /* */ /* - Digunakan encoder autonics */ /* - Konfigurasi Motor dan Encoder sbb : */ @@ -56,9 +56,9 @@ // Variable Atas // indek 2 untuk motor depan, 1 untuk motor belakang double speed, speed2; -const double maxSpeed = 0.95, minSpeed = 0.0, Ts = 12.5; +const double maxSpeed = 0.95, minSpeed = -0.95, Ts = 12.5; const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226; -const double kpA2=0.1293, kdA2=1.007, kiA2=0.0002986; +const double kpA2=0.13978, kdA2=0.9222, kiA2=0.00038636; double a1,b1,c1; double a2,b2,c2; double current_error1, previous_error1_1 = 0, previous_error1_2 = 0; @@ -67,23 +67,19 @@ double previous_speed2 = 0; float rpm, rpm2; -double target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus +double target_rpm = 18.0, target_rpm2 = 18.0; // selisih 4 bagus, sama bagus const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27 const float pwmPowerUp = 1.0; const float pwmPowerDown = -1.0; double jarak_ping=0; -double ping_target = 12; +double ping_target = 14; double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0; -double ping_Kp = -0.13, ping_Kd =-0.049, ping_Ts=10; +double ping_Kp = -0.2747, ping_Kd =0, ping_Ts=10; double ping_pwm, ping_previous_pwm = 0; // Variable Bawah -float Vt; -float keliling_enc = PI*D_ENCODER; -float keliling_robot = PI*D_ROBOT; -float speedT = 0.2; float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri float tuneDpn = 1.0; // Tunning PWM motor Depan float tuneBlk = 1.0; // Tunning PWM motor belakang @@ -93,8 +89,8 @@ float tuneR = 1.0; float tuneL = 1.0; float serong = 0.4; -float rasio = 3.0; -float t_new = 0.25; +float rasio = 1.4545; +float t_new = 0.1; /* variable tunning */ float tunning_L; @@ -102,9 +98,6 @@ float tunning_Dpn; float tunning_Blk; -/* Variabel Encoder Bawah */ -float errTetha, Tetha; // Variabel yang didapatkan encoder - /* Deklarasi Variable Millis */ unsigned long int previousMillis = 0; // PID launcher unsigned long int currentMillis; @@ -121,6 +114,7 @@ bool isReload = false; bool ReloadOn = false; bool flag_Pneu = false; +bool flag_paku = false; bool ready = false; /*****************************************************/ @@ -132,8 +126,6 @@ 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); @@ -154,12 +146,13 @@ Motor motorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5); /* Deklarasi Motor Launcher */ -Motor launcherDpn(PA_5,PB_12,PA_11); // pwm ,fwd, rev +Motor launcherDpn(PA_5,PA_11,PB_12); // pwm ,fwd, rev Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev /* Deklarasi Penumatik Launcher */ DigitalOut pneumatik(PA_4, PullUp); +DigitalOut pneu_paku(PC_2, PullUp); /*Dekalrasi Limit Switch */ //DigitalIn infraAtas(PC_9, PullUp); @@ -320,6 +313,10 @@ // Power Screw Down caseJoystick = 12; } + else if (joystick.L3){ + // Paku Bumi ON/OFF + caseJoystick = 34; + } else { tuneAksel = 0.6; @@ -329,28 +326,6 @@ 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 setCenter(){ -// Fungsi untuk menentukan center dari robot - encoderBase.reset(); -} void init_rpm (){ if (target_rpm>maxRPM-2){ @@ -375,8 +350,8 @@ // Pivot Kanan motorDpn.speed(-PIVOT); motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); + motorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); break; } case (2): @@ -384,8 +359,8 @@ // Pivot Kiri motorDpn.speed(PIVOT); motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); + motorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); break; } case (17): @@ -393,8 +368,8 @@ // Kanan + Rotasi Kanan motorDpn.speed(-PIVOT); motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); + motorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); break; } case (18): @@ -402,8 +377,8 @@ // Kanan + Rotasi Kiri motorDpn.speed(PIVOT); motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); + motorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); break; } case (19): @@ -411,8 +386,8 @@ // Kiri + Rotasi Kanan motorDpn.speed(-PIVOT); motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); + motorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); break; } case (20): @@ -420,8 +395,8 @@ // Kiri + Rotasi Kiri motorDpn.speed(PIVOT); motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); + motorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); break; } case (21): @@ -429,8 +404,8 @@ // Maju + Rotasi Kanan motorDpn.speed(-PIVOT); motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); + motorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); break; } case (22): @@ -438,8 +413,8 @@ // Maju + Rotasi Kiri motorDpn.speed(PIVOT); motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); + motorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); break; } case (23): @@ -447,8 +422,8 @@ // Mundur + Rotasi Kanan motorDpn.speed(-PIVOT); motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); + motorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); break; } case (24): @@ -456,8 +431,8 @@ // Mundur + Rotasi Kiri motorDpn.speed(PIVOT); motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); + motorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); break; } case (25): @@ -569,8 +544,8 @@ if (aksel>300) tuneAksel = 0.9; - motorR.speed(tuneAksel+t_new); - motorL.speed(-tuneAksel-t_new); + motorR.speed(tuneAksel); + motorL.speed(-tuneAksel); motorDpn.brake(1); motorBlk.brake(1); break; @@ -582,8 +557,8 @@ if (aksel>300) tuneAksel = 0.9; - motorR.speed(-tuneAksel-t_new); - motorL.speed(tuneAksel+t_new); + motorR.speed(-tuneAksel); + motorL.speed(tuneAksel); motorDpn.brake(1); motorBlk.brake(1); break; @@ -633,8 +608,8 @@ case (31) : { // start - target_rpm2 = 22; - target_rpm = 22; + target_rpm2 = 23; + target_rpm = 23; init_rpm(); break; } @@ -649,8 +624,8 @@ case (33) : { // R3 - target_rpm2 = 17; - target_rpm = 17; + target_rpm2 = 18; + target_rpm = 18; init_rpm(); break; } @@ -661,6 +636,15 @@ isReload = true; break; } + case (34) :{ + pneu_paku = !pneu_paku; + wait_ms(50); + if (pneu_paku == 1){ + PIVOT = 0.17; + }else{ + PIVOT = 0.8; + } + } default : { tuneAksel = 0.6; @@ -678,6 +662,7 @@ if(ReloadOn){ if(isReload){ powerScrew.speed(pwmPowerDown); + pc.printf("%.2f\n", jarak_ping); if((!limitBawah)||(!limitBawah1)){ isReload = false; ReloadOn = false; @@ -693,16 +678,12 @@ ping_sum_error += ping_current_error*ping_Ts; ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts; - if (ping_pwm>1) ping_pwm=1; - if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5; - if (ping_pwm<-0.049 && ping_pwm>-0.5) ping_pwm = -0.5; - if (ping_pwm<-1) ping_pwm=-1; - + pc.printf("%.2f\n", jarak_ping); powerScrew.speed(ping_pwm); ping_previous_error1 = ping_current_error; } - if ((jarak_ping>(ping_target-3))&&(jarak_ping<(ping_target+3))){ + if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){ ready = true; }else{ ready = false; @@ -772,7 +753,7 @@ encLauncherDpn.reset(); previousMillis2 = currentMillis2; } - pc.printf("%.2f\t%.2f\n",rpm,rpm2); + //pc.printf("%.2f\t%.2f\n",rpm,rpm2); } else { @@ -802,8 +783,6 @@ // initializing encoder pneumatik =1; - setCenter(); - wait_ms(500); //initializing PING