LAMPIRAN

Lampiran pemrograman :

//LCD
#include <LiquidCrystal.h>
LiquidCrystal lcd();

//Sensor
int sen[7]={A1,A2,A3,A4,A5,A6,A7};
int nilai[7]={0,0,0,0,0,0,0}; //nilai data analog
int value[7]={0,0,0,0,0,0,0}; //nilai data analog jadi digital
int maxbatas[7]={913,915,887,920,912,911,905};
int minbatas[7]={54,56,50,55,52,53,51};
int batas[7];

//PID
//motor
int pwmki=12;
int moki=13;
int pwmka=10;
int moka=11;

void io (){
for (int a=0; a<=6; a++){
pinMode (sen[a], INPUT);
}

pinMode (pwmki, OUTPUT);

pinMode (moki, OUTPUT);
pinMode (pwmka, OUTPUT);
pinMode (moka, OUTPUT);
digitalWrite (pwmki, LOW);
digitalWrite (moki, LOW);
digitalWrite (pwmka, LOW);
digitalWrite (moka, LOW);
lcd.begin(16, 2);
lcd.setCursor (0,0); lcd.print(" YUMNA ");
lcd.setCursor (0,1); lcd.print(" BISMILLAH ");
delay(1000);
}
void baca_sen(){
for (int a=0; a<=6; a++){
nilai[a]=analogRead(sen[a]);
batas[a]=(maxbatas[a]+minbatas[a])/2;

if (nilai[a]>=batas[a]){
 value[a]=1;
}

else {
value[a]=0;
}
}
}

float error_sen(){
baca_sen();
float error = 0;

for (int x = 0; x < 7; x++)

{
if (x < 3) {
error = error + value[x] * (x - 3);
}
else if (x == 3) {
error = error + value[x] * 0;
}

else {
error = error + value[x] * (x - 4);
}
if (error > 3 || error < -3) {
error = error/2;
}
}
return error;
delay(1);
}

float Last_error = 0; float Ts = 1;

void PID(float Kp, float Ki, float Kd, float Sp_kecepatan) {
float Sp_sensor = 0;
float Error = Sp_sensor - error_sen();
if (Error == 0 ){Error=Last_error;}
float P = Kp * Error;
float D1 = Kd * 10;
float D2 = D1 / Ts;
float D3 = Error - Last_error;
float D = D2 * D3;
float I1 = Ki / 10;
float I2 = Error + Last_error;
float I3 = I1 * I2;
float I = I3 * Ts;
Last_error = Error;
float Pd = P + D;
float Pid = Pd + I;
 int pwm_kanan = Sp_kecepatan + Pid;
int pwm_kiri = Sp_kecepatan - Pid;
motors(pwm_kanan, pwm_kiri);
lcd.clear();

lcd.setCursor(0, 1);
lcd.print(Pid);
delay(1);
}

void motors(int kanan, int kiri) {

analogWrite (pwmki, kiri);
analogWrite (moki, 0);
analogWrite (pwmka, kanan);
analogWrite (moka, 0);
}

void tampil_analog(){
baca_sen();
lcd.clear();
lcd.setCursor(0,0); lcd.print(nilai[0]);
lcd.setCursor(4,0); lcd.print(nilai[1]);
lcd.setCursor(8,0); lcd.print(nilai[2]);
lcd.setCursor(12,0); lcd.print(nilai[3]);
lcd.setCursor(0,1); lcd.print(nilai[4]);
lcd.setCursor(4,1); lcd.print(nilai[5]);
lcd.setCursor(8,1); lcd.print(nilai[6]);
// lcd.setCursor(12,1); lcd.print(nilai[7]);
delay(100);
}

void tampil_digital(){
baca_sen();
lcd.clear();
lcd.setCursor(1,0); lcd.print(value[0]);
lcd.setCursor(3,0); lcd.print(value[1]);
lcd.setCursor(5,0); lcd.print(value[2]);
lcd.setCursor(7,0); lcd.print(value[3]);
lcd.setCursor(9,0); lcd.print(value[4]);
lcd.setCursor(11,0); lcd.print(value[5]);
lcd.setCursor(13,0); lcd.print(value[6]);
//lcd.setCursor(15,0); lcd.print(value[7]);
// delay(100);
}

void setup() {
io();

}
void loop() {
tampil_digital();
PID(100,0,90,150);
delay(1);
}