jarak=(jrkKiri+jrkDepan+jrkSKiri)/3; lside=((jrkKiri*5)+(jrkSKiri/4))/2; //sisi kiri(rata-rata ultrasonik samping k iri) haluan_ki=20-lside; //haluan=seting - side kanan if (jarak>=12) { if ((haluan_ki>=-5)&&(haluan_ki<=5)) { forward(); } if ((haluan_ki>5)&&(haluan_ki<=10))

{ forward(); } if ((haluan_ki>10)&&(haluan_ki<=15)) { forward(); } if ((haluan_ki>15)&&(haluan_ki<=18)) { forward(); } if (haluan_ki>18) { rightTurn(440); } // if ((haluan_ki<-5)&&(haluan_ki>=-10)) { forward(); } if ((haluan_ki<-10)&&(haluan_ki>=-15)) { forward(); } if (haluan_ki<-15) { forward(); } //================================= //if (us4>40) { forward(); } if (jrkSKiri<=15) { rightTurn(440); } if (jrkDepan<=12) { rightTurn(440); } } else { //if (us4>40) { forward(); } if (jrkSKiri<=15) { rightTurn(440); } if (jrkDepan<=12) { rightTurn(440); } else{ rightTurn(440); } }

##################pid 2 byte MV, MVmax, MVmin, P, Ki, Kd, Kp, MAXSpeed, MINSpeed, MAXPWM, MINPWM, rpwm, lpwm, P,I,D,rate , last_error, intervalPWM,PV, Pingka, Pingki; Pingki = jrkKiri + jrkSKiri + jrkDepan; Pingka = jrkDepan + jrkSKanan + jrkKanan; PV = Pingka Pingki; MVmax = 100; MVmin = -100; SP = 0; error = SP PV ; last_error = error; Kp = 1; Ki = 0,5; Kd = 1; MAXSpeed = 250; MINSpeed = 0; intervalPWM = (MAXSpeed MAXPWM = MAXSpeed ; MINPWM = MINSpeed; error = SP PV; MINSpeed) / 5;

P = (Kp * error) / 10; I = I + error; I = (I * Ki) / 10; rate = error last_error; D = (rate * Kd) / 10; last_error = error; MV = P + I + D; if (MV == 0) { analogWrite(2, MAXPWM); analogWrite(5, MAXPWM); } else if (MV > 0) { // alihkan ke kiri rpwm = MAXPWM ((intervalPWM 20) * MV); lpwm = (MAXPWM (intervalPWM * MV) 20); if (lpwm < MINPWM) analogWrite(2, MINPWM); if (lpwm > MAXPWM) analogWrite(2, MAXPWM); if (rpwm < MINPWM) analogWrite(5, MINPWM); if (rpwm > MAXPWM) analogWrite(5, MAXPWM); } else if (MV < 0) { // alihkan ke kanan lpwm = MAXPWM + ( ( intervalPWM 20 ) * MV); rpwm = MAXPWM + ( ( intervalPWM * MV ) 20 ); if (lpwm < MINPWM) analogWrite(2, MINPWM); if (lpwm > MAXPWM) analogWrite(2, MAXPWM); if (rpwm < MINPWM) analogWrite(5, MINPWM); if (rpwm > MAXPWM) analogWrite(5, MAXPWM); } if (MV>MVmax) { //belokan kanan siku rightTurn(430); // belok kanan patah

analogWrite(2, MAXPWM); analogWrite(5, MINPWM); } else if(MV<MVmin) { //belokan kiri siku leftTurn(430); // belok kiri patah analogWrite(2, MAXPWM); analogWrite(5, MAXPWM); }