![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Buat agip
Dependencies: Motor_1 encoderKRAI mbed millis
Fork of Robo_Taker_Nasional_2018 by
Diff: main.cpp
- Revision:
- 5:4a70c53d7f86
- Parent:
- 3:b1403fcdaeb1
- Child:
- 6:bb7e29420efd
diff -r b1403fcdaeb1 -r 4a70c53d7f86 main.cpp --- a/main.cpp Thu Mar 01 11:02:03 2018 +0000 +++ b/main.cpp Thu Mar 08 08:45:09 2018 +0000 @@ -18,7 +18,7 @@ //Konstanta PID Sudut #define KP_W 1.0 #define KI_W 0.0065 -#define KD_W 125 +#define KD_W 175 #define MOTOR_LIMIT_MAX 1 #define MOTOR_LIMIT_MIN -1 @@ -45,6 +45,7 @@ // Fungsi dan Prosedur void gerakMotor(); void hitungPID(float theta_s); +void hitungParameter(); void printPulse(); void case_gerak(); float compute_Alpha(float x_s, float y_s, float x, float y,float theta); @@ -55,6 +56,7 @@ float pulse_B=0; float pulse_C=0; float Vr = 0; +float Vr_max = 0; float Vw = 0; float a = 0; float w = 0; @@ -70,9 +72,9 @@ float theta_error_prev = 0; float sum_theta_error = 0; float theta_error; -unsigned long last_mt_print, last_mt_pid, last_mt_rotasi; +unsigned long last_mt_print, last_mt_pid, last_mt_aksel, last_mt_desel, last_mt_rotasi; bool print_pulse = 0; -bool modeauto = 0; +bool modeauto = 1; int main(){ encoder_A.reset(); @@ -95,30 +97,14 @@ case_gerak(); gerakMotor(); - - if ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){ + if (millis() - last_mt_pid > TS){ + hitungParameter(); + last_mt_pid = millis(); + } + if (!(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){ hitungPID(theta_s); - last_mt_pid = millis(); } if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){ - pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK; - pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK; - pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK; - - //Compute value - theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L); - x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev); - y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev); - - //Update value - theta_prev = theta; - x_prev = x; - y_prev = y; - - encoder_A.reset(); - encoder_B.reset(); - encoder_C.reset(); - if(modeauto) Vw = 0; } if (millis() - last_mt_print > TS+5){ @@ -130,7 +116,7 @@ } } -void hitungPID(float theta_s){ +void hitungParameter(){ pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK; pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK; pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK; @@ -148,7 +134,8 @@ encoder_A.reset(); encoder_B.reset(); encoder_C.reset(); - +} +void hitungPID(float theta_s){ //theta_s = theta_s/RAD_TO_DEG; //menghitung error jarak x,y terhaadap xs,ys theta_error = theta_s - (theta*RAD_TO_DEG); @@ -170,13 +157,29 @@ } void gerakMotor(){ - if ((Vw == 0) && (Vr == 0)){ - motor1.brake(BRAKE_HIGH); - motor2.brake(BRAKE_HIGH); - motor3.brake(BRAKE_HIGH); - print_pulse = 0; + if ((Vw == 0) && (Vr_max == 0)){ + if (Vr >= 0.05){ + if (millis() - last_mt_desel > 70){ + Vr -= 0.1; + last_mt_desel = millis(); + } + } else { + motor1.brake(BRAKE_HIGH); + motor2.brake(BRAKE_HIGH); + motor3.brake(BRAKE_HIGH); + print_pulse = 0; + Vr = 0; + } } else { - //a = compute_Alpha(x_s, y_s, x, y, theta); + if ((millis() - last_mt_aksel > 150) && Vr < Vr_max){ + if (Vr < 0.275) + Vr = 0.275; + else + Vr += 0.05; + last_mt_aksel = millis(); + } + if (Vr > Vr_max && Vr_max >= 0.000) + Vr = Vr_max; motor1.speed((-1*Vr*cos(a) + Vw)); motor2.speed((Vr*(0.5*cos(a) + 0.866*sin(a)) + Vw)); motor3.speed((Vr*(0.5*cos(a) - 0.866*sin(a)) + Vw)); @@ -199,11 +202,24 @@ void case_gerak(){ // Rotasi if(modeauto){ - if (!stick.L1 && stick.R1) // Pivot Kanan - theta_s += 0.15; - else if (!stick.R1 && stick.L1) // Pivot Kiri - theta_s -= 0.15; - + if (!stick.L1 && stick.R1){ // Pivot Kanan + theta = 0.0; + theta_prev = 0.0; + theta_s = 0; + theta_error_prev = 0; + sum_theta_error = 0; + theta_error = 0; + Vw = 0.2; + } + else if (!stick.R1 && stick.L1){ // Pivot Kiri + theta = 0.0; + theta_prev = 0.0; + theta_s = 0; + theta_error_prev = 0; + sum_theta_error = 0; + theta_error = 0; + Vw = -0.2; + } } else if(!modeauto){ if (!stick.L1 && stick.R1) // Pivot Kanan @@ -221,54 +237,61 @@ theta_s = 0; theta_error_prev = 0; sum_theta_error = 0; - theta_error; + theta_error = 0; } // Linier - Vr = 0.5; if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){ a = -90/RAD_TO_DEG; // Maju + Vr_max = 0.8; // x_s = 0;// Maju // y_s = 10000; } else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri)){ a = 90/RAD_TO_DEG; // Mundur + Vr_max = 0.8; // x_s = 0; // Mundur // y_s = -10000; } else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan)){ a = -135/RAD_TO_DEG; // Serong Atas Kanan + Vr_max = 0.7; // x_s = 50000; // Maju+Kanan // y_s = 50000; } else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan)){ a = 135/RAD_TO_DEG; // Serong Bawah Kanan + Vr_max = 0.7; // x_s = 50000; // Mundur+Kanan // y_s = -50000; } else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan)){ a = -45/RAD_TO_DEG; // Serong Atas Kiri + Vr_max = 0.7; // x_s = 50000; // Maju+Kiri // y_s = -50000; } else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan)){ a = 45/RAD_TO_DEG; // Serong Bawah Kiri + Vr_max = 0.7; // x_s = -50000; // Mundur+Kiri // y_s = -50000; } else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri)){ a = 180/RAD_TO_DEG; // Kanan + Vr_max = 0.5; // x_s = 50000; // Kanan // y_s = 0; } else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){ a = 0/RAD_TO_DEG; // Kiri + Vr_max = 0.5; // x_s = -50000; // Kiri // y_s = 0; } else { - Vr = 0; - a = 0; + Vr_max = 0; + //a = 0; } if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))